From 5efa8579ca1fd7daff7a8dcc05fd53df3ee9ef4f Mon Sep 17 00:00:00 2001 From: JonD07 Date: Mon, 16 Feb 2026 17:23:33 -0500 Subject: [PATCH 1/3] Initial pilot port --- nimbus/CMakeLists.txt | 23 +- nimbus/include/nimbus/pilot.h | 2 +- nimbus/include/nimbus/planner.h | 15 + nimbus/launch/nimbus.launch | 2 +- nimbus/nimbus/nimbus_planner.py | 1 - nimbus/package.xml | 1 + nimbus/src/planner.cpp | 677 ++++++++++++++++++++++++++++++++ 7 files changed, 717 insertions(+), 4 deletions(-) create mode 100644 nimbus/include/nimbus/planner.h create mode 100644 nimbus/src/planner.cpp diff --git a/nimbus/CMakeLists.txt b/nimbus/CMakeLists.txt index 3a844ec..64ca7f6 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,25 @@ install(TARGETS pilot 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/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..8ce0efc --- /dev/null +++ b/nimbus/include/nimbus/planner.h @@ -0,0 +1,15 @@ +/* + * planner.h + * + * Created by: Jonathan Diller + * On: Feb 16, 2026 + * + * Description: This is the path planner node for the Nimbus autonomy stack. + */ + +#pragma once + + +#define DEBUG_PILOT false + + diff --git a/nimbus/launch/nimbus.launch b/nimbus/launch/nimbus.launch index e4986fe..668d8f8 100644 --- a/nimbus/launch/nimbus.launch +++ b/nimbus/launch/nimbus.launch @@ -27,7 +27,7 @@ - + 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/planner.cpp b/nimbus/src/planner.cpp new file mode 100644 index 0000000..7fbbb4b --- /dev/null +++ b/nimbus/src/planner.cpp @@ -0,0 +1,677 @@ +#include "nimbus/planner.h" + + +/*************************** + * + * QuadPosition Class + * + ***************************/ + + +#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; + +// Constants +#define DEBUG_PLANNER false +#define EARTH_RADIUS_M 6371000.0 +#define INF std::numeric_limits::infinity() + +// MAVLink Commands +#define MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION 5001 +#define MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION 5002 + +// ---------------------------------------------------------------------------- +// Helper Structs & Classes +// ---------------------------------------------------------------------------- + +struct WaypointPosition { + double latitude; + double longitude; + double relative_altitude; +}; + +struct Point2D { + double x; + double y; +}; + +// --- Coordinate Math Helpers (Replaces UTM) --- + +// 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) { + 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 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; +} + +// --- Thread-Safe Data Classes --- + +class PX4Mission { +public: + PX4Mission() : has_mission_(false) {} + + void set_mission(const mavros_msgs::msg::WaypointList::SharedPtr msg) { + std::lock_guard lock(mutex_); + latest_mission_ = *msg; + has_mission_ = true; + } + + bool get_waypoint(size_t index, WaypointPosition& out_wp) { + std::lock_guard lock(mutex_); + if (!has_mission_ || index >= latest_mission_.waypoints.size()) { + return false; + } + const auto& wp = latest_mission_.waypoints[index]; + out_wp = {wp.x_lat, wp.y_long, wp.z_alt}; + return true; + } + + std::vector get_all_waypoints() { + std::lock_guard lock(mutex_); + if (!has_mission_) return {}; + return latest_mission_.waypoints; + } + + bool has_mission() { + std::lock_guard lock(mutex_); + return has_mission_; + } + + bool valid_waypoint(int index) { + std::lock_guard lock(mutex_); + if (!has_mission_) return false; + return (index >= 0 && index < (int)latest_mission_.waypoints.size()); + } + +private: + std::mutex 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) { + std::lock_guard lock(mutex_); + latest_fence_ = *msg; + has_fence_ = true; + } + + std::vector get_fence_points() { + std::lock_guard lock(mutex_); + if (!has_fence_) return {}; + return latest_fence_.waypoints; + } + + bool has_fence() { + std::lock_guard lock(mutex_); + return has_fence_; + } + +private: + std::mutex 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) { + std::lock_guard lock(mutex_); + latest_position_ = *msg; + has_position_ = true; + } + + bool get_position(sensor_msgs::msg::NavSatFix& out_pos) { + std::lock_guard lock(mutex_); + if (!has_position_) return false; + out_pos = latest_position_; + return true; + } + +private: + std::mutex mutex_; + sensor_msgs::msg::NavSatFix latest_position_; + bool has_position_; +}; + +// ---------------------------------------------------------------------------- +// Nimbus Planner Node +// ---------------------------------------------------------------------------- + +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() : Node("nimbus_planner") { + + // Callback group for concurrent Action Server/Client execution + cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + + // QoS + rclcpp::QoS best_effort(10); + best_effort.reliability(rclcpp::ReliabilityPolicy::BestEffort); + + // Subscribers + mission_sub_ = this->create_subscription( + "/mavros/mission/waypoints", best_effort, + std::bind(&NimbusPlanner::mission_callback, this, std::placeholders::_1)); + + geofence_sub_ = this->create_subscription( + "/mavros/geofence/fences", best_effort, + std::bind(&NimbusPlanner::geofence_callback, this, std::placeholders::_1)); + + position_sub_ = this->create_subscription( + "/mavros/global_position/global", best_effort, + std::bind(&NimbusPlanner::position_callback, this, std::placeholders::_1)); + + // Action Server (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_ + ); + + // Action Client (Client) + navigator_client_ = rclcpp_action::create_client( + this, "/nimbus/navigator/set_route", cb_group_); + + RCLCPP_INFO(this->get_logger(), "Nimbus Planner initialized."); + } + + // --- Public Interface for Main Loop --- + bool has_data() { + return px4_mission_.has_mission() && px4_geofence_.has_fence(); + } + + void build_graph() { + RCLCPP_INFO(this->get_logger(), "Building waypoint graph..."); + + auto mission_wps = px4_mission_.get_all_waypoints(); + auto fence_wps = px4_geofence_.get_fence_points(); + + if (mission_wps.empty()) { + RCLCPP_WARN(this->get_logger(), "No mission waypoints to build graph."); + return; + } + + // 1. Establish Coordinate System (Origin = Waypoint 0) + double origin_lat = mission_wps[0].x_lat; + double origin_lon = mission_wps[0].y_long; + + // 2. 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)); + } + + // 3. Parse Geofences & 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; + } + + // Parse Fences + struct Poly { + std::vector points; + int type; // 0 = exclusion, 1 = inclusion + }; + std::vector polygons; + std::vector current_poly_pts; + int current_type = 0; // default exclusion + + for (const auto& wp : fence_wps) { + 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; + + if (wp.command == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION) { + if (current_type == 1 && !current_poly_pts.empty()) { + polygons.push_back({current_poly_pts, 1}); + current_poly_pts.clear(); + } + current_type = 0; + current_poly_pts.push_back(p); + } else if (wp.command == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION) { + if (current_type == 0 && !current_poly_pts.empty()) { + polygons.push_back({current_poly_pts, 0}); + current_poly_pts.clear(); + } + current_type = 1; + current_poly_pts.push_back(p); + } else { + current_poly_pts.push_back(p); + } + } + if (!current_poly_pts.empty()) { + polygons.push_back({current_poly_pts, current_type}); + } + + // 4. 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); + + for (const auto& poly : polygons) { + if (poly.type == 0) { // Exclusion + 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)); + } + } + + // Dilation + 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); + } + + // 5. Build 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); + } + +private: + // Internal Data + PX4Mission px4_mission_; + PX4Geofence px4_geofence_; + QuadPosition quad_position_; + + rclcpp::CallbackGroup::SharedPtr cb_group_; + + rclcpp::Subscription::SharedPtr mission_sub_; + rclcpp::Subscription::SharedPtr geofence_sub_; + rclcpp::Subscription::SharedPtr position_sub_; + + rclcpp_action::Server::SharedPtr action_server_; + rclcpp_action::Client::SharedPtr navigator_client_; + + // 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; + + // --- Callbacks --- + void 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 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 position_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg) { + quad_position_.set_position(msg); + } + + // --- Action Server Logic --- + rclcpp_action::GoalResponse 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 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 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(); + } + + void execute(const std::shared_ptr goal_handle) { + RCLCPP_INFO(this->get_logger(), "Executing WaypointMove..."); + + auto result = std::make_shared(); + auto feedback = std::make_shared(); + const auto goal = goal_handle->get_goal(); + + // 1. Check Navigator Availability + 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; + } + + // 2. Validate Target + 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; + } + + // 3. Plan Route + int start_idx = get_closest_waypoint(); + if (start_idx == -1) { + // Fallback if GPS not ready + RCLCPP_WARN(this->get_logger(), "Could not determine start index (no GPS?). Defaulting to 0."); + start_idx = 0; + } + + std::vector route = calculate_route(start_idx, goal->waypoint); + + if (route.empty()) { + RCLCPP_WARN(this->get_logger(), "No route found to waypoint %d", goal->waypoint); + result->success = false; + goal_handle->abort(result); + return; + } + + // 4. Send to Navigator + 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 size %zu to Navigator.", route.size()); + + 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 acceptance + if (future_goal.wait_for(std::chrono::seconds(2)) != std::future_status::ready) { + RCLCPP_ERROR(this->get_logger(), "Navigator timed out accepting goal."); + result->success = false; + goal_handle->abort(result); + return; + } + + auto nav_goal_handle = future_goal.get(); + if (!nav_goal_handle) { + RCLCPP_ERROR(this->get_logger(), "Navigator rejected goal."); + result->success = false; + goal_handle->abort(result); + return; + } + + // 5. Monitor Execution Loop + auto result_future = navigator_client_->async_get_result(nav_goal_handle); + + while (rclcpp::ok()) { + // Check for cancel + if (goal_handle->is_canceling()) { + RCLCPP_INFO(this->get_logger(), "Canceling Navigator goal..."); + navigator_client_->async_cancel_goal(nav_goal_handle); + result->success = false; + goal_handle->canceled(result); + return; + } + + // Check if finished + if (result_future.wait_for(std::chrono::milliseconds(100)) == 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; + } + } + } + + // --- Helpers --- + + int get_closest_waypoint() { + sensor_msgs::msg::NavSatFix pos; + if (!quad_position_.get_position(pos)) return -1; + + auto wps = px4_mission_.get_all_waypoints(); + if (wps.empty()) return -1; + + int best_idx = -1; + double min_dist = INF; + + for (size_t i = 0; i < wps.size(); ++i) { + double d = get_distance_meters( + pos.latitude, pos.longitude, + wps[i].x_lat, wps[i].y_long + ); + if (d < min_dist) { + min_dist = d; + best_idx = static_cast(i); + } + } + return best_idx; + } + + // --- A* Search --- + struct AStarNode { + int id; + double f_score; + bool operator>(const AStarNode& other) const { return f_score > other.f_score; } + }; + + std::vector calculate_route(int start, int end) { + if (graph_.empty()) { + RCLCPP_WARN(this->get_logger(), "Graph is empty!"); + return {}; + } + + // Heuristic Lambda + WaypointPosition target_wp; + if (!px4_mission_.get_waypoint(end, target_wp)) return {}; + + auto heuristic = [&](int id) -> double { + WaypointPosition wp; + px4_mission_.get_waypoint(id, wp); + return get_distance_meters(wp.latitude, wp.longitude, target_wp.latitude, target_wp.longitude); + }; + + std::priority_queue, std::greater> open_set; + std::unordered_map g_score; + std::unordered_map came_from; + + g_score[start] = 0.0; + open_set.push({start, heuristic(start)}); + + while(!open_set.empty()) { + int current = open_set.top().id; + open_set.pop(); + + if (current == end) { + // Reconstruct + std::vector path; + while (current != start) { + path.push_back(current); + current = came_from[current]; + } + path.push_back(start); + std::reverse(path.begin(), path.end()); + return path; + } + + for (auto& edge : graph_[current]) { + int neighbor = edge.first; + double weight = edge.second; + double tentative_g = g_score[current] + weight; + + if (g_score.find(neighbor) == g_score.end() || tentative_g < g_score[neighbor]) { + came_from[neighbor] = current; + g_score[neighbor] = tentative_g; + double f = tentative_g + heuristic(neighbor); + open_set.push({neighbor, f}); + } + } + } + + return {}; // No path + } +}; + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + + // 1. Wait for Data (Throttle Logs) + rclcpp::Rate rate(1.0); + while (rclcpp::ok() && !node->has_data()) { + RCLCPP_INFO_THROTTLE(node->get_logger(), *node->get_clock(), 1000, + "Waiting for valid PX4 Mission and Geofence..."); + rclcpp::spin_some(node); + rate.sleep(); + } + + // 2. Build Graph once data is present + if (rclcpp::ok()) { + node->build_graph(); + } + + // 3. Spin with MultiThreadedExecutor (required for Action Server + Client) + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); + + rclcpp::shutdown(); + return 0; +} From a50219674197ca3e739172baa159b511f62c1aa0 Mon Sep 17 00:00:00 2001 From: JonD07 Date: Wed, 18 Feb 2026 14:48:49 -0500 Subject: [PATCH 2/3] CPP planner complete --- nimbus/include/nimbus/planner.h | 183 +++++ nimbus/src/pilot.cpp | 4 +- nimbus/src/planner.cpp | 1281 ++++++++++++++++--------------- 3 files changed, 842 insertions(+), 626 deletions(-) diff --git a/nimbus/include/nimbus/planner.h b/nimbus/include/nimbus/planner.h index 8ce0efc..d5f27a0 100644 --- a/nimbus/include/nimbus/planner.h +++ b/nimbus/include/nimbus/planner.h @@ -9,7 +9,190 @@ #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/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 index 7fbbb4b..5b9986b 100644 --- a/nimbus/src/planner.cpp +++ b/nimbus/src/planner.cpp @@ -1,68 +1,576 @@ #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; +} -#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; - -// Constants -#define DEBUG_PLANNER false -#define EARTH_RADIUS_M 6371000.0 -#define INF std::numeric_limits::infinity() - -// MAVLink Commands -#define MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION 5001 -#define MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION 5002 - -// ---------------------------------------------------------------------------- -// Helper Structs & Classes -// ---------------------------------------------------------------------------- - -struct WaypointPosition { - double latitude; - double longitude; - double relative_altitude; -}; +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; + } +} -struct Point2D { - double x; - double y; -}; -// --- Coordinate Math Helpers (Replaces UTM) --- +/*************************** + * + * 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 latlon_to_meters(double lat, double lon, double origin_lat, double origin_lon) { +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; @@ -77,7 +585,7 @@ Point2D latlon_to_meters(double lat, double lon, double origin_lat, double origi } // Haversine Distance -double get_distance_meters(double lat1, double lon1, double lat2, double lon2) { +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) + @@ -87,587 +595,112 @@ double get_distance_meters(double lat1, double lon1, double lat2, double lon2) { return EARTH_RADIUS_M * c; } -// --- Thread-Safe Data Classes --- - -class PX4Mission { -public: - PX4Mission() : has_mission_(false) {} - - void set_mission(const mavros_msgs::msg::WaypointList::SharedPtr msg) { - std::lock_guard lock(mutex_); - latest_mission_ = *msg; - has_mission_ = true; - } - - bool get_waypoint(size_t index, WaypointPosition& out_wp) { - std::lock_guard lock(mutex_); - if (!has_mission_ || index >= latest_mission_.waypoints.size()) { - return false; - } - const auto& wp = latest_mission_.waypoints[index]; - out_wp = {wp.x_lat, wp.y_long, wp.z_alt}; - return true; - } - - std::vector get_all_waypoints() { - std::lock_guard lock(mutex_); - if (!has_mission_) return {}; - return latest_mission_.waypoints; - } - - bool has_mission() { - std::lock_guard lock(mutex_); - return has_mission_; - } - - bool valid_waypoint(int index) { - std::lock_guard lock(mutex_); - if (!has_mission_) return false; - return (index >= 0 && index < (int)latest_mission_.waypoints.size()); - } - -private: - std::mutex mutex_; - mavros_msgs::msg::WaypointList latest_mission_; - bool has_mission_; -}; +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); + } +} -class PX4Geofence { -public: - PX4Geofence() : has_fence_(false) {} - - void set_fence(const mavros_msgs::msg::WaypointList::SharedPtr msg) { - std::lock_guard lock(mutex_); - latest_fence_ = *msg; - has_fence_ = true; - } - - std::vector get_fence_points() { - std::lock_guard lock(mutex_); - if (!has_fence_) return {}; - return latest_fence_.waypoints; - } - - bool has_fence() { - std::lock_guard lock(mutex_); - return has_fence_; - } - -private: - std::mutex mutex_; - mavros_msgs::msg::WaypointList latest_fence_; - bool has_fence_; +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); + } }; -class QuadPosition { -public: - QuadPosition() : has_position_(false) {} - - void set_position(const sensor_msgs::msg::NavSatFix::SharedPtr msg) { - std::lock_guard lock(mutex_); - latest_position_ = *msg; - has_position_ = true; - } - - bool get_position(sensor_msgs::msg::NavSatFix& out_pos) { - std::lock_guard lock(mutex_); - if (!has_position_) return false; - out_pos = latest_position_; - return true; - } - -private: - std::mutex mutex_; - sensor_msgs::msg::NavSatFix latest_position_; - bool has_position_; -}; +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; +} + -// ---------------------------------------------------------------------------- -// Nimbus Planner Node -// ---------------------------------------------------------------------------- - -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() : Node("nimbus_planner") { - - // Callback group for concurrent Action Server/Client execution - cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); - - // QoS - rclcpp::QoS best_effort(10); - best_effort.reliability(rclcpp::ReliabilityPolicy::BestEffort); - - // Subscribers - mission_sub_ = this->create_subscription( - "/mavros/mission/waypoints", best_effort, - std::bind(&NimbusPlanner::mission_callback, this, std::placeholders::_1)); - - geofence_sub_ = this->create_subscription( - "/mavros/geofence/fences", best_effort, - std::bind(&NimbusPlanner::geofence_callback, this, std::placeholders::_1)); - - position_sub_ = this->create_subscription( - "/mavros/global_position/global", best_effort, - std::bind(&NimbusPlanner::position_callback, this, std::placeholders::_1)); - - // Action Server (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_ - ); - - // Action Client (Client) - navigator_client_ = rclcpp_action::create_client( - this, "/nimbus/navigator/set_route", cb_group_); - - RCLCPP_INFO(this->get_logger(), "Nimbus Planner initialized."); - } - - // --- Public Interface for Main Loop --- - bool has_data() { - return px4_mission_.has_mission() && px4_geofence_.has_fence(); - } - - void build_graph() { - RCLCPP_INFO(this->get_logger(), "Building waypoint graph..."); - - auto mission_wps = px4_mission_.get_all_waypoints(); - auto fence_wps = px4_geofence_.get_fence_points(); - - if (mission_wps.empty()) { - RCLCPP_WARN(this->get_logger(), "No mission waypoints to build graph."); - return; - } - - // 1. Establish Coordinate System (Origin = Waypoint 0) - double origin_lat = mission_wps[0].x_lat; - double origin_lon = mission_wps[0].y_long; - - // 2. 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)); - } - - // 3. Parse Geofences & 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; - } - - // Parse Fences - struct Poly { - std::vector points; - int type; // 0 = exclusion, 1 = inclusion - }; - std::vector polygons; - std::vector current_poly_pts; - int current_type = 0; // default exclusion - - for (const auto& wp : fence_wps) { - 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; - - if (wp.command == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION) { - if (current_type == 1 && !current_poly_pts.empty()) { - polygons.push_back({current_poly_pts, 1}); - current_poly_pts.clear(); - } - current_type = 0; - current_poly_pts.push_back(p); - } else if (wp.command == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION) { - if (current_type == 0 && !current_poly_pts.empty()) { - polygons.push_back({current_poly_pts, 0}); - current_poly_pts.clear(); - } - current_type = 1; - current_poly_pts.push_back(p); - } else { - current_poly_pts.push_back(p); - } - } - if (!current_poly_pts.empty()) { - polygons.push_back({current_poly_pts, current_type}); - } - - // 4. 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); - - for (const auto& poly : polygons) { - if (poly.type == 0) { // Exclusion - 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)); - } - } - - // Dilation - 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); - } - - // 5. Build 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); - } - -private: - // Internal Data - PX4Mission px4_mission_; - PX4Geofence px4_geofence_; - QuadPosition quad_position_; - - rclcpp::CallbackGroup::SharedPtr cb_group_; - - rclcpp::Subscription::SharedPtr mission_sub_; - rclcpp::Subscription::SharedPtr geofence_sub_; - rclcpp::Subscription::SharedPtr position_sub_; - - rclcpp_action::Server::SharedPtr action_server_; - rclcpp_action::Client::SharedPtr navigator_client_; - - // 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; - - // --- Callbacks --- - void 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 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 position_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg) { - quad_position_.set_position(msg); - } - - // --- Action Server Logic --- - rclcpp_action::GoalResponse 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 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 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(); - } - - void execute(const std::shared_ptr goal_handle) { - RCLCPP_INFO(this->get_logger(), "Executing WaypointMove..."); - - auto result = std::make_shared(); - auto feedback = std::make_shared(); - const auto goal = goal_handle->get_goal(); - - // 1. Check Navigator Availability - 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; - } - - // 2. Validate Target - 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; - } - - // 3. Plan Route - int start_idx = get_closest_waypoint(); - if (start_idx == -1) { - // Fallback if GPS not ready - RCLCPP_WARN(this->get_logger(), "Could not determine start index (no GPS?). Defaulting to 0."); - start_idx = 0; - } - - std::vector route = calculate_route(start_idx, goal->waypoint); - - if (route.empty()) { - RCLCPP_WARN(this->get_logger(), "No route found to waypoint %d", goal->waypoint); - result->success = false; - goal_handle->abort(result); - return; - } - - // 4. Send to Navigator - 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 size %zu to Navigator.", route.size()); - - 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 acceptance - if (future_goal.wait_for(std::chrono::seconds(2)) != std::future_status::ready) { - RCLCPP_ERROR(this->get_logger(), "Navigator timed out accepting goal."); - result->success = false; - goal_handle->abort(result); - return; - } - - auto nav_goal_handle = future_goal.get(); - if (!nav_goal_handle) { - RCLCPP_ERROR(this->get_logger(), "Navigator rejected goal."); - result->success = false; - goal_handle->abort(result); - return; - } - - // 5. Monitor Execution Loop - auto result_future = navigator_client_->async_get_result(nav_goal_handle); - - while (rclcpp::ok()) { - // Check for cancel - if (goal_handle->is_canceling()) { - RCLCPP_INFO(this->get_logger(), "Canceling Navigator goal..."); - navigator_client_->async_cancel_goal(nav_goal_handle); - result->success = false; - goal_handle->canceled(result); - return; - } - - // Check if finished - if (result_future.wait_for(std::chrono::milliseconds(100)) == 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; - } - } - } - - // --- Helpers --- - - int get_closest_waypoint() { - sensor_msgs::msg::NavSatFix pos; - if (!quad_position_.get_position(pos)) return -1; - - auto wps = px4_mission_.get_all_waypoints(); - if (wps.empty()) return -1; - - int best_idx = -1; - double min_dist = INF; - - for (size_t i = 0; i < wps.size(); ++i) { - double d = get_distance_meters( - pos.latitude, pos.longitude, - wps[i].x_lat, wps[i].y_long - ); - if (d < min_dist) { - min_dist = d; - best_idx = static_cast(i); - } - } - return best_idx; - } - - // --- A* Search --- - struct AStarNode { - int id; - double f_score; - bool operator>(const AStarNode& other) const { return f_score > other.f_score; } - }; - - std::vector calculate_route(int start, int end) { - if (graph_.empty()) { - RCLCPP_WARN(this->get_logger(), "Graph is empty!"); - return {}; - } - - // Heuristic Lambda - WaypointPosition target_wp; - if (!px4_mission_.get_waypoint(end, target_wp)) return {}; - - auto heuristic = [&](int id) -> double { - WaypointPosition wp; - px4_mission_.get_waypoint(id, wp); - return get_distance_meters(wp.latitude, wp.longitude, target_wp.latitude, target_wp.longitude); - }; - - std::priority_queue, std::greater> open_set; - std::unordered_map g_score; - std::unordered_map came_from; - - g_score[start] = 0.0; - open_set.push({start, heuristic(start)}); - - while(!open_set.empty()) { - int current = open_set.top().id; - open_set.pop(); - - if (current == end) { - // Reconstruct - std::vector path; - while (current != start) { - path.push_back(current); - current = came_from[current]; - } - path.push_back(start); - std::reverse(path.begin(), path.end()); - return path; - } - - for (auto& edge : graph_[current]) { - int neighbor = edge.first; - double weight = edge.second; - double tentative_g = g_score[current] + weight; - - if (g_score.find(neighbor) == g_score.end() || tentative_g < g_score[neighbor]) { - came_from[neighbor] = current; - g_score[neighbor] = tentative_g; - double f = tentative_g + heuristic(neighbor); - open_set.push({neighbor, f}); - } - } - } - - return {}; // No path - } -}; int main(int argc, char **argv) { + // Create node rclcpp::init(argc, argv); auto node = std::make_shared(); - // 1. Wait for Data (Throttle Logs) - rclcpp::Rate rate(1.0); - while (rclcpp::ok() && !node->has_data()) { - RCLCPP_INFO_THROTTLE(node->get_logger(), *node->get_clock(), 1000, - "Waiting for valid PX4 Mission and Geofence..."); - rclcpp::spin_some(node); - rate.sleep(); - } - - // 2. Build Graph once data is present - if (rclcpp::ok()) { - node->build_graph(); - } - - // 3. Spin with MultiThreadedExecutor (required for Action Server + Client) + // Spin with MultiThreadedExecutor rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(node); executor.spin(); From 9609f44fdf36060a2bf4b97193c8a6433fcf0743 Mon Sep 17 00:00:00 2001 From: JonD07 Date: Wed, 18 Feb 2026 15:45:29 -0500 Subject: [PATCH 3/3] CPP navigator --- nimbus/CMakeLists.txt | 13 ++ nimbus/include/nimbus/navigator.h | 83 +++++++++ nimbus/launch/nimbus.launch | 2 +- nimbus/src/navigator.cpp | 277 ++++++++++++++++++++++++++++++ 4 files changed, 374 insertions(+), 1 deletion(-) create mode 100644 nimbus/include/nimbus/navigator.h create mode 100644 nimbus/src/navigator.cpp diff --git a/nimbus/CMakeLists.txt b/nimbus/CMakeLists.txt index 64ca7f6..1753382 100644 --- a/nimbus/CMakeLists.txt +++ b/nimbus/CMakeLists.txt @@ -47,6 +47,19 @@ 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 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/launch/nimbus.launch b/nimbus/launch/nimbus.launch index 668d8f8..aab4bb1 100644 --- a/nimbus/launch/nimbus.launch +++ b/nimbus/launch/nimbus.launch @@ -23,7 +23,7 @@ - + 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; +}