diff --git a/nimbus/CMakeLists.txt b/nimbus/CMakeLists.txt index 3a844ec..1753382 100644 --- a/nimbus/CMakeLists.txt +++ b/nimbus/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(sensor_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(mavros_msgs REQUIRED) find_package(router_interfaces REQUIRED) +find_package(OpenCV REQUIRED) # Install Python modules ament_python_install_package(nimbus) @@ -32,7 +33,8 @@ install(PROGRAMS ) -# C++ nodes +## C++ nodes +# Pilot node add_executable(pilot src/pilot.cpp) target_include_directories(pilot PUBLIC $ @@ -45,6 +47,38 @@ install(TARGETS pilot DESTINATION lib/${PROJECT_NAME} ) +# Navigator node +add_executable(navigator src/navigator.cpp) +target_include_directories(navigator PUBLIC + $ + $ +) + +ament_target_dependencies(navigator rclcpp rclcpp_action sensor_msgs nav_msgs mavros_msgs router_interfaces) + +install(TARGETS navigator + DESTINATION lib/${PROJECT_NAME} +) + +# Planner node +add_executable(planner src/planner.cpp) +target_include_directories(planner PUBLIC + $ + $ + ${OpenCV_INCLUDE_DIRS} +) + +ament_target_dependencies(planner rclcpp rclcpp_action sensor_msgs nav_msgs mavros_msgs router_interfaces) + +# Link in OpenCV +target_link_libraries(planner + ${OpenCV_LIBRARIES} +) + +install(TARGETS planner + DESTINATION lib/${PROJECT_NAME} +) + # Install other stuff install(DIRECTORY launch diff --git a/nimbus/include/nimbus/navigator.h b/nimbus/include/nimbus/navigator.h new file mode 100644 index 0000000..af728a5 --- /dev/null +++ b/nimbus/include/nimbus/navigator.h @@ -0,0 +1,83 @@ +/* + * navigator.h + * + * Created by: Jonathan Diller + * On: Feb 18, 2026 + * + * Description: This is the navigator node of the Nimbus autonomy stack. + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "router_interfaces/action/waypoint_move.hpp" +#include "router_interfaces/action/waypoint_sequence.hpp" + + +#define DEBUG_NAV false +#define DEFAULT_ACCEPTANCE_RADIUS 2.0 + + +class Navigator : public rclcpp::Node { +public: + // Define types for easier reference + using WaypointMove = router_interfaces::action::WaypointMove; + using WaypointSequence = router_interfaces::action::WaypointSequence; + using GoalHandleWaypointMove = rclcpp_action::ClientGoalHandle; + using GoalHandleWaypointSequence = rclcpp_action::ServerGoalHandle; + + Navigator(); + virtual ~Navigator() = default; + +private: + double acceptance_radius_; + // Track when we have completed a waypoint + bool reached_waypoint_; + double dist_to_waypoint_; + double dist_total_; + + // Handle needed to cancel waypoint actions + GoalHandleWaypointMove::SharedPtr current_client_goal_handle_; + + // Action Client + rclcpp_action::Client::SharedPtr wp_action_client_; + // Action Server + rclcpp_action::Server::SharedPtr move_action_server_; + + // Request Waypoint action from pilot + void send_wp_goal(int waypoint, float tolerance); + + // Waypoint request response + void wp_response_callback(const GoalHandleWaypointMove::SharedPtr & goal_handle); + + // Waypoint request feedback + void wp_feedback_callback(GoalHandleWaypointMove::SharedPtr, + const std::shared_ptr feedback_msg); + + // Waypoint request final result + void wp_result_callback(const GoalHandleWaypointMove::WrappedResult & result); + + // Position goal validation + rclcpp_action::GoalResponse position_goal_callback( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal); + + // Position cancel handling + rclcpp_action::CancelResponse position_cancel_callback( + const std::shared_ptr goal_handle); + + // Position accepted (Starts execution thread) + void position_accepted_callback( + const std::shared_ptr goal_handle); + + // Main position-move execution + void position_execute_callback( + const std::shared_ptr goal_handle); +}; diff --git a/nimbus/include/nimbus/pilot.h b/nimbus/include/nimbus/pilot.h index b1e6160..1f83080 100644 --- a/nimbus/include/nimbus/pilot.h +++ b/nimbus/include/nimbus/pilot.h @@ -1,5 +1,5 @@ /* - * Command.h + * pilot.h * * Created by: Jonathan Diller * On: Nov 25, 2025 diff --git a/nimbus/include/nimbus/planner.h b/nimbus/include/nimbus/planner.h new file mode 100644 index 0000000..d5f27a0 --- /dev/null +++ b/nimbus/include/nimbus/planner.h @@ -0,0 +1,198 @@ +/* + * planner.h + * + * Created by: Jonathan Diller + * On: Feb 16, 2026 + * + * Description: This is the path planner node for the Nimbus autonomy stack. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +// Action Interfaces +#include "router_interfaces/action/waypoint_move.hpp" +#include "router_interfaces/action/waypoint_sequence.hpp" + +// Standard Libraries +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// OpenCV +#include + +using namespace std::chrono_literals; + + +#define DEBUG_PILOT false + +// Constants +#define DEBUG_PLANNER false +#define EARTH_RADIUS_M 6371000.0 +#define INF std::numeric_limits::infinity() + + + +// ---------------------------------------------------------------------------- +// Helper Structs & Classes +// ---------------------------------------------------------------------------- + +struct WaypointPosition { + double latitude; + double longitude; + double relative_altitude; +}; + +struct Point2D { + double x; + double y; +}; + +// Parse Fences +struct Poly { + std::vector points; + int type; // 0 = exclusion, 1 = inclusion +}; + +struct AStarNode { + int id; + double f_score; + bool operator>(const AStarNode& other) const { return f_score > other.f_score; } +}; + +enum E_MavCMDPolygonType { + INCLUSIVE = 5001, + EXCLUSIVE = 5002 +}; + + + +class PX4Mission { +public: + PX4Mission() : has_mission_(false) {} + + void set_mission(const mavros_msgs::msg::WaypointList::SharedPtr msg); + bool get_waypoint(size_t index, WaypointPosition& out_wp); + // Fills waypoints vector with waypoints that we currently track. Returns false if we do not have a PX4 mission, true o.w. + bool get_all_waypoints(std::vector* waypoints); + bool has_mission(); + bool valid_waypoint(size_t index); + +private: + std::mutex ms_mutex_; + mavros_msgs::msg::WaypointList latest_mission_; + bool has_mission_; +}; + + +class PX4Geofence { +public: + PX4Geofence() : has_fence_(false) {} + + void set_fence(const mavros_msgs::msg::WaypointList::SharedPtr msg); + // Fills vector with geofence waypoints that we currently track. Returns false if we do not have any geofence data, true o.w. + bool get_fence_points(std::vector* fence_points); + bool has_fence(); + +private: + std::mutex gf_mutex_; + mavros_msgs::msg::WaypointList latest_fence_; + bool has_fence_; +}; + + +class QuadPosition { +public: + QuadPosition() : has_position_(false) {} + + void set_position(const sensor_msgs::msg::NavSatFix::SharedPtr msg); + bool get_position(sensor_msgs::msg::NavSatFix& out_pos); + +private: + std::mutex pos_mutex_; + sensor_msgs::msg::NavSatFix latest_position_; + bool has_position_; +}; + + +class NimbusPlanner : public rclcpp::Node { +public: + using WaypointMove = router_interfaces::action::WaypointMove; + using WaypointSequence = router_interfaces::action::WaypointSequence; + using GoalHandleWaypointMove = rclcpp_action::ServerGoalHandle; + using GoalHandleWaypointSequence = rclcpp_action::ClientGoalHandle; + + NimbusPlanner(); + +private: + // Internal Data + PX4Mission px4_mission_; + PX4Geofence px4_geofence_; + QuadPosition quad_position_; + + // Subscribers + rclcpp::Subscription::SharedPtr mission_sub_; + rclcpp::Subscription::SharedPtr geofence_sub_; + rclcpp::Subscription::SharedPtr position_sub_; + + // Action server/clients + rclcpp::CallbackGroup::SharedPtr cb_group_; + rclcpp_action::Server::SharedPtr action_server_; + rclcpp_action::Client::SharedPtr navigator_client_; + + // Startup timer + rclcpp::TimerBase::SharedPtr startup_timer_; + + // Graph: ID -> (Neighbor ID -> Weight) + std::unordered_map> graph_; + + // Config + const double MAX_EDGE_LENGTH = 500.0; + const double MAP_RESOLUTION = 10.0; + const double MAP_BUFFER = 50.0; + const int OBSTACLE_DILATION = 1; + + // --- Graph building functions --- + bool has_graph_data(); + // Build waypoint graph + void build_graph(); + + // --- Callbacks --- + void startup_check_callback(); + void mission_callback(const mavros_msgs::msg::WaypointList::SharedPtr msg); + void geofence_callback(const mavros_msgs::msg::WaypointList::SharedPtr msg); + void position_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg); + + // --- Action Server Logic --- + rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); + rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr goal_handle); + void handle_accepted(const std::shared_ptr goal_handle); + // Sub-process function for executing waypoint move + void execute(const std::shared_ptr goal_handle); + + // --- Helpers --- + int get_closest_waypoint(); + // Converts Lat/Lon to Meters (Local Tangent Plane) relative to an origin + Point2D latlon_to_meters(double lat, double lon, double origin_lat, double origin_lon); + // Haversine Distance + double get_distance_meters(double lat1, double lon1, double lat2, double lon2); + + // --- A* Search --- + bool calculate_route(int start, int end, std::vector* route); + double heuristic(int node_id, int goal_id); + bool A_start(int start, int end, std::vector* route); +}; diff --git a/nimbus/launch/nimbus.launch b/nimbus/launch/nimbus.launch index e4986fe..aab4bb1 100644 --- a/nimbus/launch/nimbus.launch +++ b/nimbus/launch/nimbus.launch @@ -23,11 +23,11 @@ - + - + diff --git a/nimbus/nimbus/nimbus_planner.py b/nimbus/nimbus/nimbus_planner.py index 530e2ec..b3f2d61 100644 --- a/nimbus/nimbus/nimbus_planner.py +++ b/nimbus/nimbus/nimbus_planner.py @@ -7,7 +7,6 @@ import heapq import math from threading import Lock -from threading import Lock # ROS 2 stuff import rclpy diff --git a/nimbus/package.xml b/nimbus/package.xml index fc29332..4ffc8ea 100644 --- a/nimbus/package.xml +++ b/nimbus/package.xml @@ -16,6 +16,7 @@ nav_msgs mavros_msgs router_interfaces + libopencv-dev ament_lint_auto ament_lint_common diff --git a/nimbus/src/navigator.cpp b/nimbus/src/navigator.cpp new file mode 100644 index 0000000..94a09e6 --- /dev/null +++ b/nimbus/src/navigator.cpp @@ -0,0 +1,277 @@ +#include "nimbus/navigator.h" + + +/*************************** + * + * Navigator Class + * + ***************************/ + +using namespace std::placeholders; + +Navigator::Navigator() : Node("navigator") { + // Declare parameters + this->declare_parameter("acceptance_radius", DEFAULT_ACCEPTANCE_RADIUS); + + // Get the acceptance radius + try { + acceptance_radius_ = this->get_parameter("acceptance_radius").as_double(); + } + catch (const rclcpp::ParameterTypeException & e) { + RCLCPP_ERROR(this->get_logger(), "Acceptance radius type mismatch."); + acceptance_radius_ = DEFAULT_ACCEPTANCE_RADIUS; + } + + // Validate acceptance radius + if(acceptance_radius_ < 1.0 || acceptance_radius_ > 20.0) { + RCLCPP_INFO(this->get_logger(), "%s: Acceptance radius should be a float between 1 and 20", this->get_name()); + rclcpp::shutdown(); + return; + } + + // Initialize state + current_client_goal_handle_ = nullptr; + reached_waypoint_ = true; + dist_to_waypoint_ = 0.0; + dist_total_ = -1.0; + + // Create action client + wp_action_client_ = rclcpp_action::create_client( + this, + "/nimbus/pilot/set_waypoint" + ); + + // Create the action server + move_action_server_ = rclcpp_action::create_server( + this, + "navigator/set_route", + std::bind(&Navigator::position_goal_callback, this, _1, _2), + std::bind(&Navigator::position_cancel_callback, this, _1), + std::bind(&Navigator::position_accepted_callback, this, _1) + ); + + RCLCPP_INFO(this->get_logger(), "Started %s:, AR: %.2f", this->get_name(), acceptance_radius_); +} + +// --- Request Waypoint action from pilot -------------------------------- +void Navigator::send_wp_goal(int waypoint, float tolerance) { + // Wait for the action server to come online + if(!wp_action_client_->wait_for_action_server(std::chrono::seconds(5))) { + RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); + return; + } + + // Create goal message + auto goal_msg = WaypointMove::Goal(); + goal_msg.waypoint = waypoint; + goal_msg.tolerance = tolerance; + + // Goal tracking + reached_waypoint_ = false; + + // Logging + RCLCPP_INFO(this->get_logger(), "Sending pilot waypoint %d, tolerance = %.2f", waypoint, tolerance); + + // Send goal asynchronously + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + + // Attach callbacks + send_goal_options.goal_response_callback = + std::bind(&Navigator::wp_response_callback, this, _1); + + send_goal_options.feedback_callback = + std::bind(&Navigator::wp_feedback_callback, this, _1, _2); + + send_goal_options.result_callback = + std::bind(&Navigator::wp_result_callback, this, _1); + + wp_action_client_->async_send_goal(goal_msg, send_goal_options); +} + +// --- Waypoint request response ----------------------------------------- +void Navigator::wp_response_callback(const GoalHandleWaypointMove::SharedPtr & goal_handle) { + if(!goal_handle) { + RCLCPP_WARN(this->get_logger(), "Waypoint rejected"); + reached_waypoint_ = true; + return; + } + + RCLCPP_INFO(this->get_logger(), "Waypoint accepted"); + + // Store the action handle (used to cancel action, if needed) + current_client_goal_handle_ = goal_handle; +} + +// --- Waypoint request feedback ----------------------------------------- +void Navigator::wp_feedback_callback(GoalHandleWaypointMove::SharedPtr, const std::shared_ptr feedback_msg) { + dist_to_waypoint_ = feedback_msg->distance_to_go; + + if(dist_total_ < 0) { + dist_total_ = feedback_msg->distance_to_go; + } + + // Logging + if(DEBUG_NAV) { + RCLCPP_INFO(this->get_logger(), "Distance to go: %.2f m", feedback_msg->distance_to_go); + } +} + +// --- Waypoint request final result ------------------------------------- +void Navigator::wp_result_callback(const GoalHandleWaypointMove::WrappedResult & result) { + // Regardless of the result, mark that the action ended + reached_waypoint_ = true; + dist_total_ = -1; + + bool success = (result.code == rclcpp_action::ResultCode::SUCCEEDED); + RCLCPP_INFO(this->get_logger(), "Waypoint action result: %s", success ? "True" : "False"); +} + +// --- Position goal callback ------------------------------------------ +rclcpp_action::GoalResponse Navigator::position_goal_callback(const rclcpp_action::GoalUUID&, std::shared_ptr goal_request) { + RCLCPP_INFO(this->get_logger(), "Received WaypointSequence request, tolerance: %.2f", goal_request->tolerance); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +// --- Position cancel handling ------------------------------------------ +rclcpp_action::CancelResponse Navigator::position_cancel_callback(const std::shared_ptr) { + RCLCPP_INFO(this->get_logger(), "Received cancel request."); + return rclcpp_action::CancelResponse::ACCEPT; +} + +// --- Position accepted callback ---------------------------------------- +void Navigator::position_accepted_callback(const std::shared_ptr goal_handle) { + // Execute action (follow route) in new thread + std::thread{std::bind(&Navigator::position_execute_callback, this, _1), goal_handle}.detach(); +} + +// --- Main position-move execution -------------------------------------- +void Navigator::position_execute_callback(const std::shared_ptr goal_handle) { + RCLCPP_INFO(this->get_logger(), "Executing WaypointSequence action..."); + + const auto goal = goal_handle->get_goal(); + + // Copy the route so we can modify (pop) it + std::queue quad_route; + for(const int wp : goal->waypoints) { + quad_route.push(wp); + } +// std::vector quad_route = goal->waypoints; + + // Waypoint tolerance should be no less than navigator's acceptance radius + double waypoint_tolerance = std::max((double)goal->tolerance, acceptance_radius_); + + // Verify that the sequence isn't empty... + if(quad_route.empty()) { + RCLCPP_INFO(this->get_logger(), "Received empty waypoint sequence!"); + auto result = std::make_shared(); + result->success = true; + goal_handle->succeed(result); + return; + } + + // Print the goal info (Accessing last element safely) + RCLCPP_INFO( + this->get_logger(), + "Received WaypointSequence w/ %zu stops, going to %d", + quad_route.size(), + quad_route.back() + ); + + // Track progress along route + auto feedback_msg = std::make_shared(); + double work_complete = 0; + double total_work = static_cast(quad_route.size()); + + // Send first waypoint + send_wp_goal(quad_route.front(), waypoint_tolerance); + quad_route.pop(); + + rclcpp::Rate loop_rate(10); // Check status at 10Hz + + // Run action-loop + while(rclcpp::ok()) { + // Did the action get cancelled..? + if(goal_handle->is_canceling()) { + RCLCPP_INFO(this->get_logger(), "Canceling current waypoint move..."); + + // Is there an active sub-action running? + if(current_client_goal_handle_ != nullptr) { + // Request cancellation for pilot's move action + wp_action_client_->async_cancel_goal(current_client_goal_handle_); + } + + auto result = std::make_shared(); + result->success = false; + goal_handle->canceled(result); + return; + } + + // Did we reach the current waypoint? + if(reached_waypoint_) { + work_complete += 1.0; + + // Did we hit the end of the waypoint queue? + if(quad_route.empty()) { + // Done, break! + break; + } + else { + // Request next waypoint + send_wp_goal(quad_route.front(), waypoint_tolerance); + quad_route.pop(); + } + } + + // Update progress + double progress = 0.0; + if(dist_total_ > 0) { + // Add in mid-waypoint progress + progress = work_complete / total_work + + ((1.0 / total_work) * (1.0 - dist_to_waypoint_ / dist_total_)); + } + else { + // Just report the waypoint progress + progress = work_complete / total_work; + } + + // Report progress + feedback_msg->progress = progress; + goal_handle->publish_feedback(feedback_msg); + + if(DEBUG_NAV) { + RCLCPP_INFO(this->get_logger(), "Progress: %.2f", progress); + } + + // Sleep to maintain rate + loop_rate.sleep(); + } + + // Done + auto result = std::make_shared(); + result->success = true; + + RCLCPP_INFO(this->get_logger(), "WaypointSequence complete."); + goal_handle->succeed(result); +} + + + +/*************************** + * + * main function to launch Navigator node + * + ***************************/ + +int main(int argc, char ** argv) { + rclcpp::init(argc, argv); + + auto nav_node = std::make_shared(); + + // Spin with MultiThreadedExecutor + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(nav_node); + executor.spin(); + + rclcpp::shutdown(); + return 0; +} diff --git a/nimbus/src/pilot.cpp b/nimbus/src/pilot.cpp index f0f8739..c3d40bb 100644 --- a/nimbus/src/pilot.cpp +++ b/nimbus/src/pilot.cpp @@ -339,8 +339,8 @@ void Pilot::execute(const std::shared_ptr goal_handle) { return; } - // Run this feedback thread at 4 Hz - rclcpp::Rate loop_rate(4.0); + // Run this feedback thread at 30 Hz + rclcpp::Rate loop_rate(30.0); // While ROS is still running... while(rclcpp::ok()) { diff --git a/nimbus/src/planner.cpp b/nimbus/src/planner.cpp new file mode 100644 index 0000000..5b9986b --- /dev/null +++ b/nimbus/src/planner.cpp @@ -0,0 +1,710 @@ +#include "nimbus/planner.h" + + +/*************************** + * + * PX4Mission Class + * + ***************************/ + +void PX4Mission::set_mission(const mavros_msgs::msg::WaypointList::SharedPtr msg) { + // Get mutex + std::lock_guard lock(ms_mutex_); + // Save the waypoint mission + latest_mission_ = *msg; + has_mission_ = true; +} + +bool PX4Mission::get_waypoint(size_t index, WaypointPosition& out_wp) { + // Get mutex + std::lock_guard lock(ms_mutex_); + // Input validation + if(!has_mission_ || !(index < latest_mission_.waypoints.size())) { + return false; + } + // Copy over waypoint data + const auto& wp = latest_mission_.waypoints[index]; + out_wp = {wp.x_lat, wp.y_long, wp.z_alt}; + return true; +} + +// Fills waypoints vector with waypoints that we currently track. Returns false if we do not have a PX4 mission, true o.w. +bool PX4Mission::get_all_waypoints(std::vector* waypoints) { + // Get mutex + std::lock_guard lock(ms_mutex_); + // Verify we have a current mission + if(!has_mission_) { + return false; + } + else { + // Move over all waypoints + for(auto wp : latest_mission_.waypoints) { + waypoints->push_back(wp); + } + // Return successful + return true; + } +} + +bool PX4Mission::has_mission() { + std::lock_guard lock(ms_mutex_); + return has_mission_; +} + +bool PX4Mission::valid_waypoint(size_t index) { + // Get mutex + std::lock_guard lock(ms_mutex_); + if(!has_mission_) + return false; + // Should be non-zero and less than size of vector + return index < latest_mission_.waypoints.size(); +} + + +/*************************** + * + * PX4Geofence Class + * + ***************************/ + +void PX4Geofence::set_fence(const mavros_msgs::msg::WaypointList::SharedPtr msg) { + // Lock mutex + std::lock_guard lock(gf_mutex_); + // Save geofence data + latest_fence_ = *msg; + has_fence_ = true; +} + +// Fills vector with geofence waypoints that we currently track. Returns false if we do not have any geofence data, true o.w. +bool PX4Geofence::get_fence_points(std::vector* fence_points) { + std::lock_guard lock(gf_mutex_); + if(!has_fence_) { + return false; + } + else { + // Move over all waypoints + for(auto wp : latest_fence_.waypoints) { + fence_points->push_back(wp); + } + return true; + } +} + + +bool PX4Geofence::has_fence() { + std::lock_guard lock(gf_mutex_); + return has_fence_; +} + + +/*************************** + * + * QuadPosition Class + * + ***************************/ + +void QuadPosition::set_position(const sensor_msgs::msg::NavSatFix::SharedPtr msg) { + // Lock mutex + std::lock_guard lock(pos_mutex_); + // Save position + latest_position_ = *msg; + has_position_ = true; +} + +bool QuadPosition::get_position(sensor_msgs::msg::NavSatFix& out_pos) { + // Lock mutex + std::lock_guard lock(pos_mutex_); + // If we have a position... + if(!has_position_) { + return false; + } + else { + // Copy over the position + out_pos = latest_position_; + return true; + } +} + + +/*************************** + * + * NimbusPlanner Class + * + ***************************/ + +NimbusPlanner::NimbusPlanner() : Node("nimbus_planner") { + // QoS + rclcpp::QoS best_effort(10); + best_effort.reliability(rclcpp::ReliabilityPolicy::BestEffort); + + /// Subscribers + // Subscribe to PX4 mission + mission_sub_ = this->create_subscription( + "/mavros/mission/waypoints", best_effort, + std::bind(&NimbusPlanner::mission_callback, this, std::placeholders::_1)); + // Subscribe to Geofence data + geofence_sub_ = this->create_subscription( + "/mavros/geofence/fences", best_effort, + std::bind(&NimbusPlanner::geofence_callback, this, std::placeholders::_1)); + // Subscribe to position + position_sub_ = this->create_subscription( + "/mavros/global_position/global", best_effort, + std::bind(&NimbusPlanner::position_callback, this, std::placeholders::_1)); + + // Create callback group for concurrent Action Server/Client execution + cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + + // Move to waypoint action server + action_server_ = rclcpp_action::create_server( + this, + "planner/move_to_waypoint", + std::bind(&NimbusPlanner::handle_goal, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&NimbusPlanner::handle_cancel, this, std::placeholders::_1), + std::bind(&NimbusPlanner::handle_accepted, this, std::placeholders::_1), + rcl_action_server_get_default_options(), + cb_group_); + + // Set route action client + navigator_client_ = rclcpp_action::create_client( + this, "/nimbus/navigator/set_route", cb_group_); + + // Startup timer for building graph + startup_timer_ = this->create_wall_timer( + std::chrono::milliseconds(500), + std::bind(&NimbusPlanner::startup_check_callback, this)); + + RCLCPP_INFO(this->get_logger(), "Nimbus Planner initialized."); +} + + + +//////////////////// +// +/// Private +// +//////////////////// + + +// --- Public Interface for Main Loop --- +bool NimbusPlanner::has_graph_data() { + return px4_mission_.has_mission() && px4_geofence_.has_fence(); +} + +void NimbusPlanner::build_graph() { + std::vector mission_wps; + if(!px4_mission_.get_all_waypoints(&mission_wps)) { + RCLCPP_WARN(this->get_logger(), "No mission waypoints to build graph."); + return; + } + + std::vector fence_wps; + if(!px4_geofence_.get_fence_points(&fence_wps)) { + RCLCPP_WARN(this->get_logger(), "No geofence data to build graph."); + return; + } + + // Establish coordinate system (Origin = Waypoint 0) + double origin_lat = mission_wps[0].x_lat; + double origin_lon = mission_wps[0].y_long; + + // Convert mission nodes to local meters + std::vector nodes_local; + for(const auto& wp : mission_wps) { + nodes_local.push_back(latlon_to_meters(wp.x_lat, wp.y_long, origin_lat, origin_lon)); + } + + // Parse geofences and calculate map bounds + double min_x = INF, max_x = -INF, min_y = INF, max_y = -INF; + + // Include nodes in bounds + for(const auto& p : nodes_local) { + if(p.x < min_x) + min_x = p.x; + if(p.x > max_x) + max_x = p.x; + if(p.y < min_y) + min_y = p.y; + if(p.y > max_y) + max_y = p.y; + } + + // Store all obstacle polygons + std::vector polygons; + + // State variables + std::vector current_poly_pts; + E_MavCMDPolygonType current_type = E_MavCMDPolygonType::EXCLUSIVE; + int vertices_remaining = 0; + + // For each geofence waypoint + for(const auto& wp : fence_wps) { + // Check if we are starting a NEW polygon + if(vertices_remaining <= 0) { + // Clear previous points to start fresh + current_poly_pts.clear(); + + // How many vertices are in this polygon? + vertices_remaining = static_cast(wp.param1); + + // Safety check: avoid infinite loops if param1 is garbage + if(vertices_remaining <= 0) + continue; + + // Determine the type for this new polygon + if(wp.command == E_MavCMDPolygonType::INCLUSIVE) { + current_type = E_MavCMDPolygonType::INCLUSIVE; + } + else if(wp.command == E_MavCMDPolygonType::EXCLUSIVE) { + current_type = E_MavCMDPolygonType::EXCLUSIVE; + } + else { + // Bad command... + RCLCPP_WARN(this->get_logger(), "Geofence waypoint not expected type: %d", wp.command); + return; + } + } + + // Process the Point + Point2D p = latlon_to_meters(wp.x_lat, wp.y_long, origin_lat, origin_lon); + + // Update bounds + if(p.x < min_x) min_x = p.x; + if(p.x > max_x) max_x = p.x; + if(p.y < min_y) min_y = p.y; + if(p.y > max_y) max_y = p.y; + + // Add point to the current working polygon + current_poly_pts.push_back(p); + + // Decrement the counter + vertices_remaining--; + + // Check if we have finished this polygon + if(vertices_remaining == 0) { + // Polygon is complete, save it + polygons.push_back({current_poly_pts, current_type}); + } + } + + // Create OpenCV map + double width_m = max_x - min_x + (2 * MAP_BUFFER); + double height_m = max_y - min_y + (2 * MAP_BUFFER); + + int img_w = std::ceil(width_m * MAP_RESOLUTION); + int img_h = std::ceil(height_m * MAP_RESOLUTION); + + double offset_x = -min_x + MAP_BUFFER; + double offset_y = -min_y + MAP_BUFFER; + + // Coord Transform to Pixel + auto to_pix = [&](Point2D p) -> cv::Point { + int px = (p.x + offset_x) * MAP_RESOLUTION; + int py = img_h - ((p.y + offset_y) * MAP_RESOLUTION); // Flip Y + return cv::Point(px, py); + }; + + // Initialize Map (0 = Safe, 255 = Obstacle) + cv::Mat obst_map = cv::Mat::zeros(img_h, img_w, CV_8UC1); + + // Add obstacle polygons + for(const auto& poly : polygons) { + if(poly.type == E_MavCMDPolygonType::EXCLUSIVE) { + std::vector pts_pix; + for(auto& p : poly.points) pts_pix.push_back(to_pix(p)); + + std::vector> pts_wrapper = {pts_pix}; + cv::fillPoly(obst_map, pts_wrapper, cv::Scalar(255)); + } + } + + // Dilate obstacles + int kernel_size = OBSTACLE_DILATION * MAP_RESOLUTION; + if (kernel_size > 0) { + cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, + cv::Size(2*kernel_size+1, 2*kernel_size+1)); + cv::dilate(obst_map, obst_map, element); + } + + // Build the graph + graph_.clear(); + int num_edges = 0; + + for(size_t i = 0; i < nodes_local.size(); ++i) { + for(size_t j = i + 1; j < nodes_local.size(); ++j) { + // Distance check + double d_x = nodes_local[i].x - nodes_local[j].x; + double d_y = nodes_local[i].y - nodes_local[j].y; + double dist = std::sqrt(d_x*d_x + d_y*d_y); + + if(dist > MAX_EDGE_LENGTH) + continue; + + // Collision Check + cv::Point p1 = to_pix(nodes_local[i]); + cv::Point p2 = to_pix(nodes_local[j]); + + // Create a line mask for this edge + cv::LineIterator it(obst_map, p1, p2, 8); + bool collision = false; + for(int k = 0; k < it.count; k++, ++it) { + if(obst_map.at(it.pos()) > 0) { // 255 = Obstacle + collision = true; + break; + } + } + + if(!collision) { + graph_[i][j] = dist; + graph_[j][i] = dist; + num_edges++; + } + } + } + + RCLCPP_INFO(this->get_logger(), "Graph built with %zu nodes and %d edges.", nodes_local.size(), num_edges); +} + +// Startup callback to build graph +void NimbusPlanner::startup_check_callback() { + // Do we have all needed data to build the graph? + if(this->has_graph_data()) { + // Yes, build the graph + RCLCPP_INFO(this->get_logger(), "Building graph..."); + build_graph(); + + // Stop timer + startup_timer_->cancel(); + + RCLCPP_INFO(this->get_logger(), "Startup complete. Nimbus Planner ready."); + } + else { + RCLCPP_INFO(this->get_logger(), "Waiting for valid PX4 Mission and Geofence..."); + } +} + + +void NimbusPlanner::mission_callback(const mavros_msgs::msg::WaypointList::SharedPtr msg) { + RCLCPP_INFO(this->get_logger(), "Received %zu mission waypoints.", msg->waypoints.size()); + px4_mission_.set_mission(msg); +} + +void NimbusPlanner::geofence_callback(const mavros_msgs::msg::WaypointList::SharedPtr msg) { + RCLCPP_INFO(this->get_logger(), "Received %zu geofence vertices.", msg->waypoints.size()); + px4_geofence_.set_fence(msg); +} + +void NimbusPlanner::position_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg) { + quad_position_.set_position(msg); +} + + +rclcpp_action::GoalResponse NimbusPlanner::handle_goal(const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal) { + (void)uuid; + RCLCPP_INFO(this->get_logger(), "Received goal request for WP %d", goal->waypoint); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse NimbusPlanner::handle_cancel(const std::shared_ptr goal_handle) { + (void)goal_handle; + RCLCPP_INFO(this->get_logger(), "Received cancel request"); + return rclcpp_action::CancelResponse::ACCEPT; +} + +void NimbusPlanner::handle_accepted(const std::shared_ptr goal_handle) { + // Execute in new thread + std::thread{std::bind(&NimbusPlanner::execute, this, std::placeholders::_1), goal_handle}.detach(); +} + +// Sub-process function for executing waypoint move +void NimbusPlanner::execute(const std::shared_ptr goal_handle) { + auto result = std::make_shared(); + auto feedback = std::make_shared(); + const auto goal = goal_handle->get_goal(); + + RCLCPP_INFO(this->get_logger(), "Executing WaypointMove --> %d", goal->waypoint); + + // Check that the navigator is running + if(!navigator_client_->wait_for_action_server(std::chrono::seconds(5))) { + RCLCPP_ERROR(this->get_logger(), "Navigator Action Server not available!"); + result->success = false; + goal_handle->abort(result); + return; + } + + // Validate requested waypoint + if(!px4_mission_.valid_waypoint(goal->waypoint)) { + RCLCPP_ERROR(this->get_logger(), "Target waypoint %d is invalid.", goal->waypoint); + result->success = false; + goal_handle->abort(result); + return; + } + + // Get closest waypoint to current position + int start_idx = get_closest_waypoint(); + if(start_idx == -1) { + // Failed to find a start point.. we might not have current quad position, or no mission + RCLCPP_ERROR(this->get_logger(), "Could not determine start index (no GPS/PX4 Mission?)"); + result->success = false; + goal_handle->abort(result); + return; + } + + // Do path planning + std::vector route; + if(!calculate_route(start_idx, goal->waypoint, &route)) { + RCLCPP_WARN(this->get_logger(), "No route found to waypoint %d", goal->waypoint); + result->success = false; + goal_handle->abort(result); + return; + } + + // Push route into WaypointSequence message + auto nav_goal = WaypointSequence::Goal(); + for(int wp : route) { + nav_goal.waypoints.push_back(wp); + } + nav_goal.tolerance = goal->tolerance; + + RCLCPP_INFO(this->get_logger(), "Sending route (sized = %zu) to Navigator.", route.size()); + + // Send route to nav + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.feedback_callback = + [goal_handle, feedback]( + GoalHandleWaypointSequence::SharedPtr, + const std::shared_ptr nav_fb) + { + feedback->distance_to_go = nav_fb->progress; + goal_handle->publish_feedback(feedback); + }; + auto future_goal = navigator_client_->async_send_goal(nav_goal, send_goal_options); + + // Wait for nav to accept the route + if(future_goal.wait_for(std::chrono::seconds(2)) != std::future_status::ready) { + RCLCPP_ERROR(this->get_logger(), "Navigator timed out before accepting goal"); + result->success = false; + goal_handle->abort(result); + return; + } + + // Verify that the nav responded positively... + auto nav_goal_handle = future_goal.get(); + if(!nav_goal_handle) { + RCLCPP_ERROR(this->get_logger(), "Navigator rejected route!"); + result->success = false; + goal_handle->abort(result); + return; + } + + // Monitor execution loop + auto result_future = navigator_client_->async_get_result(nav_goal_handle); + + // Run this feedback thread at 15 Hz + rclcpp::Rate loop_rate(15.0); + + while(rclcpp::ok()) { + // Check if someone cancelled move action + if(goal_handle->is_canceling()) { + // This action was cancelled... + RCLCPP_INFO(this->get_logger(), "Canceling Navigator goal..."); + // Tell the navigator to cancel running the route + navigator_client_->async_cancel_goal(nav_goal_handle); + result->success = false; + goal_handle->canceled(result); + return; + } + + // Check if the nav finished route action + if(result_future.wait_for(0s) == std::future_status::ready) { + auto wrapped_result = result_future.get(); + if(wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) { + RCLCPP_INFO(this->get_logger(), "Route execution complete."); + result->success = true; + goal_handle->succeed(result); + } + else { + RCLCPP_WARN(this->get_logger(), "Navigator execution failed."); + result->success = false; + goal_handle->abort(result); + } + return; + } + + loop_rate.sleep(); + } +} + +// --- Helpers --- +int NimbusPlanner::get_closest_waypoint() { + // Get our current position + sensor_msgs::msg::NavSatFix pos; + if(!quad_position_.get_position(pos)) { + RCLCPP_WARN(this->get_logger(), "get_closest_waypoint() : could not access current position"); + return -1; + } + + std::vector wps; + if(!px4_mission_.get_all_waypoints(&wps)) { + RCLCPP_WARN(this->get_logger(), "get_closest_waypoint() : could not access mission data"); + return -1; + } + + int best_idx = -1; + double min_dist = INF; + + // Cycle through each waypoint + for(size_t i = 0; i < wps.size(); ++i) { + // Get distance to this waypoint + double d = get_distance_meters(pos.latitude, pos.longitude, wps[i].x_lat, wps[i].y_long + ); + + // Is this better that current closest? + if(d < min_dist) { + // Yes, update + min_dist = d; + best_idx = static_cast(i); + } + } + + return best_idx; +} + +// Converts Lat/Lon to Meters (Local Tangent Plane) relative to an origin +Point2D NimbusPlanner::latlon_to_meters(double lat, double lon, double origin_lat, double origin_lon) { + double d_lat = (lat - origin_lat) * M_PI / 180.0; + double d_lon = (lon - origin_lon) * M_PI / 180.0; + + // Y is North (d_lat) + // X is East (d_lon * cos(lat)) + double r_lat = origin_lat * M_PI / 180.0; + + double x = EARTH_RADIUS_M * d_lon * std::cos(r_lat); + double y = EARTH_RADIUS_M * d_lat; + + return {x, y}; +} + +// Haversine Distance +double NimbusPlanner::get_distance_meters(double lat1, double lon1, double lat2, double lon2) { + double dlat = (lat2 - lat1) * M_PI / 180.0; + double dlon = (lon2 - lon1) * M_PI / 180.0; + double a = std::sin(dlat / 2) * std::sin(dlat / 2) + + std::cos(lat1 * M_PI / 180.0) * std::cos(lat2 * M_PI / 180.0) * + std::sin(dlon / 2) * std::sin(dlon / 2); + double c = 2 * std::atan2(std::sqrt(a), std::sqrt(1 - a)); + return EARTH_RADIUS_M * c; +} + +bool NimbusPlanner::calculate_route(int start, int end, std::vector* route) { + // Clear anything in the route vector + route->clear(); + + // Verify that we have a local graph + if(graph_.empty()) { + RCLCPP_WARN(this->get_logger(), "calculate_route() : Graph is empty!"); + return false; + } + + // Validate that the start and end nodes exist in graph and in PX4 Mission + if(!px4_mission_.valid_waypoint(start)) { + RCLCPP_WARN(this->get_logger(), "calculate_route() : Bad start waypoint: %d", start); + return false; + } + else if(!px4_mission_.valid_waypoint(end)) { + RCLCPP_WARN(this->get_logger(), "calculate_route() : Bad target waypoint: %d", end); + return false; + } + else if(graph_.count(start) <= 0) { + RCLCPP_WARN(this->get_logger(), "calculate_route() : Graph has no data for ID %d", start); + return false; + } + else if(graph_.count(end) <= 0) { + RCLCPP_WARN(this->get_logger(), "calculate_route() : Graph has no data for ID %d", end); + return false; + } + else { + // If we made it this far, it should be safe to run A* + return A_start(start, end, route); + } +} + +double NimbusPlanner::heuristic(int current_id, int goal_id) { + WaypointPosition current_wp; + WaypointPosition goal_wp; + + // Fetch waypoints + if(!px4_mission_.get_waypoint(current_id, current_wp) || !px4_mission_.get_waypoint(goal_id, goal_wp)) { + return INF; + } + else { + // Return Euclidean distance + return get_distance_meters(current_wp.latitude, current_wp.longitude, goal_wp.latitude, goal_wp.longitude); + } +}; + +bool NimbusPlanner::A_start(int start, int end, std::vector* route) { + // Create priority queue, track progress + std::priority_queue, std::greater> open_set; + std::unordered_map g_score; + std::unordered_map came_from; + + // Push start onto queue + g_score[start] = 0.0; + open_set.push({start, heuristic(start, end)}); + + // While there are still nodes in the priority queue + while(!open_set.empty()) { + // Get next node + int current = open_set.top().id; + open_set.pop(); + + // Did we hit the target? + if(current == end) { + // Yes, reconstruct the route + while (current != start) { + route->push_back(current); + current = came_from[current]; + } + route->push_back(start); + std::reverse(route->begin(), route->end()); + return true; + } + + // Explore all neighbors of the current node + for(auto& edge : graph_[current]) { + // Calculate new cost + int neighbor = edge.first; + double weight = edge.second; + double tentative_g = g_score[current] + weight; + + // Is this a new node, or a better cost than the previous cost? + if(g_score.find(neighbor) == g_score.end() || tentative_g < g_score[neighbor]) { + // Add/update cost + came_from[neighbor] = current; + g_score[neighbor] = tentative_g; + double f = tentative_g + heuristic(neighbor, end); + open_set.push({neighbor, f}); + } + } + } + + // If we made it this far, there is no route to the target in the graph + RCLCPP_WARN(this->get_logger(), "A_start() : Failed to find route %d --> %d", start, end); + return false; +} + + + +int main(int argc, char **argv) { + // Create node + rclcpp::init(argc, argv); + auto node = std::make_shared(); + + // Spin with MultiThreadedExecutor + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); + + rclcpp::shutdown(); + return 0; +}