From 021e488dda8b2044bb4aba02d1f06459ed3edf13 Mon Sep 17 00:00:00 2001 From: abubakaraliyubadawi Date: Sun, 6 Apr 2025 10:45:36 +0200 Subject: [PATCH 1/8] Added waypoint manager node --- mission/waypoint_manager/CMakeLists.txt | 56 +++ .../config/waypoint_manager_params.yaml | 7 + .../waypoint_manager/waypoint_manager.hpp | 79 ++++ .../launch/waypoint_manager.launch.py | 24 ++ mission/waypoint_manager/package.xml | 24 ++ .../waypoint_manager/src/waypoint_manager.cpp | 357 ++++++++++++++++++ .../src/waypoint_manager_node.cpp | 14 + 7 files changed, 561 insertions(+) create mode 100644 mission/waypoint_manager/CMakeLists.txt create mode 100644 mission/waypoint_manager/config/waypoint_manager_params.yaml create mode 100644 mission/waypoint_manager/include/waypoint_manager/waypoint_manager.hpp create mode 100644 mission/waypoint_manager/launch/waypoint_manager.launch.py create mode 100644 mission/waypoint_manager/package.xml create mode 100644 mission/waypoint_manager/src/waypoint_manager.cpp create mode 100644 mission/waypoint_manager/src/waypoint_manager_node.cpp diff --git a/mission/waypoint_manager/CMakeLists.txt b/mission/waypoint_manager/CMakeLists.txt new file mode 100644 index 00000000..611f374d --- /dev/null +++ b/mission/waypoint_manager/CMakeLists.txt @@ -0,0 +1,56 @@ +cmake_minimum_required(VERSION 3.8) +project(waypoint_manager) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(Eigen3 REQUIRED) + +include_directories(include) + +add_executable(waypoint_manager_node + src/waypoint_manager_node.cpp + src/waypoint_manager.cpp +) + +ament_target_dependencies(waypoint_manager_node + rclcpp + rclcpp_action + geometry_msgs + nav_msgs + vortex_msgs + tf2 + tf2_geometry_msgs + Eigen3 +) + +install(TARGETS + waypoint_manager_node + DESTINATION lib/${PROJECT_NAME} +) + +install( + DIRECTORY include/ + DESTINATION include +) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME}/ +) + +ament_package() diff --git a/mission/waypoint_manager/config/waypoint_manager_params.yaml b/mission/waypoint_manager/config/waypoint_manager_params.yaml new file mode 100644 index 00000000..6dbbeac9 --- /dev/null +++ b/mission/waypoint_manager/config/waypoint_manager_params.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + action_servers: + waypoint_manager: "waypoint_manager" + reference_filter: "reference_filter" + topics: + pose: "pose" diff --git a/mission/waypoint_manager/include/waypoint_manager/waypoint_manager.hpp b/mission/waypoint_manager/include/waypoint_manager/waypoint_manager.hpp new file mode 100644 index 00000000..a2af20bc --- /dev/null +++ b/mission/waypoint_manager/include/waypoint_manager/waypoint_manager.hpp @@ -0,0 +1,79 @@ +#ifndef WAYPOINT_MANAGER_HPP +#define WAYPOINT_MANAGER_HPP + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class WaypointManagerNode : public rclcpp::Node { + public: + explicit WaypointManagerNode(); + + private: + using WaypointManagerAction = vortex_msgs::action::WaypointManager; + using GoalHandleWaypointManager = + rclcpp_action::ServerGoalHandle; + + using ReferenceFilterAction = vortex_msgs::action::ReferenceFilterWaypoint; + + rclcpp_action::Server::SharedPtr action_server_; + + rclcpp_action::Client::SharedPtr + reference_filter_client_; + + rclcpp::Subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; + + geometry_msgs::msg::Pose current_pose_; + + std::mutex mutex_; + + rclcpp::CallbackGroup::SharedPtr server_cb_group_; + + rclcpp::CallbackGroup::SharedPtr client_cb_group_; + + std::shared_ptr current_goal_handle_; + std::vector waypoints_; + size_t current_waypoint_index_{0}; + double switching_threshold_{0.5}; + std::string target_server_{"reference_filter"}; + bool navigation_active_{false}; + std::shared_ptr> current_waypoint_promise_; + + rclcpp_action::GoalUUID preempted_goal_id_; + + void setup_action_server(); + void setup_action_clients(); + void setup_subscribers(); + + 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); + + void execute_waypoint_navigation( + const std::shared_ptr goal_handle); + + void pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + + double calculate_distance(const geometry_msgs::msg::Pose& pose1, + const geometry_msgs::msg::Pose& pose2); + + void send_waypoint_to_target_server( + const geometry_msgs::msg::PoseStamped& waypoint); +}; + +#endif diff --git a/mission/waypoint_manager/launch/waypoint_manager.launch.py b/mission/waypoint_manager/launch/waypoint_manager.launch.py new file mode 100644 index 00000000..8dc71a49 --- /dev/null +++ b/mission/waypoint_manager/launch/waypoint_manager.launch.py @@ -0,0 +1,24 @@ +from os import path + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + config_file = path.join( + get_package_share_directory('waypoint_manager'), + 'config', + 'waypoint_manager_params.yaml', + ) + + waypoint_manager_node = Node( + package='waypoint_manager', + executable='waypoint_manager_node', + name='waypoint_manager_node', + namespace='orca', + parameters=[config_file], + output='screen', + ) + + return LaunchDescription([waypoint_manager_node]) diff --git a/mission/waypoint_manager/package.xml b/mission/waypoint_manager/package.xml new file mode 100644 index 00000000..37f46b50 --- /dev/null +++ b/mission/waypoint_manager/package.xml @@ -0,0 +1,24 @@ + + + + waypoint_manager + 0.0.0 + Waypoint Manager for Orca task execution + your_name + MIT + + ament_cmake + + rclcpp + rclcpp_action + geometry_msgs + nav_msgs + vortex_msgs + tf2 + tf2_geometry_msgs + eigen + + + ament_cmake + + diff --git a/mission/waypoint_manager/src/waypoint_manager.cpp b/mission/waypoint_manager/src/waypoint_manager.cpp new file mode 100644 index 00000000..478332c7 --- /dev/null +++ b/mission/waypoint_manager/src/waypoint_manager.cpp @@ -0,0 +1,357 @@ +#include "waypoint_manager/waypoint_manager.hpp" +#include +#include +#include +#include + +using namespace std::placeholders; + +WaypointManagerNode::WaypointManagerNode() : Node("waypoint_manager_node") { + // Create callback groups for server and client + server_cb_group_ = + this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + client_cb_group_ = + this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + + // Setup action server, clients and subscribers + setup_action_server(); + setup_action_clients(); + setup_subscribers(); + + RCLCPP_INFO(this->get_logger(), "Waypoint Manager Node initialized"); +} + +void WaypointManagerNode::setup_action_server() { + // Declare parameters for action server name + this->declare_parameter("action_servers.waypoint_manager", + "waypoint_manager"); + std::string action_server_name = + this->get_parameter("action_servers.waypoint_manager").as_string(); + + // Create the action server + action_server_ = rclcpp_action::create_server( + this, action_server_name, + std::bind(&WaypointManagerNode::handle_goal, this, _1, _2), + std::bind(&WaypointManagerNode::handle_cancel, this, _1), + std::bind(&WaypointManagerNode::handle_accepted, this, _1), + rcl_action_server_get_default_options(), server_cb_group_); + + RCLCPP_INFO(this->get_logger(), "Action server '%s' started", + action_server_name.c_str()); +} + +void WaypointManagerNode::setup_action_clients() { + // Declare parameters for client action server names + this->declare_parameter("action_servers.reference_filter", + "reference_filter"); + std::string reference_filter_server = + this->get_parameter("action_servers.reference_filter").as_string(); + + // Create action clients + reference_filter_client_ = + rclcpp_action::create_client( + this, reference_filter_server, client_cb_group_); + + RCLCPP_INFO(this->get_logger(), "Action client created for '%s'", + reference_filter_server.c_str()); +} + +void WaypointManagerNode::setup_subscribers() { + // Declare parameters for subscription topics + this->declare_parameter("topics.pose", "pose"); + std::string pose_topic = this->get_parameter("topics.pose").as_string(); + + // Quality of service profile + rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; + auto qos_sensor_data = rclcpp::QoS( + rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); + + // Create pose subscription + pose_sub_ = this->create_subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>( + pose_topic, qos_sensor_data, + std::bind(&WaypointManagerNode::pose_callback, this, _1)); + + RCLCPP_INFO(this->get_logger(), "Subscribed to pose topic: %s", + pose_topic.c_str()); +} + +rclcpp_action::GoalResponse WaypointManagerNode::handle_goal( + const rclcpp_action::GoalUUID& uuid, + std::shared_ptr goal) { + (void)uuid; // Unused + + // Check if goal is valid + if (goal->waypoints.empty()) { + RCLCPP_ERROR(this->get_logger(), "Received empty waypoint list"); + return rclcpp_action::GoalResponse::REJECT; + } + + // Check if target server is valid - only allow reference_filter + if (goal->target_server != "reference_filter") { + RCLCPP_ERROR( + this->get_logger(), + "Invalid target server: %s. Only 'reference_filter' is supported", + goal->target_server.c_str()); + return rclcpp_action::GoalResponse::REJECT; + } + + // If there's an active goal, preempt it + { + std::lock_guard lock(mutex_); + if (current_goal_handle_ && current_goal_handle_->is_active()) { + RCLCPP_INFO(this->get_logger(), + "Preempting current goal for new goal"); + preempted_goal_id_ = current_goal_handle_->get_goal_id(); + } + } + + RCLCPP_INFO(this->get_logger(), "Accepting new goal with %zu waypoints", + goal->waypoints.size()); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse WaypointManagerNode::handle_cancel( + const std::shared_ptr goal_handle) { + (void)goal_handle; // Unused + RCLCPP_INFO(this->get_logger(), "Received cancel request"); + return rclcpp_action::CancelResponse::ACCEPT; +} + +void WaypointManagerNode::handle_accepted( + const std::shared_ptr goal_handle) { + // Start a new thread to process the goal + std::thread{ + std::bind(&WaypointManagerNode::execute_waypoint_navigation, this, _1), + goal_handle} + .detach(); +} + +void WaypointManagerNode::execute_waypoint_navigation( + const std::shared_ptr goal_handle) { + // Set current goal handle and extract goal information + { + std::lock_guard lock(mutex_); + current_goal_handle_ = goal_handle; + navigation_active_ = true; + + auto goal = goal_handle->get_goal(); + waypoints_ = goal->waypoints; + target_server_ = goal->target_server; + switching_threshold_ = goal->switching_threshold; + current_waypoint_index_ = 0; + } + + RCLCPP_INFO(this->get_logger(), + "Starting waypoint navigation with %zu waypoints", + waypoints_.size()); + + // Create result and feedback objects + auto result = std::make_shared(); + auto feedback = std::make_shared(); + + // Main execution loop + while (rclcpp::ok()) { + // Check if goal was canceled or preempted + { + std::lock_guard lock(mutex_); + + if (goal_handle->get_goal_id() == preempted_goal_id_) { + RCLCPP_INFO(this->get_logger(), "Goal was preempted"); + result->success = false; + result->completed_waypoints = current_waypoint_index_; + goal_handle->abort(result); + navigation_active_ = false; + return; + } + + if (goal_handle->is_canceling()) { + RCLCPP_INFO(this->get_logger(), "Goal was canceled"); + result->success = false; + result->completed_waypoints = current_waypoint_index_; + goal_handle->canceled(result); + navigation_active_ = false; + return; + } + } + + // Check if we've completed all waypoints + if (current_waypoint_index_ >= waypoints_.size()) { + RCLCPP_INFO(this->get_logger(), "All waypoints completed"); + result->success = true; + result->completed_waypoints = waypoints_.size(); + goal_handle->succeed(result); + + std::lock_guard lock(mutex_); + navigation_active_ = false; + return; + } + + // Send current waypoint to target server (if not already navigating) + { + std::lock_guard lock(mutex_); + if (!current_waypoint_promise_) { + RCLCPP_INFO(this->get_logger(), + "Sending waypoint %zu to target server", + current_waypoint_index_); + send_waypoint_to_target_server( + waypoints_[current_waypoint_index_]); + } + } + + // Calculate distance to current waypoint and check if we've reached it + geometry_msgs::msg::Pose target_pose = + waypoints_[current_waypoint_index_].pose; + geometry_msgs::msg::Pose current_pose; + { + std::lock_guard lock(mutex_); + current_pose = current_pose_; + } + + double distance = calculate_distance(current_pose, target_pose); + + // Update feedback + { + std::lock_guard lock(mutex_); + feedback->current_pose = current_pose; + feedback->current_waypoint_index = current_waypoint_index_; + feedback->distance_to_waypoint = distance; + goal_handle->publish_feedback(feedback); + } + + // Check if we've reached the waypoint + if (distance < switching_threshold_) { + RCLCPP_INFO(this->get_logger(), + "Reached waypoint %zu, distance: %f", + current_waypoint_index_, distance); + + // If we have a promise, complete it (to cancel any pending target + // server action) + { + std::lock_guard lock(mutex_); + if (current_waypoint_promise_) { + current_waypoint_promise_->set_value(true); + current_waypoint_promise_.reset(); + } + } + + // Move to next waypoint + current_waypoint_index_++; + } + + // Sleep to avoid busy waiting + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } +} + +void WaypointManagerNode::pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { + std::lock_guard lock(mutex_); + current_pose_ = msg->pose.pose; +} + +double WaypointManagerNode::calculate_distance( + const geometry_msgs::msg::Pose& pose1, + const geometry_msgs::msg::Pose& pose2) { + // Calculate Euclidean distance between two poses (position only) + double dx = pose1.position.x - pose2.position.x; + double dy = pose1.position.y - pose2.position.y; + double dz = pose1.position.z - pose2.position.z; + + return std::sqrt(dx * dx + dy * dy + dz * dz); +} + +void WaypointManagerNode::send_waypoint_to_target_server( + const geometry_msgs::msg::PoseStamped& waypoint) { + // Create a new promise for this waypoint + current_waypoint_promise_ = std::make_shared>(); + + // Check if reference filter client is ready + if (!reference_filter_client_->wait_for_action_server( + std::chrono::seconds(1))) { + RCLCPP_ERROR(this->get_logger(), + "Reference filter action server not available"); + current_waypoint_promise_->set_value(false); + current_waypoint_promise_.reset(); + return; + } + + // Create goal + auto goal_msg = ReferenceFilterAction::Goal(); + goal_msg.goal = waypoint; + + // Send goal with inline lambdas that have the correct signatures + auto send_goal_options = + rclcpp_action::Client::SendGoalOptions(); + + send_goal_options.goal_response_callback = + [this](const rclcpp_action::ClientGoalHandle< + ReferenceFilterAction>::SharedPtr& goal_handle) { + if (!goal_handle) { + RCLCPP_ERROR(this->get_logger(), + "Reference filter goal was rejected"); + std::lock_guard lock(mutex_); + if (current_waypoint_promise_) { + current_waypoint_promise_->set_value(false); + current_waypoint_promise_.reset(); + } + } else { + RCLCPP_INFO(this->get_logger(), + "Reference filter goal accepted"); + } + }; + + send_goal_options.feedback_callback = + [this](rclcpp_action::ClientGoalHandle::SharedPtr + goal_handle, + const std::shared_ptr + feedback) { + (void)goal_handle; // Unused + (void)feedback; // Unused, we're not using the feedback from the + // reference filter + }; + + send_goal_options.result_callback = + [this](const rclcpp_action::ClientGoalHandle< + ReferenceFilterAction>::WrappedResult& result) { + std::lock_guard lock(mutex_); + + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + RCLCPP_INFO(this->get_logger(), + "Reference filter goal succeeded"); + if (current_waypoint_promise_) { + current_waypoint_promise_->set_value(true); + current_waypoint_promise_.reset(); + } + break; + case rclcpp_action::ResultCode::ABORTED: + RCLCPP_ERROR(this->get_logger(), + "Reference filter goal was aborted"); + if (current_waypoint_promise_) { + current_waypoint_promise_->set_value(false); + current_waypoint_promise_.reset(); + } + break; + case rclcpp_action::ResultCode::CANCELED: + RCLCPP_INFO(this->get_logger(), + "Reference filter goal was canceled"); + if (current_waypoint_promise_) { + current_waypoint_promise_->set_value(false); + current_waypoint_promise_.reset(); + } + break; + default: + RCLCPP_ERROR( + this->get_logger(), + "Unknown result code from reference filter action"); + if (current_waypoint_promise_) { + current_waypoint_promise_->set_value(false); + current_waypoint_promise_.reset(); + } + break; + } + }; + + reference_filter_client_->async_send_goal(goal_msg, send_goal_options); +} diff --git a/mission/waypoint_manager/src/waypoint_manager_node.cpp b/mission/waypoint_manager/src/waypoint_manager_node.cpp new file mode 100644 index 00000000..56fc046b --- /dev/null +++ b/mission/waypoint_manager/src/waypoint_manager_node.cpp @@ -0,0 +1,14 @@ +#include +#include "waypoint_manager/waypoint_manager.hpp" + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + RCLCPP_INFO(rclcpp::get_logger("waypoint_manager"), + "Starting Waypoint Manager node"); + auto node = std::make_shared(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); + rclcpp::shutdown(); + return 0; +} From 2f164eb00206b474be6240fe207a21a2bf38a2be Mon Sep 17 00:00:00 2001 From: abubakaraliyubadawi Date: Sun, 6 Apr 2025 15:07:53 +0200 Subject: [PATCH 2/8] Modified the logger to use spdlog for logging --- .../config/reference_filter_params.yaml | 10 +- mission/waypoint_manager/CMakeLists.txt | 4 + mission/waypoint_manager/README.md | 136 ++++++++++++++++++ .../config/waypoint_manager_params.yaml | 12 +- .../waypoint_manager/waypoint_manager.hpp | 11 +- .../launch/waypoint_manager.launch.py | 42 +++++- mission/waypoint_manager/package.xml | 2 + .../waypoint_manager/src/waypoint_manager.cpp | 102 ++++--------- .../src/waypoint_manager_node.cpp | 12 +- 9 files changed, 231 insertions(+), 100 deletions(-) create mode 100644 mission/waypoint_manager/README.md diff --git a/guidance/reference_filter_dp/config/reference_filter_params.yaml b/guidance/reference_filter_dp/config/reference_filter_params.yaml index a9cf24aa..a396eabb 100644 --- a/guidance/reference_filter_dp/config/reference_filter_params.yaml +++ b/guidance/reference_filter_dp/config/reference_filter_params.yaml @@ -1,4 +1,12 @@ /**: ros__parameters: - zeta: [1., 1., 1., 1., 1., 1.] + zeta: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] omega: [0.2, 0.2, 0.2, 0.2, 0.2, 0.2] + topics: + pose: "/orca/pose" + twist: "/orca/twist" + guidance: + dp: "/orca/guidance/dp" + aruco_board_pose_camera: "/orca/aruco_board_pose_camera" + action_servers: + reference_filter: "reference_filter" diff --git a/mission/waypoint_manager/CMakeLists.txt b/mission/waypoint_manager/CMakeLists.txt index 611f374d..854a0a60 100644 --- a/mission/waypoint_manager/CMakeLists.txt +++ b/mission/waypoint_manager/CMakeLists.txt @@ -18,6 +18,8 @@ find_package(vortex_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(Eigen3 REQUIRED) +find_package(spdlog REQUIRED) +find_package(fmt REQUIRED) include_directories(include) @@ -35,7 +37,9 @@ ament_target_dependencies(waypoint_manager_node tf2 tf2_geometry_msgs Eigen3 + spdlog ) +target_link_libraries(waypoint_manager_node fmt::fmt) install(TARGETS waypoint_manager_node diff --git a/mission/waypoint_manager/README.md b/mission/waypoint_manager/README.md new file mode 100644 index 00000000..c7fcd236 --- /dev/null +++ b/mission/waypoint_manager/README.md @@ -0,0 +1,136 @@ +# Waypoint Manager + +The Waypoint Manager is a ROS 2 node designed for Orca (our autonomous underwater vehicle) that provides high-level waypoint navigation capabilities by coordinating with the Reference Filter for smooth trajectory execution. + +## Overview + +The Waypoint Manager acts as a coordinator between high-level mission planning and low-level trajectory generation. It manages sequences of waypoints and delegates the detailed trajectory generation to the Reference Filter, creating a layered navigation architecture for Orca. + +## System Architecture + +### Waypoint Manager Node + +The Waypoint Manager handles higher-level navigation logic: +- Receives waypoint sequences via an action server +- Manages waypoint-to-waypoint transitions based on proximity +- Determines when waypoints are reached using configurable thresholds +- Handles preemption of current navigation goals for new requests +- Provides feedback on Orca's navigation progress + +### Integration with Reference Filter + +The Reference Filter provides smoother trajectories via a third-order model that generates reference values for position, velocity, and acceleration. The integration works as follows: + +1. **Client-Server Model**: The Waypoint Manager is a client of the Reference Filter's action server +2. **Single Waypoint Delegation**: The Waypoint Manager sends one waypoint at a time to the Reference Filter +3. **Distance-Based Switching**: When Orca is within the switching threshold of the current waypoint, the Waypoint Manager advances to the next waypoint +4. **Asynchronous Communication**: Uses promises and futures to handle asynchronous action server interactions + +## How It Works + +The waypoint navigation process follows these steps: + +1. **Waypoint Sequence Reception**: The Waypoint Manager receives a sequence of waypoints through its action server +2. **First Waypoint Dispatch**: It sends the first waypoint to the Reference Filter's action server +3. **Position Monitoring**: Continuously monitors Orca's position +4. **Threshold Checking**: Compares the distance to the current waypoint against the switching threshold +5. **Waypoint Advancement**: When within threshold, it: + - Cancels the current Reference Filter goal (if needed) + - Advances to the next waypoint + - Sends the new waypoint to the Reference Filter +6. **Completion Notification**: When all waypoints are reached, the action is marked as succeeded + +## Custom Action Types + +The Waypoint Manager uses a custom action definition: + +### WaypointManager.action + +``` +# Goal +geometry_msgs/PoseStamped[] waypoints # Array of waypoints for Orca to visit +string target_server # Name of the target action server +float64 switching_threshold # Distance threshold for switching waypoints +--- +# Result +bool success +uint32 completed_waypoints # Number of waypoints completed +--- +# Feedback +geometry_msgs/Pose current_pose # Current pose of Orca +uint32 current_waypoint_index # Index of the current waypoint +float64 distance_to_waypoint # Distance to the current waypoint +``` + +This action allows clients to: +- Send multiple waypoints in a single request +- Specify which server should handle the trajectory generation (currently only supporting "reference_filter") +- Set a custom threshold for when to consider a waypoint reached +- Receive feedback on progress including the current position, active waypoint, and distance remaining + +The Waypoint Manager in turn uses the Reference Filter's action server: + +### ReferenceFilterWaypoint.action + +``` +# Goal +geometry_msgs/PoseStamped goal +--- +# Result +bool success +vortex_msgs/ReferenceFilter result +--- +# Feedback +vortex_msgs/ReferenceFilter feedback +``` + +## Implementation Details + +### Multi-threading Approach + +The Waypoint Manager uses a multi-threaded architecture to handle: +- Main thread for ROS 2 callbacks +- Separate execution thread for waypoint navigation +- Thread-safe communication using mutexes and promises/futures + +### Error Handling + +The system includes robust error handling: +- Detection of Reference Filter action server unavailability +- Goal rejection handling +- Preemption of current goals +- Clean cancellation of navigation sequences + +## Interfaces + +### Action Servers + +- **`/orca/waypoint_manager`**: Accepts navigation requests with lists of waypoints + +### Action Clients + +- **`/orca/reference_filter`**: Sends individual waypoints to the Reference Filter node + +### Subscriptions + +- **`/orca/pose`**: Orca's pose feedback (PoseWithCovarianceStamped) + +## Parameters + +| Parameter | Description | Default | +|-----------|-------------|---------| +| `topics.pose` | Topic for Orca's pose | `/orca/pose` | +| `action_servers.waypoint_manager` | Name of the waypoint manager action server | `waypoint_manager` | +| `action_servers.reference_filter` | Name of the reference filter action server | `reference_filter` | + + +## Dependencies + +- ROS 2 +- rclcpp +- rclcpp_action +- geometry_msgs +- vortex_msgs +- tf2, tf2_geometry_msgs +- spdlog (for logging) +- fmt (for string formatting) diff --git a/mission/waypoint_manager/config/waypoint_manager_params.yaml b/mission/waypoint_manager/config/waypoint_manager_params.yaml index 6dbbeac9..601b7973 100644 --- a/mission/waypoint_manager/config/waypoint_manager_params.yaml +++ b/mission/waypoint_manager/config/waypoint_manager_params.yaml @@ -1,7 +1,7 @@ /**: - ros__parameters: - action_servers: - waypoint_manager: "waypoint_manager" - reference_filter: "reference_filter" - topics: - pose: "pose" + ros__parameters: + topics: + pose: "/orca/pose" + action_servers: + waypoint_manager: "waypoint_manager" + reference_filter: "reference_filter" diff --git a/mission/waypoint_manager/include/waypoint_manager/waypoint_manager.hpp b/mission/waypoint_manager/include/waypoint_manager/waypoint_manager.hpp index a2af20bc..302340b1 100644 --- a/mission/waypoint_manager/include/waypoint_manager/waypoint_manager.hpp +++ b/mission/waypoint_manager/include/waypoint_manager/waypoint_manager.hpp @@ -1,6 +1,6 @@ #ifndef WAYPOINT_MANAGER_HPP #define WAYPOINT_MANAGER_HPP - +#include #include #include #include @@ -20,25 +20,18 @@ class WaypointManagerNode : public rclcpp::Node { using WaypointManagerAction = vortex_msgs::action::WaypointManager; using GoalHandleWaypointManager = rclcpp_action::ServerGoalHandle; - using ReferenceFilterAction = vortex_msgs::action::ReferenceFilterWaypoint; rclcpp_action::Server::SharedPtr action_server_; - rclcpp_action::Client::SharedPtr reference_filter_client_; - rclcpp::Subscription< geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; geometry_msgs::msg::Pose current_pose_; - std::mutex mutex_; - rclcpp::CallbackGroup::SharedPtr server_cb_group_; - rclcpp::CallbackGroup::SharedPtr client_cb_group_; - std::shared_ptr current_goal_handle_; std::vector waypoints_; size_t current_waypoint_index_{0}; @@ -46,7 +39,6 @@ class WaypointManagerNode : public rclcpp::Node { std::string target_server_{"reference_filter"}; bool navigation_active_{false}; std::shared_ptr> current_waypoint_promise_; - rclcpp_action::GoalUUID preempted_goal_id_; void setup_action_server(); @@ -75,5 +67,4 @@ class WaypointManagerNode : public rclcpp::Node { void send_waypoint_to_target_server( const geometry_msgs::msg::PoseStamped& waypoint); }; - #endif diff --git a/mission/waypoint_manager/launch/waypoint_manager.launch.py b/mission/waypoint_manager/launch/waypoint_manager.launch.py index 8dc71a49..8088cda0 100644 --- a/mission/waypoint_manager/launch/waypoint_manager.launch.py +++ b/mission/waypoint_manager/launch/waypoint_manager.launch.py @@ -1,15 +1,33 @@ -from os import path +import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node def generate_launch_description(): - config_file = path.join( - get_package_share_directory('waypoint_manager'), - 'config', - 'waypoint_manager_params.yaml', + waypoint_manager_pkg_dir = get_package_share_directory('waypoint_manager') + reference_filter_pkg_dir = get_package_share_directory('reference_filter_dp') + + reference_filter_config = os.path.join( + reference_filter_pkg_dir, 'config', 'reference_filter_params.yaml' + ) + + waypoint_manager_config = os.path.join( + waypoint_manager_pkg_dir, 'config', 'waypoint_manager_params.yaml' + ) + + use_sim_time = LaunchConfiguration('use_sim_time', default='false') + + reference_filter_node = Node( + package='reference_filter_dp', + executable='reference_filter_dp_node', + name='reference_filter_node', + namespace='orca', + parameters=[reference_filter_config, {'use_sim_time': use_sim_time}], + output='screen', ) waypoint_manager_node = Node( @@ -17,8 +35,18 @@ def generate_launch_description(): executable='waypoint_manager_node', name='waypoint_manager_node', namespace='orca', - parameters=[config_file], + parameters=[waypoint_manager_config, {'use_sim_time': use_sim_time}], output='screen', ) - return LaunchDescription([waypoint_manager_node]) + return LaunchDescription( + [ + DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use simulation (Gazebo) clock if true', + ), + reference_filter_node, + waypoint_manager_node, + ] + ) diff --git a/mission/waypoint_manager/package.xml b/mission/waypoint_manager/package.xml index 37f46b50..d5379a9b 100644 --- a/mission/waypoint_manager/package.xml +++ b/mission/waypoint_manager/package.xml @@ -17,6 +17,8 @@ tf2 tf2_geometry_msgs eigen + spdlog + fmt ament_cmake diff --git a/mission/waypoint_manager/src/waypoint_manager.cpp b/mission/waypoint_manager/src/waypoint_manager.cpp index 478332c7..8e01b71e 100644 --- a/mission/waypoint_manager/src/waypoint_manager.cpp +++ b/mission/waypoint_manager/src/waypoint_manager.cpp @@ -7,28 +7,24 @@ using namespace std::placeholders; WaypointManagerNode::WaypointManagerNode() : Node("waypoint_manager_node") { - // Create callback groups for server and client server_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); client_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); - // Setup action server, clients and subscribers setup_action_server(); setup_action_clients(); setup_subscribers(); - RCLCPP_INFO(this->get_logger(), "Waypoint Manager Node initialized"); + spdlog::info("Waypoint Manager Node initialized"); } void WaypointManagerNode::setup_action_server() { - // Declare parameters for action server name this->declare_parameter("action_servers.waypoint_manager", "waypoint_manager"); std::string action_server_name = this->get_parameter("action_servers.waypoint_manager").as_string(); - // Create the action server action_server_ = rclcpp_action::create_server( this, action_server_name, std::bind(&WaypointManagerNode::handle_goal, this, _1, _2), @@ -36,44 +32,36 @@ void WaypointManagerNode::setup_action_server() { std::bind(&WaypointManagerNode::handle_accepted, this, _1), rcl_action_server_get_default_options(), server_cb_group_); - RCLCPP_INFO(this->get_logger(), "Action server '%s' started", - action_server_name.c_str()); + spdlog::info("Action server '{}' started", action_server_name); } void WaypointManagerNode::setup_action_clients() { - // Declare parameters for client action server names this->declare_parameter("action_servers.reference_filter", "reference_filter"); std::string reference_filter_server = this->get_parameter("action_servers.reference_filter").as_string(); - // Create action clients reference_filter_client_ = rclcpp_action::create_client( this, reference_filter_server, client_cb_group_); - RCLCPP_INFO(this->get_logger(), "Action client created for '%s'", - reference_filter_server.c_str()); + spdlog::info("Action client created for '{}'", reference_filter_server); } void WaypointManagerNode::setup_subscribers() { - // Declare parameters for subscription topics this->declare_parameter("topics.pose", "pose"); std::string pose_topic = this->get_parameter("topics.pose").as_string(); - // Quality of service profile rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; auto qos_sensor_data = rclcpp::QoS( rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); - // Create pose subscription pose_sub_ = this->create_subscription< geometry_msgs::msg::PoseWithCovarianceStamped>( pose_topic, qos_sensor_data, std::bind(&WaypointManagerNode::pose_callback, this, _1)); - RCLCPP_INFO(this->get_logger(), "Subscribed to pose topic: %s", - pose_topic.c_str()); + spdlog::info("Subscribed to pose topic: {}", pose_topic); } rclcpp_action::GoalResponse WaypointManagerNode::handle_goal( @@ -81,46 +69,40 @@ rclcpp_action::GoalResponse WaypointManagerNode::handle_goal( std::shared_ptr goal) { (void)uuid; // Unused - // Check if goal is valid if (goal->waypoints.empty()) { - RCLCPP_ERROR(this->get_logger(), "Received empty waypoint list"); + spdlog::error("Received empty waypoint list"); return rclcpp_action::GoalResponse::REJECT; } - // Check if target server is valid - only allow reference_filter if (goal->target_server != "reference_filter") { - RCLCPP_ERROR( - this->get_logger(), - "Invalid target server: %s. Only 'reference_filter' is supported", - goal->target_server.c_str()); + spdlog::error( + "Invalid target server: {}. Only 'reference_filter' is supported", + goal->target_server); return rclcpp_action::GoalResponse::REJECT; } - // If there's an active goal, preempt it { std::lock_guard lock(mutex_); if (current_goal_handle_ && current_goal_handle_->is_active()) { - RCLCPP_INFO(this->get_logger(), - "Preempting current goal for new goal"); + spdlog::info("Preempting current goal for new goal"); preempted_goal_id_ = current_goal_handle_->get_goal_id(); } } - RCLCPP_INFO(this->get_logger(), "Accepting new goal with %zu waypoints", - goal->waypoints.size()); + spdlog::info("Accepting new goal with {} waypoints", + goal->waypoints.size()); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } rclcpp_action::CancelResponse WaypointManagerNode::handle_cancel( const std::shared_ptr goal_handle) { (void)goal_handle; // Unused - RCLCPP_INFO(this->get_logger(), "Received cancel request"); + spdlog::info("Received cancel request"); return rclcpp_action::CancelResponse::ACCEPT; } void WaypointManagerNode::handle_accepted( const std::shared_ptr goal_handle) { - // Start a new thread to process the goal std::thread{ std::bind(&WaypointManagerNode::execute_waypoint_navigation, this, _1), goal_handle} @@ -129,7 +111,6 @@ void WaypointManagerNode::handle_accepted( void WaypointManagerNode::execute_waypoint_navigation( const std::shared_ptr goal_handle) { - // Set current goal handle and extract goal information { std::lock_guard lock(mutex_); current_goal_handle_ = goal_handle; @@ -142,22 +123,18 @@ void WaypointManagerNode::execute_waypoint_navigation( current_waypoint_index_ = 0; } - RCLCPP_INFO(this->get_logger(), - "Starting waypoint navigation with %zu waypoints", - waypoints_.size()); + spdlog::info("Starting waypoint navigation with {} waypoints", + waypoints_.size()); - // Create result and feedback objects auto result = std::make_shared(); auto feedback = std::make_shared(); - // Main execution loop while (rclcpp::ok()) { - // Check if goal was canceled or preempted { std::lock_guard lock(mutex_); if (goal_handle->get_goal_id() == preempted_goal_id_) { - RCLCPP_INFO(this->get_logger(), "Goal was preempted"); + spdlog::info("Goal was preempted"); result->success = false; result->completed_waypoints = current_waypoint_index_; goal_handle->abort(result); @@ -166,7 +143,7 @@ void WaypointManagerNode::execute_waypoint_navigation( } if (goal_handle->is_canceling()) { - RCLCPP_INFO(this->get_logger(), "Goal was canceled"); + spdlog::info("Goal was canceled"); result->success = false; result->completed_waypoints = current_waypoint_index_; goal_handle->canceled(result); @@ -175,9 +152,8 @@ void WaypointManagerNode::execute_waypoint_navigation( } } - // Check if we've completed all waypoints if (current_waypoint_index_ >= waypoints_.size()) { - RCLCPP_INFO(this->get_logger(), "All waypoints completed"); + spdlog::info("All waypoints completed"); result->success = true; result->completed_waypoints = waypoints_.size(); goal_handle->succeed(result); @@ -187,19 +163,16 @@ void WaypointManagerNode::execute_waypoint_navigation( return; } - // Send current waypoint to target server (if not already navigating) { std::lock_guard lock(mutex_); if (!current_waypoint_promise_) { - RCLCPP_INFO(this->get_logger(), - "Sending waypoint %zu to target server", - current_waypoint_index_); + spdlog::info("Sending waypoint {} to target server", + current_waypoint_index_); send_waypoint_to_target_server( waypoints_[current_waypoint_index_]); } } - // Calculate distance to current waypoint and check if we've reached it geometry_msgs::msg::Pose target_pose = waypoints_[current_waypoint_index_].pose; geometry_msgs::msg::Pose current_pose; @@ -210,7 +183,6 @@ void WaypointManagerNode::execute_waypoint_navigation( double distance = calculate_distance(current_pose, target_pose); - // Update feedback { std::lock_guard lock(mutex_); feedback->current_pose = current_pose; @@ -219,14 +191,10 @@ void WaypointManagerNode::execute_waypoint_navigation( goal_handle->publish_feedback(feedback); } - // Check if we've reached the waypoint if (distance < switching_threshold_) { - RCLCPP_INFO(this->get_logger(), - "Reached waypoint %zu, distance: %f", - current_waypoint_index_, distance); + spdlog::info("Reached waypoint {}, distance: {}", + current_waypoint_index_, distance); - // If we have a promise, complete it (to cancel any pending target - // server action) { std::lock_guard lock(mutex_); if (current_waypoint_promise_) { @@ -235,11 +203,9 @@ void WaypointManagerNode::execute_waypoint_navigation( } } - // Move to next waypoint current_waypoint_index_++; } - // Sleep to avoid busy waiting std::this_thread::sleep_for(std::chrono::milliseconds(100)); } } @@ -253,7 +219,6 @@ void WaypointManagerNode::pose_callback( double WaypointManagerNode::calculate_distance( const geometry_msgs::msg::Pose& pose1, const geometry_msgs::msg::Pose& pose2) { - // Calculate Euclidean distance between two poses (position only) double dx = pose1.position.x - pose2.position.x; double dy = pose1.position.y - pose2.position.y; double dz = pose1.position.z - pose2.position.z; @@ -263,24 +228,19 @@ double WaypointManagerNode::calculate_distance( void WaypointManagerNode::send_waypoint_to_target_server( const geometry_msgs::msg::PoseStamped& waypoint) { - // Create a new promise for this waypoint current_waypoint_promise_ = std::make_shared>(); - // Check if reference filter client is ready if (!reference_filter_client_->wait_for_action_server( std::chrono::seconds(1))) { - RCLCPP_ERROR(this->get_logger(), - "Reference filter action server not available"); + spdlog::error("Reference filter action server not available"); current_waypoint_promise_->set_value(false); current_waypoint_promise_.reset(); return; } - // Create goal auto goal_msg = ReferenceFilterAction::Goal(); goal_msg.goal = waypoint; - // Send goal with inline lambdas that have the correct signatures auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); @@ -288,16 +248,14 @@ void WaypointManagerNode::send_waypoint_to_target_server( [this](const rclcpp_action::ClientGoalHandle< ReferenceFilterAction>::SharedPtr& goal_handle) { if (!goal_handle) { - RCLCPP_ERROR(this->get_logger(), - "Reference filter goal was rejected"); + spdlog::error("Reference filter goal was rejected"); std::lock_guard lock(mutex_); if (current_waypoint_promise_) { current_waypoint_promise_->set_value(false); current_waypoint_promise_.reset(); } } else { - RCLCPP_INFO(this->get_logger(), - "Reference filter goal accepted"); + spdlog::info("Reference filter goal accepted"); } }; @@ -318,32 +276,28 @@ void WaypointManagerNode::send_waypoint_to_target_server( switch (result.code) { case rclcpp_action::ResultCode::SUCCEEDED: - RCLCPP_INFO(this->get_logger(), - "Reference filter goal succeeded"); + spdlog::info("Reference filter goal succeeded"); if (current_waypoint_promise_) { current_waypoint_promise_->set_value(true); current_waypoint_promise_.reset(); } break; case rclcpp_action::ResultCode::ABORTED: - RCLCPP_ERROR(this->get_logger(), - "Reference filter goal was aborted"); + spdlog::error("Reference filter goal was aborted"); if (current_waypoint_promise_) { current_waypoint_promise_->set_value(false); current_waypoint_promise_.reset(); } break; case rclcpp_action::ResultCode::CANCELED: - RCLCPP_INFO(this->get_logger(), - "Reference filter goal was canceled"); + spdlog::info("Reference filter goal was canceled"); if (current_waypoint_promise_) { current_waypoint_promise_->set_value(false); current_waypoint_promise_.reset(); } break; default: - RCLCPP_ERROR( - this->get_logger(), + spdlog::error( "Unknown result code from reference filter action"); if (current_waypoint_promise_) { current_waypoint_promise_->set_value(false); diff --git a/mission/waypoint_manager/src/waypoint_manager_node.cpp b/mission/waypoint_manager/src/waypoint_manager_node.cpp index 56fc046b..6c6ef67b 100644 --- a/mission/waypoint_manager/src/waypoint_manager_node.cpp +++ b/mission/waypoint_manager/src/waypoint_manager_node.cpp @@ -1,14 +1,22 @@ +#include +#include #include #include "waypoint_manager/waypoint_manager.hpp" int main(int argc, char** argv) { + auto console = spdlog::stdout_color_mt("waypoint_manager"); + spdlog::set_default_logger(console); + spdlog::set_level(spdlog::level::info); + spdlog::set_pattern("[%Y-%m-%d %H:%M:%S.%e] [%n] [%^%l%$] %v"); + spdlog::info("Starting Waypoint Manager"); + rclcpp::init(argc, argv); - RCLCPP_INFO(rclcpp::get_logger("waypoint_manager"), - "Starting Waypoint Manager node"); auto node = std::make_shared(); rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(node); + spdlog::info("Starting executor - Waypoint Manager is running"); executor.spin(); + spdlog::info("Shutting down Waypoint Manager"); rclcpp::shutdown(); return 0; } From e476b5247ede83e0bcd6ca0b93a881687ecd0fbd Mon Sep 17 00:00:00 2001 From: abubakaraliyubadawi Date: Mon, 7 Apr 2025 22:25:27 +0200 Subject: [PATCH 3/8] modified launch file and added config to orca.yaml --- auv_setup/config/robots/orca.yaml | 1 + .../launch/waypoint_manager.launch.py | 22 ++----------------- mission/waypoint_manager/package.xml | 2 +- 3 files changed, 4 insertions(+), 21 deletions(-) diff --git a/auv_setup/config/robots/orca.yaml b/auv_setup/config/robots/orca.yaml index 47039a4b..12b747c8 100644 --- a/auv_setup/config/robots/orca.yaml +++ b/auv_setup/config/robots/orca.yaml @@ -134,6 +134,7 @@ action_servers: reference_filter: "reference_filter" + waypoint_manager: "waypoint_manager" los: "los_guidance" fsm: diff --git a/mission/waypoint_manager/launch/waypoint_manager.launch.py b/mission/waypoint_manager/launch/waypoint_manager.launch.py index 8088cda0..79cd67f9 100644 --- a/mission/waypoint_manager/launch/waypoint_manager.launch.py +++ b/mission/waypoint_manager/launch/waypoint_manager.launch.py @@ -3,31 +3,14 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node def generate_launch_description(): waypoint_manager_pkg_dir = get_package_share_directory('waypoint_manager') - reference_filter_pkg_dir = get_package_share_directory('reference_filter_dp') - - reference_filter_config = os.path.join( - reference_filter_pkg_dir, 'config', 'reference_filter_params.yaml' - ) waypoint_manager_config = os.path.join( - waypoint_manager_pkg_dir, 'config', 'waypoint_manager_params.yaml' - ) - - use_sim_time = LaunchConfiguration('use_sim_time', default='false') - - reference_filter_node = Node( - package='reference_filter_dp', - executable='reference_filter_dp_node', - name='reference_filter_node', - namespace='orca', - parameters=[reference_filter_config, {'use_sim_time': use_sim_time}], - output='screen', + waypoint_manager_pkg_dir, 'config', 'orca.yaml' ) waypoint_manager_node = Node( @@ -35,7 +18,7 @@ def generate_launch_description(): executable='waypoint_manager_node', name='waypoint_manager_node', namespace='orca', - parameters=[waypoint_manager_config, {'use_sim_time': use_sim_time}], + parameters=[waypoint_manager_config], output='screen', ) @@ -46,7 +29,6 @@ def generate_launch_description(): default_value='false', description='Use simulation (Gazebo) clock if true', ), - reference_filter_node, waypoint_manager_node, ] ) diff --git a/mission/waypoint_manager/package.xml b/mission/waypoint_manager/package.xml index d5379a9b..8ad32fd3 100644 --- a/mission/waypoint_manager/package.xml +++ b/mission/waypoint_manager/package.xml @@ -4,7 +4,7 @@ waypoint_manager 0.0.0 Waypoint Manager for Orca task execution - your_name + Abubakar Aliyu Badawi MIT ament_cmake From 6995482f71873d26a1febb32660106dd0c47fd21 Mon Sep 17 00:00:00 2001 From: abubakaraliyubadawi Date: Wed, 9 Apr 2025 15:54:33 +0200 Subject: [PATCH 4/8] Refactor the waypoint node --- .../waypoint_manager/waypoint_manager.hpp | 39 ++- .../launch/waypoint_manager.launch.py | 20 +- .../waypoint_manager/src/waypoint_manager.cpp | 321 ++++++++++-------- 3 files changed, 222 insertions(+), 158 deletions(-) diff --git a/mission/waypoint_manager/include/waypoint_manager/waypoint_manager.hpp b/mission/waypoint_manager/include/waypoint_manager/waypoint_manager.hpp index 302340b1..1c325085 100644 --- a/mission/waypoint_manager/include/waypoint_manager/waypoint_manager.hpp +++ b/mission/waypoint_manager/include/waypoint_manager/waypoint_manager.hpp @@ -1,7 +1,9 @@ #ifndef WAYPOINT_MANAGER_HPP #define WAYPOINT_MANAGER_HPP + #include #include +#include #include #include #include @@ -9,12 +11,14 @@ #include #include #include +#include #include #include class WaypointManagerNode : public rclcpp::Node { public: explicit WaypointManagerNode(); + ~WaypointManagerNode(); private: using WaypointManagerAction = vortex_msgs::action::WaypointManager; @@ -22,49 +26,68 @@ class WaypointManagerNode : public rclcpp::Node { rclcpp_action::ServerGoalHandle; using ReferenceFilterAction = vortex_msgs::action::ReferenceFilterWaypoint; + // Action server and client rclcpp_action::Server::SharedPtr action_server_; rclcpp_action::Client::SharedPtr reference_filter_client_; + + // Subscription rclcpp::Subscription< geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; + // Current state geometry_msgs::msg::Pose current_pose_; - std::mutex mutex_; + + // Mutex and condition variable for thread synchronization + std::mutex queue_mutex_; + std::condition_variable waypoint_cv_; + + // Thread control + bool running_{false}; + std::thread worker_thread_; + + // Callback groups rclcpp::CallbackGroup::SharedPtr server_cb_group_; rclcpp::CallbackGroup::SharedPtr client_cb_group_; + + // Goal tracking std::shared_ptr current_goal_handle_; std::vector waypoints_; size_t current_waypoint_index_{0}; + size_t completed_waypoints_{0}; double switching_threshold_{0.5}; std::string target_server_{"reference_filter"}; bool navigation_active_{false}; - std::shared_ptr> current_waypoint_promise_; rclcpp_action::GoalUUID preempted_goal_id_; + // Setup methods void setup_action_server(); void setup_action_clients(); void setup_subscribers(); + // Action handlers rclcpp_action::GoalResponse handle_goal( - const rclcpp_action::GoalUUID& uuid, + const rclcpp_action::GoalUUID& /*uuid*/, std::shared_ptr goal); rclcpp_action::CancelResponse handle_cancel( - const std::shared_ptr goal_handle); + const std::shared_ptr /*goal_handle*/); void handle_accepted( const std::shared_ptr goal_handle); - void execute_waypoint_navigation( - const std::shared_ptr goal_handle); + // Worker thread method + void process_waypoints_thread(); + // Utility methods void pose_callback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); double calculate_distance(const geometry_msgs::msg::Pose& pose1, const geometry_msgs::msg::Pose& pose2); - void send_waypoint_to_target_server( + bool send_waypoint_to_target_server( const geometry_msgs::msg::PoseStamped& waypoint); }; -#endif + +#endif // WAYPOINT_MANAGER_HPP diff --git a/mission/waypoint_manager/launch/waypoint_manager.launch.py b/mission/waypoint_manager/launch/waypoint_manager.launch.py index 79cd67f9..bc72549b 100644 --- a/mission/waypoint_manager/launch/waypoint_manager.launch.py +++ b/mission/waypoint_manager/launch/waypoint_manager.launch.py @@ -2,15 +2,12 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument from launch_ros.actions import Node def generate_launch_description(): - waypoint_manager_pkg_dir = get_package_share_directory('waypoint_manager') - - waypoint_manager_config = os.path.join( - waypoint_manager_pkg_dir, 'config', 'orca.yaml' + orca_config = os.path.join( + get_package_share_directory("auv_setup"), "config", "robots", "orca.yaml" ) waypoint_manager_node = Node( @@ -18,17 +15,8 @@ def generate_launch_description(): executable='waypoint_manager_node', name='waypoint_manager_node', namespace='orca', - parameters=[waypoint_manager_config], + parameters=[orca_config], output='screen', ) - return LaunchDescription( - [ - DeclareLaunchArgument( - 'use_sim_time', - default_value='false', - description='Use simulation (Gazebo) clock if true', - ), - waypoint_manager_node, - ] - ) + return LaunchDescription([waypoint_manager_node]) diff --git a/mission/waypoint_manager/src/waypoint_manager.cpp b/mission/waypoint_manager/src/waypoint_manager.cpp index 8e01b71e..ed2b14d1 100644 --- a/mission/waypoint_manager/src/waypoint_manager.cpp +++ b/mission/waypoint_manager/src/waypoint_manager.cpp @@ -16,9 +16,27 @@ WaypointManagerNode::WaypointManagerNode() : Node("waypoint_manager_node") { setup_action_clients(); setup_subscribers(); + // Start the worker thread + running_ = true; + worker_thread_ = + std::thread(&WaypointManagerNode::process_waypoints_thread, this); + spdlog::info("Waypoint Manager Node initialized"); } +WaypointManagerNode::~WaypointManagerNode() { + // Signal thread to stop and wait for it to finish + { + std::lock_guard lock(queue_mutex_); + running_ = false; + } + waypoint_cv_.notify_all(); + + if (worker_thread_.joinable()) { + worker_thread_.join(); + } +} + void WaypointManagerNode::setup_action_server() { this->declare_parameter("action_servers.waypoint_manager", "waypoint_manager"); @@ -65,10 +83,8 @@ void WaypointManagerNode::setup_subscribers() { } rclcpp_action::GoalResponse WaypointManagerNode::handle_goal( - const rclcpp_action::GoalUUID& uuid, + const rclcpp_action::GoalUUID& /*uuid*/, std::shared_ptr goal) { - (void)uuid; // Unused - if (goal->waypoints.empty()) { spdlog::error("Received empty waypoint list"); return rclcpp_action::GoalResponse::REJECT; @@ -81,12 +97,13 @@ rclcpp_action::GoalResponse WaypointManagerNode::handle_goal( return rclcpp_action::GoalResponse::REJECT; } - { - std::lock_guard lock(mutex_); - if (current_goal_handle_ && current_goal_handle_->is_active()) { - spdlog::info("Preempting current goal for new goal"); - preempted_goal_id_ = current_goal_handle_->get_goal_id(); - } + // Check if there's an active goal + std::lock_guard lock(queue_mutex_); + if (current_goal_handle_ && current_goal_handle_->is_active()) { + spdlog::info("Preempting current goal for new goal"); + preempted_goal_id_ = current_goal_handle_->get_goal_id(); + // Signal the worker thread to check for preemption + waypoint_cv_.notify_all(); } spdlog::info("Accepting new goal with {} waypoints", @@ -95,124 +112,172 @@ rclcpp_action::GoalResponse WaypointManagerNode::handle_goal( } rclcpp_action::CancelResponse WaypointManagerNode::handle_cancel( - const std::shared_ptr goal_handle) { - (void)goal_handle; // Unused + const std::shared_ptr /*goal_handle*/) { spdlog::info("Received cancel request"); + + // Signal the worker thread to check for cancellation + waypoint_cv_.notify_all(); + return rclcpp_action::CancelResponse::ACCEPT; } void WaypointManagerNode::handle_accepted( const std::shared_ptr goal_handle) { - std::thread{ - std::bind(&WaypointManagerNode::execute_waypoint_navigation, this, _1), - goal_handle} - .detach(); -} + std::lock_guard lock(queue_mutex_); -void WaypointManagerNode::execute_waypoint_navigation( - const std::shared_ptr goal_handle) { - { - std::lock_guard lock(mutex_); - current_goal_handle_ = goal_handle; - navigation_active_ = true; - - auto goal = goal_handle->get_goal(); - waypoints_ = goal->waypoints; - target_server_ = goal->target_server; - switching_threshold_ = goal->switching_threshold; - current_waypoint_index_ = 0; - } + // Store the goal handle + current_goal_handle_ = goal_handle; + + // Get the goal + auto goal = goal_handle->get_goal(); - spdlog::info("Starting waypoint navigation with {} waypoints", - waypoints_.size()); + // Store goal parameters + waypoints_ = goal->waypoints; + target_server_ = goal->target_server; + switching_threshold_ = goal->switching_threshold; + current_waypoint_index_ = 0; + completed_waypoints_ = 0; + navigation_active_ = true; - auto result = std::make_shared(); + // Signal the worker thread that a new goal is available + waypoint_cv_.notify_all(); +} + +void WaypointManagerNode::process_waypoints_thread() { auto feedback = std::make_shared(); - while (rclcpp::ok()) { + while (true) { + std::shared_ptr goal_handle; + std::vector waypoints; + size_t current_index; + double threshold; + bool should_process = false; + + // Wait for work or shutdown signal { - std::lock_guard lock(mutex_); + std::unique_lock lock(queue_mutex_); - if (goal_handle->get_goal_id() == preempted_goal_id_) { - spdlog::info("Goal was preempted"); - result->success = false; - result->completed_waypoints = current_waypoint_index_; - goal_handle->abort(result); - navigation_active_ = false; - return; - } + waypoint_cv_.wait(lock, [this]() { + return !running_ || + (navigation_active_ && + current_waypoint_index_ < waypoints_.size()); + }); - if (goal_handle->is_canceling()) { - spdlog::info("Goal was canceled"); - result->success = false; - result->completed_waypoints = current_waypoint_index_; - goal_handle->canceled(result); - navigation_active_ = false; - return; + // Check if we're shutting down + if (!running_) { + break; } - } - if (current_waypoint_index_ >= waypoints_.size()) { - spdlog::info("All waypoints completed"); - result->success = true; - result->completed_waypoints = waypoints_.size(); - goal_handle->succeed(result); - - std::lock_guard lock(mutex_); - navigation_active_ = false; - return; - } + // Check if goal was preempted or canceled + if (current_goal_handle_) { + if (current_goal_handle_->get_goal_id() == preempted_goal_id_) { + spdlog::info("Goal was preempted"); + auto result = + std::make_shared(); + result->success = false; + result->completed_waypoints = completed_waypoints_; + current_goal_handle_->abort(result); + navigation_active_ = false; + continue; + } - { - std::lock_guard lock(mutex_); - if (!current_waypoint_promise_) { - spdlog::info("Sending waypoint {} to target server", - current_waypoint_index_); - send_waypoint_to_target_server( - waypoints_[current_waypoint_index_]); + if (current_goal_handle_->is_canceling()) { + spdlog::info("Goal was canceled"); + auto result = + std::make_shared(); + result->success = false; + result->completed_waypoints = completed_waypoints_; + current_goal_handle_->canceled(result); + navigation_active_ = false; + continue; + } } - } - geometry_msgs::msg::Pose target_pose = - waypoints_[current_waypoint_index_].pose; - geometry_msgs::msg::Pose current_pose; - { - std::lock_guard lock(mutex_); - current_pose = current_pose_; + // Check if we have an active goal and waypoints to process + if (navigation_active_ && + current_waypoint_index_ < waypoints_.size()) { + goal_handle = current_goal_handle_; + waypoints = waypoints_; + current_index = current_waypoint_index_; + threshold = switching_threshold_; + should_process = true; + } } - double distance = calculate_distance(current_pose, target_pose); + // Process the current waypoint (outside of lock) + if (should_process && goal_handle) { + geometry_msgs::msg::PoseStamped current_waypoint = + waypoints[current_index]; + + // Send waypoint to target server + spdlog::info("Sending waypoint {} to target server", current_index); + bool waypoint_executed = + send_waypoint_to_target_server(current_waypoint); + + if (waypoint_executed) { + // Check if we should move to the next waypoint based on + // distance + geometry_msgs::msg::Pose target_pose = current_waypoint.pose; + geometry_msgs::msg::Pose current_pose; + + { + std::lock_guard lock(queue_mutex_); + current_pose = current_pose_; + } - { - std::lock_guard lock(mutex_); - feedback->current_pose = current_pose; - feedback->current_waypoint_index = current_waypoint_index_; - feedback->distance_to_waypoint = distance; - goal_handle->publish_feedback(feedback); - } + double distance = calculate_distance(current_pose, target_pose); - if (distance < switching_threshold_) { - spdlog::info("Reached waypoint {}, distance: {}", - current_waypoint_index_, distance); + // Update feedback + { + std::lock_guard lock(queue_mutex_); + feedback->current_pose = current_pose; + feedback->current_waypoint_index = current_index; + feedback->distance_to_waypoint = distance; + goal_handle->publish_feedback(feedback); + } - { - std::lock_guard lock(mutex_); - if (current_waypoint_promise_) { - current_waypoint_promise_->set_value(true); - current_waypoint_promise_.reset(); + if (distance < threshold) { + spdlog::info("Reached waypoint {}, distance: {}", + current_index, distance); + + // Update waypoint index and completed count + { + std::lock_guard lock(queue_mutex_); + current_waypoint_index_++; + completed_waypoints_++; + + // Check if we've completed all waypoints + if (current_waypoint_index_ >= waypoints_.size()) { + spdlog::info("All waypoints completed"); + auto result = std::make_shared< + WaypointManagerAction::Result>(); + result->success = true; + result->completed_waypoints = completed_waypoints_; + goal_handle->succeed(result); + navigation_active_ = false; + } + } + } else { + // Wait a bit before checking distance again + std::this_thread::sleep_for(std::chrono::milliseconds(100)); } + } else { + // If waypoint execution failed, abort the goal + std::lock_guard lock(queue_mutex_); + spdlog::error("Failed to execute waypoint {}", current_index); + auto result = std::make_shared(); + result->success = false; + result->completed_waypoints = completed_waypoints_; + goal_handle->abort(result); + navigation_active_ = false; } - - current_waypoint_index_++; } - - std::this_thread::sleep_for(std::chrono::milliseconds(100)); } } void WaypointManagerNode::pose_callback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { - std::lock_guard lock(mutex_); + std::lock_guard lock(queue_mutex_); current_pose_ = msg->pose.pose; } @@ -226,16 +291,15 @@ double WaypointManagerNode::calculate_distance( return std::sqrt(dx * dx + dy * dy + dz * dz); } -void WaypointManagerNode::send_waypoint_to_target_server( +bool WaypointManagerNode::send_waypoint_to_target_server( const geometry_msgs::msg::PoseStamped& waypoint) { - current_waypoint_promise_ = std::make_shared>(); + std::promise waypoint_promise; + std::future waypoint_future = waypoint_promise.get_future(); if (!reference_filter_client_->wait_for_action_server( std::chrono::seconds(1))) { spdlog::error("Reference filter action server not available"); - current_waypoint_promise_->set_value(false); - current_waypoint_promise_.reset(); - return; + return false; } auto goal_msg = ReferenceFilterAction::Goal(); @@ -245,67 +309,56 @@ void WaypointManagerNode::send_waypoint_to_target_server( rclcpp_action::Client::SendGoalOptions(); send_goal_options.goal_response_callback = - [this](const rclcpp_action::ClientGoalHandle< - ReferenceFilterAction>::SharedPtr& goal_handle) { + [&waypoint_promise](const typename rclcpp_action::ClientGoalHandle< + ReferenceFilterAction>::SharedPtr& goal_handle) { if (!goal_handle) { spdlog::error("Reference filter goal was rejected"); - std::lock_guard lock(mutex_); - if (current_waypoint_promise_) { - current_waypoint_promise_->set_value(false); - current_waypoint_promise_.reset(); - } + waypoint_promise.set_value(false); } else { spdlog::info("Reference filter goal accepted"); } }; send_goal_options.feedback_callback = - [this](rclcpp_action::ClientGoalHandle::SharedPtr - goal_handle, - const std::shared_ptr - feedback) { - (void)goal_handle; // Unused - (void)feedback; // Unused, we're not using the feedback from the - // reference filter + [](const typename rclcpp_action::ClientGoalHandle< + ReferenceFilterAction>::SharedPtr&, + const std::shared_ptr) { + // We're not using the feedback from the reference filter }; send_goal_options.result_callback = - [this](const rclcpp_action::ClientGoalHandle< - ReferenceFilterAction>::WrappedResult& result) { - std::lock_guard lock(mutex_); - + [&waypoint_promise](const typename rclcpp_action::ClientGoalHandle< + ReferenceFilterAction>::WrappedResult& result) { switch (result.code) { case rclcpp_action::ResultCode::SUCCEEDED: spdlog::info("Reference filter goal succeeded"); - if (current_waypoint_promise_) { - current_waypoint_promise_->set_value(true); - current_waypoint_promise_.reset(); - } + waypoint_promise.set_value(true); break; case rclcpp_action::ResultCode::ABORTED: spdlog::error("Reference filter goal was aborted"); - if (current_waypoint_promise_) { - current_waypoint_promise_->set_value(false); - current_waypoint_promise_.reset(); - } + waypoint_promise.set_value(false); break; case rclcpp_action::ResultCode::CANCELED: spdlog::info("Reference filter goal was canceled"); - if (current_waypoint_promise_) { - current_waypoint_promise_->set_value(false); - current_waypoint_promise_.reset(); - } + waypoint_promise.set_value(false); break; default: spdlog::error( "Unknown result code from reference filter action"); - if (current_waypoint_promise_) { - current_waypoint_promise_->set_value(false); - current_waypoint_promise_.reset(); - } + waypoint_promise.set_value(false); break; } }; reference_filter_client_->async_send_goal(goal_msg, send_goal_options); + + // Wait for the waypoint execution to complete + // This blocks until the reference filter action is complete + try { + return waypoint_future.get(); + } catch (const std::exception& e) { + spdlog::error("Exception while waiting for waypoint execution: {}", + e.what()); + return false; + } } From ab634c9a032537de58aa9809515b019fba7efd10 Mon Sep 17 00:00:00 2001 From: abubakaraliyubadawi Date: Wed, 9 Apr 2025 19:12:04 +0200 Subject: [PATCH 5/8] fixed reference filter parameter file --- .../config/reference_filter_params.yaml | 8 -------- 1 file changed, 8 deletions(-) diff --git a/guidance/reference_filter_dp/config/reference_filter_params.yaml b/guidance/reference_filter_dp/config/reference_filter_params.yaml index a396eabb..e3101d12 100644 --- a/guidance/reference_filter_dp/config/reference_filter_params.yaml +++ b/guidance/reference_filter_dp/config/reference_filter_params.yaml @@ -2,11 +2,3 @@ ros__parameters: zeta: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] omega: [0.2, 0.2, 0.2, 0.2, 0.2, 0.2] - topics: - pose: "/orca/pose" - twist: "/orca/twist" - guidance: - dp: "/orca/guidance/dp" - aruco_board_pose_camera: "/orca/aruco_board_pose_camera" - action_servers: - reference_filter: "reference_filter" From 3c89bb62bcc4580ba5f6dbf98f8a961512d26f2a Mon Sep 17 00:00:00 2001 From: abubakaraliyubadawi Date: Thu, 10 Apr 2025 02:20:16 +0200 Subject: [PATCH 6/8] Refactor waypoint manager --- .../waypoint_manager/waypoint_manager.hpp | 68 +-- .../waypoint_manager/src/waypoint_manager.cpp | 460 +++++++++--------- 2 files changed, 258 insertions(+), 270 deletions(-) diff --git a/mission/waypoint_manager/include/waypoint_manager/waypoint_manager.hpp b/mission/waypoint_manager/include/waypoint_manager/waypoint_manager.hpp index 1c325085..76104a22 100644 --- a/mission/waypoint_manager/include/waypoint_manager/waypoint_manager.hpp +++ b/mission/waypoint_manager/include/waypoint_manager/waypoint_manager.hpp @@ -3,28 +3,30 @@ #include #include -#include #include #include #include -#include #include #include #include -#include #include #include class WaypointManagerNode : public rclcpp::Node { public: explicit WaypointManagerNode(); - ~WaypointManagerNode(); + ~WaypointManagerNode() = default; private: using WaypointManagerAction = vortex_msgs::action::WaypointManager; using GoalHandleWaypointManager = rclcpp_action::ServerGoalHandle; using ReferenceFilterAction = vortex_msgs::action::ReferenceFilterWaypoint; + using GoalHandleReferenceFilter = + rclcpp_action::ClientGoalHandle; + + // State enumeration + enum class NavState { IDLE, NAVIGATING, WAITING_FOR_REFERENCE_FILTER }; // Action server and client rclcpp_action::Server::SharedPtr action_server_; @@ -37,57 +39,55 @@ class WaypointManagerNode : public rclcpp::Node { // Current state geometry_msgs::msg::Pose current_pose_; - - // Mutex and condition variable for thread synchronization - std::mutex queue_mutex_; - std::condition_variable waypoint_cv_; - - // Thread control - bool running_{false}; - std::thread worker_thread_; - - // Callback groups - rclcpp::CallbackGroup::SharedPtr server_cb_group_; - rclcpp::CallbackGroup::SharedPtr client_cb_group_; - - // Goal tracking + NavState nav_state_ = NavState::IDLE; std::shared_ptr current_goal_handle_; std::vector waypoints_; - size_t current_waypoint_index_{0}; - size_t completed_waypoints_{0}; - double switching_threshold_{0.5}; + size_t current_waypoint_index_ = 0; + size_t completed_waypoints_ = 0; std::string target_server_{"reference_filter"}; - bool navigation_active_{false}; - rclcpp_action::GoalUUID preempted_goal_id_; + std::shared_ptr reference_filter_goal_handle_; // Setup methods void setup_action_server(); - void setup_action_clients(); - void setup_subscribers(); + void setup_action_client(); + void setup_subscriber(); - // Action handlers + // Action server handlers rclcpp_action::GoalResponse handle_goal( - const rclcpp_action::GoalUUID& /*uuid*/, + const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal); rclcpp_action::CancelResponse handle_cancel( - const std::shared_ptr /*goal_handle*/); + const std::shared_ptr goal_handle); void handle_accepted( const std::shared_ptr goal_handle); - // Worker thread method - void process_waypoints_thread(); + // Navigation methods + void start_navigation(); + void send_next_waypoint(); + void complete_navigation(bool success); + void publish_feedback(); - // Utility methods + // Callbacks void pose_callback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + // Reference filter action callbacks + void reference_filter_goal_response_callback( + std::shared_future + future); + + void reference_filter_feedback_callback( + typename GoalHandleReferenceFilter::SharedPtr goal_handle, + const std::shared_ptr feedback); + + void reference_filter_result_callback( + const typename GoalHandleReferenceFilter::WrappedResult& result); + + // Utility methods double calculate_distance(const geometry_msgs::msg::Pose& pose1, const geometry_msgs::msg::Pose& pose2); - - bool send_waypoint_to_target_server( - const geometry_msgs::msg::PoseStamped& waypoint); }; #endif // WAYPOINT_MANAGER_HPP diff --git a/mission/waypoint_manager/src/waypoint_manager.cpp b/mission/waypoint_manager/src/waypoint_manager.cpp index ed2b14d1..ef7c59fe 100644 --- a/mission/waypoint_manager/src/waypoint_manager.cpp +++ b/mission/waypoint_manager/src/waypoint_manager.cpp @@ -1,72 +1,60 @@ #include "waypoint_manager/waypoint_manager.hpp" #include -#include #include -#include - -using namespace std::placeholders; WaypointManagerNode::WaypointManagerNode() : Node("waypoint_manager_node") { - server_cb_group_ = + // Create callback groups to allow concurrent execution + auto server_cb_group = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); - client_cb_group_ = + auto client_cb_group = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); setup_action_server(); - setup_action_clients(); - setup_subscribers(); - - // Start the worker thread - running_ = true; - worker_thread_ = - std::thread(&WaypointManagerNode::process_waypoints_thread, this); + setup_action_client(); + setup_subscriber(); spdlog::info("Waypoint Manager Node initialized"); } -WaypointManagerNode::~WaypointManagerNode() { - // Signal thread to stop and wait for it to finish - { - std::lock_guard lock(queue_mutex_); - running_ = false; - } - waypoint_cv_.notify_all(); - - if (worker_thread_.joinable()) { - worker_thread_.join(); - } -} - void WaypointManagerNode::setup_action_server() { this->declare_parameter("action_servers.waypoint_manager", "waypoint_manager"); std::string action_server_name = this->get_parameter("action_servers.waypoint_manager").as_string(); + rclcpp::SubscriptionOptions server_options; + server_options.callback_group = + this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + action_server_ = rclcpp_action::create_server( this, action_server_name, - std::bind(&WaypointManagerNode::handle_goal, this, _1, _2), - std::bind(&WaypointManagerNode::handle_cancel, this, _1), - std::bind(&WaypointManagerNode::handle_accepted, this, _1), - rcl_action_server_get_default_options(), server_cb_group_); + std::bind(&WaypointManagerNode::handle_goal, this, + std::placeholders::_1, std::placeholders::_2), + std::bind(&WaypointManagerNode::handle_cancel, this, + std::placeholders::_1), + std::bind(&WaypointManagerNode::handle_accepted, this, + std::placeholders::_1), + rcl_action_server_get_default_options(), server_options.callback_group); spdlog::info("Action server '{}' started", action_server_name); } -void WaypointManagerNode::setup_action_clients() { +void WaypointManagerNode::setup_action_client() { this->declare_parameter("action_servers.reference_filter", "reference_filter"); std::string reference_filter_server = this->get_parameter("action_servers.reference_filter").as_string(); + auto client_cb_group = + this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); reference_filter_client_ = rclcpp_action::create_client( - this, reference_filter_server, client_cb_group_); + this, reference_filter_server, client_cb_group); spdlog::info("Action client created for '{}'", reference_filter_server); } -void WaypointManagerNode::setup_subscribers() { +void WaypointManagerNode::setup_subscriber() { this->declare_parameter("topics.pose", "pose"); std::string pose_topic = this->get_parameter("topics.pose").as_string(); @@ -74,10 +62,16 @@ void WaypointManagerNode::setup_subscribers() { auto qos_sensor_data = rclcpp::QoS( rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = + this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + pose_sub_ = this->create_subscription< geometry_msgs::msg::PoseWithCovarianceStamped>( pose_topic, qos_sensor_data, - std::bind(&WaypointManagerNode::pose_callback, this, _1)); + std::bind(&WaypointManagerNode::pose_callback, this, + std::placeholders::_1), + sub_options); spdlog::info("Subscribed to pose topic: {}", pose_topic); } @@ -97,13 +91,11 @@ rclcpp_action::GoalResponse WaypointManagerNode::handle_goal( return rclcpp_action::GoalResponse::REJECT; } - // Check if there's an active goal - std::lock_guard lock(queue_mutex_); - if (current_goal_handle_ && current_goal_handle_->is_active()) { - spdlog::info("Preempting current goal for new goal"); - preempted_goal_id_ = current_goal_handle_->get_goal_id(); - // Signal the worker thread to check for preemption - waypoint_cv_.notify_all(); + // If we have an active goal, we'll cancel it when we accept the new one + if (nav_state_ != NavState::IDLE) { + spdlog::info( + "New goal received while already navigating. Previous goal will be " + "aborted."); } spdlog::info("Accepting new goal with {} waypoints", @@ -115,17 +107,35 @@ rclcpp_action::CancelResponse WaypointManagerNode::handle_cancel( const std::shared_ptr /*goal_handle*/) { spdlog::info("Received cancel request"); - // Signal the worker thread to check for cancellation - waypoint_cv_.notify_all(); + // Cancel any ongoing reference filter action + if (reference_filter_goal_handle_) { + reference_filter_client_->async_cancel_goal( + reference_filter_goal_handle_); + reference_filter_goal_handle_.reset(); + } + nav_state_ = NavState::IDLE; return rclcpp_action::CancelResponse::ACCEPT; } void WaypointManagerNode::handle_accepted( const std::shared_ptr goal_handle) { - std::lock_guard lock(queue_mutex_); + // If we have an active goal, abort it + if (current_goal_handle_ && current_goal_handle_->is_active()) { + auto result = std::make_shared(); + result->success = false; + result->completed_waypoints = completed_waypoints_; + current_goal_handle_->abort(result); + + // Cancel any ongoing reference filter action + if (reference_filter_goal_handle_) { + reference_filter_client_->async_cancel_goal( + reference_filter_goal_handle_); + reference_filter_goal_handle_.reset(); + } + } - // Store the goal handle + // Store the new goal handle current_goal_handle_ = goal_handle; // Get the goal @@ -134,151 +144,201 @@ void WaypointManagerNode::handle_accepted( // Store goal parameters waypoints_ = goal->waypoints; target_server_ = goal->target_server; - switching_threshold_ = goal->switching_threshold; current_waypoint_index_ = 0; completed_waypoints_ = 0; - navigation_active_ = true; - // Signal the worker thread that a new goal is available - waypoint_cv_.notify_all(); + // Start navigation process + start_navigation(); } -void WaypointManagerNode::process_waypoints_thread() { - auto feedback = std::make_shared(); +void WaypointManagerNode::start_navigation() { + nav_state_ = NavState::NAVIGATING; + send_next_waypoint(); +} - while (true) { - std::shared_ptr goal_handle; - std::vector waypoints; - size_t current_index; - double threshold; - bool should_process = false; - - // Wait for work or shutdown signal - { - std::unique_lock lock(queue_mutex_); - - waypoint_cv_.wait(lock, [this]() { - return !running_ || - (navigation_active_ && - current_waypoint_index_ < waypoints_.size()); +void WaypointManagerNode::send_next_waypoint() { + if (current_waypoint_index_ >= waypoints_.size()) { + // All waypoints completed + complete_navigation(true); + return; + } + + // Check if reference filter server is available + if (!reference_filter_client_->wait_for_action_server( + std::chrono::seconds(1))) { + // Server not available - retry with exponential backoff + auto retry_timer = + this->create_wall_timer(std::chrono::seconds(1), [this]() { + if (reference_filter_client_->wait_for_action_server( + std::chrono::milliseconds(100))) { + send_next_waypoint(); + } }); - // Check if we're shutting down - if (!running_) { - break; + spdlog::warn( + "Reference filter action server not available, waiting..."); + return; + } + + // Send current waypoint to reference filter + auto goal_msg = ReferenceFilterAction::Goal(); + goal_msg.goal = waypoints_[current_waypoint_index_]; + + spdlog::info("Sending waypoint {} to reference filter", + current_waypoint_index_); + + auto send_goal_options = + rclcpp_action::Client::SendGoalOptions(); + + // Use lambda functions instead of std::bind to avoid type mismatches + send_goal_options.goal_response_callback = + [this](const std::shared_ptr& goal_handle) { + if (!goal_handle) { + spdlog::error("Reference filter rejected goal"); + complete_navigation(false); + return; } - // Check if goal was preempted or canceled - if (current_goal_handle_) { - if (current_goal_handle_->get_goal_id() == preempted_goal_id_) { - spdlog::info("Goal was preempted"); - auto result = - std::make_shared(); - result->success = false; - result->completed_waypoints = completed_waypoints_; - current_goal_handle_->abort(result); - navigation_active_ = false; - continue; - } + reference_filter_goal_handle_ = goal_handle; + spdlog::info("Reference filter accepted waypoint {}", + current_waypoint_index_); + }; - if (current_goal_handle_->is_canceling()) { - spdlog::info("Goal was canceled"); - auto result = - std::make_shared(); - result->success = false; - result->completed_waypoints = completed_waypoints_; - current_goal_handle_->canceled(result); - navigation_active_ = false; - continue; - } + send_goal_options.feedback_callback = + [this]( + const std::shared_ptr& /*goal_handle*/, + const std::shared_ptr& + feedback) { + if (nav_state_ != NavState::WAITING_FOR_REFERENCE_FILTER || + !current_goal_handle_ || !current_goal_handle_->is_active()) { + return; } - // Check if we have an active goal and waypoints to process - if (navigation_active_ && - current_waypoint_index_ < waypoints_.size()) { - goal_handle = current_goal_handle_; - waypoints = waypoints_; - current_index = current_waypoint_index_; - threshold = switching_threshold_; - should_process = true; + // Forward feedback to our own clients + auto wm_feedback = + std::make_shared(); + + // Convert reference filter feedback to waypoint manager feedback + wm_feedback->current_pose.position.x = feedback->feedback.x; + wm_feedback->current_pose.position.y = feedback->feedback.y; + wm_feedback->current_pose.position.z = feedback->feedback.z; + + // Convert Euler angles to quaternion + tf2::Quaternion q; + q.setRPY(feedback->feedback.roll, feedback->feedback.pitch, + feedback->feedback.yaw); + wm_feedback->current_pose.orientation = tf2::toMsg(q); + + wm_feedback->current_waypoint_index = current_waypoint_index_; + + // Calculate distance to current waypoint (informational only) + if (current_waypoint_index_ < waypoints_.size()) { + geometry_msgs::msg::Pose target_pose = + waypoints_[current_waypoint_index_].pose; + wm_feedback->distance_to_waypoint = + calculate_distance(wm_feedback->current_pose, target_pose); + } else { + wm_feedback->distance_to_waypoint = 0.0; } - } - // Process the current waypoint (outside of lock) - if (should_process && goal_handle) { - geometry_msgs::msg::PoseStamped current_waypoint = - waypoints[current_index]; - - // Send waypoint to target server - spdlog::info("Sending waypoint {} to target server", current_index); - bool waypoint_executed = - send_waypoint_to_target_server(current_waypoint); - - if (waypoint_executed) { - // Check if we should move to the next waypoint based on - // distance - geometry_msgs::msg::Pose target_pose = current_waypoint.pose; - geometry_msgs::msg::Pose current_pose; - - { - std::lock_guard lock(queue_mutex_); - current_pose = current_pose_; - } + current_goal_handle_->publish_feedback(wm_feedback); + }; + + send_goal_options.result_callback = + [this]( + const typename GoalHandleReferenceFilter::WrappedResult& result) { + if (nav_state_ != NavState::WAITING_FOR_REFERENCE_FILTER) { + return; + } - double distance = calculate_distance(current_pose, target_pose); + reference_filter_goal_handle_.reset(); - // Update feedback - { - std::lock_guard lock(queue_mutex_); - feedback->current_pose = current_pose; - feedback->current_waypoint_index = current_index; - feedback->distance_to_waypoint = distance; - goal_handle->publish_feedback(feedback); - } + if (result.code == rclcpp_action::ResultCode::SUCCEEDED) { + spdlog::info("Waypoint {} reached successfully", + current_waypoint_index_); + completed_waypoints_++; + current_waypoint_index_++; + nav_state_ = NavState::NAVIGATING; - if (distance < threshold) { - spdlog::info("Reached waypoint {}, distance: {}", - current_index, distance); - - // Update waypoint index and completed count - { - std::lock_guard lock(queue_mutex_); - current_waypoint_index_++; - completed_waypoints_++; - - // Check if we've completed all waypoints - if (current_waypoint_index_ >= waypoints_.size()) { - spdlog::info("All waypoints completed"); - auto result = std::make_shared< - WaypointManagerAction::Result>(); - result->success = true; - result->completed_waypoints = completed_waypoints_; - goal_handle->succeed(result); - navigation_active_ = false; - } - } - } else { - // Wait a bit before checking distance again - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } + // Move to next waypoint + send_next_waypoint(); } else { - // If waypoint execution failed, abort the goal - std::lock_guard lock(queue_mutex_); - spdlog::error("Failed to execute waypoint {}", current_index); - auto result = std::make_shared(); - result->success = false; - result->completed_waypoints = completed_waypoints_; - goal_handle->abort(result); - navigation_active_ = false; + // Handle failure or cancellation + std::string status; + switch (result.code) { + case rclcpp_action::ResultCode::ABORTED: + status = "ABORTED"; + break; + case rclcpp_action::ResultCode::CANCELED: + status = "CANCELED"; + break; + default: + status = "UNKNOWN"; + break; + } + + spdlog::warn( + "Reference filter action failed for waypoint {}: {}", + current_waypoint_index_, status); + + complete_navigation(false); } - } + }; + + nav_state_ = NavState::WAITING_FOR_REFERENCE_FILTER; + reference_filter_client_->async_send_goal(goal_msg, send_goal_options); +} + +void WaypointManagerNode::complete_navigation(bool success) { + if (!current_goal_handle_ || !current_goal_handle_->is_active()) { + return; + } + + auto result = std::make_shared(); + result->success = success; + result->completed_waypoints = completed_waypoints_; + + if (success) { + current_goal_handle_->succeed(result); + spdlog::info("All waypoints completed successfully"); + } else { + current_goal_handle_->abort(result); + spdlog::error("Navigation failed"); } + + nav_state_ = NavState::IDLE; } void WaypointManagerNode::pose_callback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { - std::lock_guard lock(queue_mutex_); + // Simple state update, no locks needed as this is atomic current_pose_ = msg->pose.pose; + + // Optionally publish feedback if we're actively navigating + if (nav_state_ == NavState::WAITING_FOR_REFERENCE_FILTER && + current_goal_handle_ && current_goal_handle_->is_active()) { + publish_feedback(); + } +} + +void WaypointManagerNode::publish_feedback() { + // Only called from pose_callback or feedback_callback + if (!current_goal_handle_ || !current_goal_handle_->is_active() || + current_waypoint_index_ >= waypoints_.size()) { + return; + } + + auto feedback = std::make_shared(); + feedback->current_pose = current_pose_; + feedback->current_waypoint_index = current_waypoint_index_; + + // Calculate distance to target (informational only) + geometry_msgs::msg::Pose target_pose = + waypoints_[current_waypoint_index_].pose; + feedback->distance_to_waypoint = + calculate_distance(current_pose_, target_pose); + + current_goal_handle_->publish_feedback(feedback); } double WaypointManagerNode::calculate_distance( @@ -290,75 +350,3 @@ double WaypointManagerNode::calculate_distance( return std::sqrt(dx * dx + dy * dy + dz * dz); } - -bool WaypointManagerNode::send_waypoint_to_target_server( - const geometry_msgs::msg::PoseStamped& waypoint) { - std::promise waypoint_promise; - std::future waypoint_future = waypoint_promise.get_future(); - - if (!reference_filter_client_->wait_for_action_server( - std::chrono::seconds(1))) { - spdlog::error("Reference filter action server not available"); - return false; - } - - auto goal_msg = ReferenceFilterAction::Goal(); - goal_msg.goal = waypoint; - - auto send_goal_options = - rclcpp_action::Client::SendGoalOptions(); - - send_goal_options.goal_response_callback = - [&waypoint_promise](const typename rclcpp_action::ClientGoalHandle< - ReferenceFilterAction>::SharedPtr& goal_handle) { - if (!goal_handle) { - spdlog::error("Reference filter goal was rejected"); - waypoint_promise.set_value(false); - } else { - spdlog::info("Reference filter goal accepted"); - } - }; - - send_goal_options.feedback_callback = - [](const typename rclcpp_action::ClientGoalHandle< - ReferenceFilterAction>::SharedPtr&, - const std::shared_ptr) { - // We're not using the feedback from the reference filter - }; - - send_goal_options.result_callback = - [&waypoint_promise](const typename rclcpp_action::ClientGoalHandle< - ReferenceFilterAction>::WrappedResult& result) { - switch (result.code) { - case rclcpp_action::ResultCode::SUCCEEDED: - spdlog::info("Reference filter goal succeeded"); - waypoint_promise.set_value(true); - break; - case rclcpp_action::ResultCode::ABORTED: - spdlog::error("Reference filter goal was aborted"); - waypoint_promise.set_value(false); - break; - case rclcpp_action::ResultCode::CANCELED: - spdlog::info("Reference filter goal was canceled"); - waypoint_promise.set_value(false); - break; - default: - spdlog::error( - "Unknown result code from reference filter action"); - waypoint_promise.set_value(false); - break; - } - }; - - reference_filter_client_->async_send_goal(goal_msg, send_goal_options); - - // Wait for the waypoint execution to complete - // This blocks until the reference filter action is complete - try { - return waypoint_future.get(); - } catch (const std::exception& e) { - spdlog::error("Exception while waiting for waypoint execution: {}", - e.what()); - return false; - } -} From c479698b48d9943e723fe957aaf18ccc7ffdce3c Mon Sep 17 00:00:00 2001 From: abubakaraliyubadawi Date: Thu, 10 Apr 2025 17:18:24 +0200 Subject: [PATCH 7/8] working in the simulator... awaiting real life test --- .../waypoint_manager/waypoint_manager.hpp | 71 +-- .../waypoint_manager/src/waypoint_manager.cpp | 424 ++++++++---------- 2 files changed, 204 insertions(+), 291 deletions(-) diff --git a/mission/waypoint_manager/include/waypoint_manager/waypoint_manager.hpp b/mission/waypoint_manager/include/waypoint_manager/waypoint_manager.hpp index 76104a22..594771f0 100644 --- a/mission/waypoint_manager/include/waypoint_manager/waypoint_manager.hpp +++ b/mission/waypoint_manager/include/waypoint_manager/waypoint_manager.hpp @@ -3,91 +3,68 @@ #include #include +#include #include #include #include +#include #include #include #include +#include #include #include class WaypointManagerNode : public rclcpp::Node { public: explicit WaypointManagerNode(); - ~WaypointManagerNode() = default; + ~WaypointManagerNode(); private: using WaypointManagerAction = vortex_msgs::action::WaypointManager; using GoalHandleWaypointManager = rclcpp_action::ServerGoalHandle; using ReferenceFilterAction = vortex_msgs::action::ReferenceFilterWaypoint; - using GoalHandleReferenceFilter = - rclcpp_action::ClientGoalHandle; - // State enumeration - enum class NavState { IDLE, NAVIGATING, WAITING_FOR_REFERENCE_FILTER }; - - // Action server and client rclcpp_action::Server::SharedPtr action_server_; rclcpp_action::Client::SharedPtr reference_filter_client_; - // Subscription - rclcpp::Subscription< - geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; + std::mutex queue_mutex_; + std::condition_variable waypoint_cv_; + + bool running_{false}; + std::thread worker_thread_; + + rclcpp::CallbackGroup::SharedPtr server_cb_group_; + rclcpp::CallbackGroup::SharedPtr client_cb_group_; - // Current state - geometry_msgs::msg::Pose current_pose_; - NavState nav_state_ = NavState::IDLE; std::shared_ptr current_goal_handle_; std::vector waypoints_; - size_t current_waypoint_index_ = 0; - size_t completed_waypoints_ = 0; + size_t current_waypoint_index_{0}; + size_t completed_waypoints_{0}; + double switching_threshold_{0.5}; std::string target_server_{"reference_filter"}; - std::shared_ptr reference_filter_goal_handle_; + bool navigation_active_{false}; + rclcpp_action::GoalUUID preempted_goal_id_; - // Setup methods void setup_action_server(); - void setup_action_client(); - void setup_subscriber(); + void setup_action_clients(); - // Action server handlers rclcpp_action::GoalResponse handle_goal( - const rclcpp_action::GoalUUID& uuid, + const rclcpp_action::GoalUUID& /*uuid*/, std::shared_ptr goal); rclcpp_action::CancelResponse handle_cancel( - const std::shared_ptr goal_handle); + const std::shared_ptr /*goal_handle*/); void handle_accepted( const std::shared_ptr goal_handle); - // Navigation methods - void start_navigation(); - void send_next_waypoint(); - void complete_navigation(bool success); - void publish_feedback(); - - // Callbacks - void pose_callback( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); - - // Reference filter action callbacks - void reference_filter_goal_response_callback( - std::shared_future - future); - - void reference_filter_feedback_callback( - typename GoalHandleReferenceFilter::SharedPtr goal_handle, - const std::shared_ptr feedback); - - void reference_filter_result_callback( - const typename GoalHandleReferenceFilter::WrappedResult& result); + void process_waypoints_thread(); - // Utility methods - double calculate_distance(const geometry_msgs::msg::Pose& pose1, - const geometry_msgs::msg::Pose& pose2); + bool send_waypoint_to_target_server( + const geometry_msgs::msg::PoseStamped& waypoint); }; -#endif // WAYPOINT_MANAGER_HPP +#endif diff --git a/mission/waypoint_manager/src/waypoint_manager.cpp b/mission/waypoint_manager/src/waypoint_manager.cpp index ef7c59fe..690c2b97 100644 --- a/mission/waypoint_manager/src/waypoint_manager.cpp +++ b/mission/waypoint_manager/src/waypoint_manager.cpp @@ -1,81 +1,68 @@ #include "waypoint_manager/waypoint_manager.hpp" #include +#include #include +#include + +using namespace std::placeholders; WaypointManagerNode::WaypointManagerNode() : Node("waypoint_manager_node") { - // Create callback groups to allow concurrent execution - auto server_cb_group = + server_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); - auto client_cb_group = + client_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); setup_action_server(); - setup_action_client(); - setup_subscriber(); + setup_action_clients(); + + running_ = true; + worker_thread_ = + std::thread(&WaypointManagerNode::process_waypoints_thread, this); spdlog::info("Waypoint Manager Node initialized"); } +WaypointManagerNode::~WaypointManagerNode() { + { + std::lock_guard lock(queue_mutex_); + running_ = false; + } + waypoint_cv_.notify_all(); + + if (worker_thread_.joinable()) { + worker_thread_.join(); + } +} + void WaypointManagerNode::setup_action_server() { this->declare_parameter("action_servers.waypoint_manager", "waypoint_manager"); std::string action_server_name = this->get_parameter("action_servers.waypoint_manager").as_string(); - rclcpp::SubscriptionOptions server_options; - server_options.callback_group = - this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); - action_server_ = rclcpp_action::create_server( this, action_server_name, - std::bind(&WaypointManagerNode::handle_goal, this, - std::placeholders::_1, std::placeholders::_2), - std::bind(&WaypointManagerNode::handle_cancel, this, - std::placeholders::_1), - std::bind(&WaypointManagerNode::handle_accepted, this, - std::placeholders::_1), - rcl_action_server_get_default_options(), server_options.callback_group); + std::bind(&WaypointManagerNode::handle_goal, this, _1, _2), + std::bind(&WaypointManagerNode::handle_cancel, this, _1), + std::bind(&WaypointManagerNode::handle_accepted, this, _1), + rcl_action_server_get_default_options(), server_cb_group_); spdlog::info("Action server '{}' started", action_server_name); } -void WaypointManagerNode::setup_action_client() { +void WaypointManagerNode::setup_action_clients() { this->declare_parameter("action_servers.reference_filter", "reference_filter"); std::string reference_filter_server = this->get_parameter("action_servers.reference_filter").as_string(); - auto client_cb_group = - this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); reference_filter_client_ = rclcpp_action::create_client( - this, reference_filter_server, client_cb_group); + this, reference_filter_server, client_cb_group_); spdlog::info("Action client created for '{}'", reference_filter_server); } -void WaypointManagerNode::setup_subscriber() { - this->declare_parameter("topics.pose", "pose"); - std::string pose_topic = this->get_parameter("topics.pose").as_string(); - - rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; - auto qos_sensor_data = rclcpp::QoS( - rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); - - rclcpp::SubscriptionOptions sub_options; - sub_options.callback_group = - this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); - - pose_sub_ = this->create_subscription< - geometry_msgs::msg::PoseWithCovarianceStamped>( - pose_topic, qos_sensor_data, - std::bind(&WaypointManagerNode::pose_callback, this, - std::placeholders::_1), - sub_options); - - spdlog::info("Subscribed to pose topic: {}", pose_topic); -} - rclcpp_action::GoalResponse WaypointManagerNode::handle_goal( const rclcpp_action::GoalUUID& /*uuid*/, std::shared_ptr goal) { @@ -91,11 +78,11 @@ rclcpp_action::GoalResponse WaypointManagerNode::handle_goal( return rclcpp_action::GoalResponse::REJECT; } - // If we have an active goal, we'll cancel it when we accept the new one - if (nav_state_ != NavState::IDLE) { - spdlog::info( - "New goal received while already navigating. Previous goal will be " - "aborted."); + std::lock_guard lock(queue_mutex_); + if (current_goal_handle_ && current_goal_handle_->is_active()) { + spdlog::info("Preempting current goal for new goal"); + preempted_goal_id_ = current_goal_handle_->get_goal_id(); + waypoint_cv_.notify_all(); } spdlog::info("Accepting new goal with {} waypoints", @@ -107,246 +94,195 @@ rclcpp_action::CancelResponse WaypointManagerNode::handle_cancel( const std::shared_ptr /*goal_handle*/) { spdlog::info("Received cancel request"); - // Cancel any ongoing reference filter action - if (reference_filter_goal_handle_) { - reference_filter_client_->async_cancel_goal( - reference_filter_goal_handle_); - reference_filter_goal_handle_.reset(); - } + waypoint_cv_.notify_all(); - nav_state_ = NavState::IDLE; return rclcpp_action::CancelResponse::ACCEPT; } void WaypointManagerNode::handle_accepted( const std::shared_ptr goal_handle) { - // If we have an active goal, abort it - if (current_goal_handle_ && current_goal_handle_->is_active()) { - auto result = std::make_shared(); - result->success = false; - result->completed_waypoints = completed_waypoints_; - current_goal_handle_->abort(result); - - // Cancel any ongoing reference filter action - if (reference_filter_goal_handle_) { - reference_filter_client_->async_cancel_goal( - reference_filter_goal_handle_); - reference_filter_goal_handle_.reset(); - } - } + std::lock_guard lock(queue_mutex_); - // Store the new goal handle current_goal_handle_ = goal_handle; - // Get the goal auto goal = goal_handle->get_goal(); - // Store goal parameters waypoints_ = goal->waypoints; target_server_ = goal->target_server; + switching_threshold_ = goal->switching_threshold; current_waypoint_index_ = 0; completed_waypoints_ = 0; + navigation_active_ = true; - // Start navigation process - start_navigation(); + waypoint_cv_.notify_all(); } -void WaypointManagerNode::start_navigation() { - nav_state_ = NavState::NAVIGATING; - send_next_waypoint(); -} - -void WaypointManagerNode::send_next_waypoint() { - if (current_waypoint_index_ >= waypoints_.size()) { - // All waypoints completed - complete_navigation(true); - return; - } - - // Check if reference filter server is available - if (!reference_filter_client_->wait_for_action_server( - std::chrono::seconds(1))) { - // Server not available - retry with exponential backoff - auto retry_timer = - this->create_wall_timer(std::chrono::seconds(1), [this]() { - if (reference_filter_client_->wait_for_action_server( - std::chrono::milliseconds(100))) { - send_next_waypoint(); - } - }); +void WaypointManagerNode::process_waypoints_thread() { + auto feedback = std::make_shared(); - spdlog::warn( - "Reference filter action server not available, waiting..."); - return; - } + while (true) { + std::shared_ptr goal_handle; + std::vector waypoints; + size_t current_index; + bool should_process = false; - // Send current waypoint to reference filter - auto goal_msg = ReferenceFilterAction::Goal(); - goal_msg.goal = waypoints_[current_waypoint_index_]; + { + std::unique_lock lock(queue_mutex_); - spdlog::info("Sending waypoint {} to reference filter", - current_waypoint_index_); - - auto send_goal_options = - rclcpp_action::Client::SendGoalOptions(); + waypoint_cv_.wait(lock, [this]() { + return !running_ || + (navigation_active_ && + current_waypoint_index_ < waypoints_.size()); + }); - // Use lambda functions instead of std::bind to avoid type mismatches - send_goal_options.goal_response_callback = - [this](const std::shared_ptr& goal_handle) { - if (!goal_handle) { - spdlog::error("Reference filter rejected goal"); - complete_navigation(false); - return; + if (!running_) { + break; } - reference_filter_goal_handle_ = goal_handle; - spdlog::info("Reference filter accepted waypoint {}", - current_waypoint_index_); - }; - - send_goal_options.feedback_callback = - [this]( - const std::shared_ptr& /*goal_handle*/, - const std::shared_ptr& - feedback) { - if (nav_state_ != NavState::WAITING_FOR_REFERENCE_FILTER || - !current_goal_handle_ || !current_goal_handle_->is_active()) { - return; - } + if (current_goal_handle_) { + if (current_goal_handle_->get_goal_id() == preempted_goal_id_) { + spdlog::info("Goal was preempted"); + auto result = + std::make_shared(); + result->success = false; + result->completed_waypoints = completed_waypoints_; + current_goal_handle_->abort(result); + navigation_active_ = false; + continue; + } - // Forward feedback to our own clients - auto wm_feedback = - std::make_shared(); - - // Convert reference filter feedback to waypoint manager feedback - wm_feedback->current_pose.position.x = feedback->feedback.x; - wm_feedback->current_pose.position.y = feedback->feedback.y; - wm_feedback->current_pose.position.z = feedback->feedback.z; - - // Convert Euler angles to quaternion - tf2::Quaternion q; - q.setRPY(feedback->feedback.roll, feedback->feedback.pitch, - feedback->feedback.yaw); - wm_feedback->current_pose.orientation = tf2::toMsg(q); - - wm_feedback->current_waypoint_index = current_waypoint_index_; - - // Calculate distance to current waypoint (informational only) - if (current_waypoint_index_ < waypoints_.size()) { - geometry_msgs::msg::Pose target_pose = - waypoints_[current_waypoint_index_].pose; - wm_feedback->distance_to_waypoint = - calculate_distance(wm_feedback->current_pose, target_pose); - } else { - wm_feedback->distance_to_waypoint = 0.0; + if (current_goal_handle_->is_canceling()) { + spdlog::info("Goal was canceled"); + auto result = + std::make_shared(); + result->success = false; + result->completed_waypoints = completed_waypoints_; + current_goal_handle_->canceled(result); + navigation_active_ = false; + continue; + } } - current_goal_handle_->publish_feedback(wm_feedback); - }; - - send_goal_options.result_callback = - [this]( - const typename GoalHandleReferenceFilter::WrappedResult& result) { - if (nav_state_ != NavState::WAITING_FOR_REFERENCE_FILTER) { - return; + if (navigation_active_ && + current_waypoint_index_ < waypoints_.size()) { + goal_handle = current_goal_handle_; + waypoints = waypoints_; + current_index = current_waypoint_index_; + should_process = true; } + } - reference_filter_goal_handle_.reset(); + if (should_process && goal_handle) { + geometry_msgs::msg::PoseStamped current_waypoint = + waypoints[current_index]; - if (result.code == rclcpp_action::ResultCode::SUCCEEDED) { - spdlog::info("Waypoint {} reached successfully", - current_waypoint_index_); - completed_waypoints_++; - current_waypoint_index_++; - nav_state_ = NavState::NAVIGATING; + spdlog::info("Sending waypoint {} to target server", + current_index + 1); + bool waypoint_executed = + send_waypoint_to_target_server(current_waypoint); - // Move to next waypoint - send_next_waypoint(); - } else { - // Handle failure or cancellation - std::string status; - switch (result.code) { - case rclcpp_action::ResultCode::ABORTED: - status = "ABORTED"; - break; - case rclcpp_action::ResultCode::CANCELED: - status = "CANCELED"; - break; - default: - status = "UNKNOWN"; - break; + if (waypoint_executed) { + { + std::lock_guard lock(queue_mutex_); + feedback->current_waypoint_index = current_index; + goal_handle->publish_feedback(feedback); } - spdlog::warn( - "Reference filter action failed for waypoint {}: {}", - current_waypoint_index_, status); - - complete_navigation(false); + spdlog::info("Reached waypoint {}", current_index + 1); + { + std::lock_guard lock(queue_mutex_); + current_waypoint_index_++; + completed_waypoints_++; + + if (current_waypoint_index_ >= waypoints_.size()) { + spdlog::info("All waypoints completed"); + auto result = + std::make_shared(); + result->success = true; + result->completed_waypoints = completed_waypoints_; + goal_handle->succeed(result); + navigation_active_ = false; + } + } + } else { + std::lock_guard lock(queue_mutex_); + spdlog::error("Failed to execute waypoint {}", + current_index + 1); + auto result = std::make_shared(); + result->success = false; + result->completed_waypoints = completed_waypoints_; + goal_handle->abort(result); + navigation_active_ = false; } - }; - - nav_state_ = NavState::WAITING_FOR_REFERENCE_FILTER; - reference_filter_client_->async_send_goal(goal_msg, send_goal_options); -} - -void WaypointManagerNode::complete_navigation(bool success) { - if (!current_goal_handle_ || !current_goal_handle_->is_active()) { - return; - } - - auto result = std::make_shared(); - result->success = success; - result->completed_waypoints = completed_waypoints_; - - if (success) { - current_goal_handle_->succeed(result); - spdlog::info("All waypoints completed successfully"); - } else { - current_goal_handle_->abort(result); - spdlog::error("Navigation failed"); + } } - - nav_state_ = NavState::IDLE; } -void WaypointManagerNode::pose_callback( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { - // Simple state update, no locks needed as this is atomic - current_pose_ = msg->pose.pose; +bool WaypointManagerNode::send_waypoint_to_target_server( + const geometry_msgs::msg::PoseStamped& waypoint) { + std::promise waypoint_promise; + std::future waypoint_future = waypoint_promise.get_future(); - // Optionally publish feedback if we're actively navigating - if (nav_state_ == NavState::WAITING_FOR_REFERENCE_FILTER && - current_goal_handle_ && current_goal_handle_->is_active()) { - publish_feedback(); + if (!reference_filter_client_->wait_for_action_server( + std::chrono::seconds(1))) { + spdlog::error("Reference filter action server not available"); + return false; } -} -void WaypointManagerNode::publish_feedback() { - // Only called from pose_callback or feedback_callback - if (!current_goal_handle_ || !current_goal_handle_->is_active() || - current_waypoint_index_ >= waypoints_.size()) { - return; - } + auto goal_msg = ReferenceFilterAction::Goal(); + goal_msg.goal = waypoint; - auto feedback = std::make_shared(); - feedback->current_pose = current_pose_; - feedback->current_waypoint_index = current_waypoint_index_; + auto send_goal_options = + rclcpp_action::Client::SendGoalOptions(); - // Calculate distance to target (informational only) - geometry_msgs::msg::Pose target_pose = - waypoints_[current_waypoint_index_].pose; - feedback->distance_to_waypoint = - calculate_distance(current_pose_, target_pose); + send_goal_options.goal_response_callback = + [&waypoint_promise](const typename rclcpp_action::ClientGoalHandle< + ReferenceFilterAction>::SharedPtr& goal_handle) { + if (!goal_handle) { + spdlog::error("Reference filter goal was rejected"); + waypoint_promise.set_value(false); + } else { + spdlog::info("Reference filter goal accepted"); + } + }; - current_goal_handle_->publish_feedback(feedback); -} + send_goal_options.feedback_callback = + [](const typename rclcpp_action::ClientGoalHandle< + ReferenceFilterAction>::SharedPtr&, + const std::shared_ptr) {}; + + send_goal_options.result_callback = + [&waypoint_promise](const typename rclcpp_action::ClientGoalHandle< + ReferenceFilterAction>::WrappedResult& result) { + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + spdlog::info("Reference filter goal succeeded"); + waypoint_promise.set_value(true); + break; + case rclcpp_action::ResultCode::ABORTED: + spdlog::error("Reference filter goal was aborted"); + waypoint_promise.set_value(false); + break; + case rclcpp_action::ResultCode::CANCELED: + spdlog::info("Reference filter goal was canceled"); + waypoint_promise.set_value(false); + break; + default: + spdlog::error( + "Unknown result code from reference filter action"); + waypoint_promise.set_value(false); + break; + } + }; -double WaypointManagerNode::calculate_distance( - const geometry_msgs::msg::Pose& pose1, - const geometry_msgs::msg::Pose& pose2) { - double dx = pose1.position.x - pose2.position.x; - double dy = pose1.position.y - pose2.position.y; - double dz = pose1.position.z - pose2.position.z; + reference_filter_client_->async_send_goal(goal_msg, send_goal_options); - return std::sqrt(dx * dx + dy * dy + dz * dz); + // Wait for the waypoint execution to complete + try { + return waypoint_future.get(); + } catch (const std::exception& e) { + spdlog::error("Exception while waiting for waypoint execution: {}", + e.what()); + return false; + } } From 58268207263dace1d25442d908bd8d106759906b Mon Sep 17 00:00:00 2001 From: abubakaraliyubadawi Date: Fri, 18 Apr 2025 12:28:57 +0200 Subject: [PATCH 8/8] Optimize thread sync using std::atomic for running_ flag and update readme --- mission/waypoint_manager/README.md | 35 ++++++------------- .../waypoint_manager/waypoint_manager.hpp | 2 +- .../waypoint_manager/src/waypoint_manager.cpp | 1 - 3 files changed, 12 insertions(+), 26 deletions(-) diff --git a/mission/waypoint_manager/README.md b/mission/waypoint_manager/README.md index c7fcd236..60a32c44 100644 --- a/mission/waypoint_manager/README.md +++ b/mission/waypoint_manager/README.md @@ -12,10 +12,9 @@ The Waypoint Manager acts as a coordinator between high-level mission planning a The Waypoint Manager handles higher-level navigation logic: - Receives waypoint sequences via an action server -- Manages waypoint-to-waypoint transitions based on proximity -- Determines when waypoints are reached using configurable thresholds +- Processes waypoints sequentially - Handles preemption of current navigation goals for new requests -- Provides feedback on Orca's navigation progress +- Provides feedback on navigation progress ### Integration with Reference Filter @@ -23,7 +22,7 @@ The Reference Filter provides smoother trajectories via a third-order model that 1. **Client-Server Model**: The Waypoint Manager is a client of the Reference Filter's action server 2. **Single Waypoint Delegation**: The Waypoint Manager sends one waypoint at a time to the Reference Filter -3. **Distance-Based Switching**: When Orca is within the switching threshold of the current waypoint, the Waypoint Manager advances to the next waypoint +3. **Completion-Based Switching**: The Waypoint Manager advances to the next waypoint when the Reference Filter reports successful completion 4. **Asynchronous Communication**: Uses promises and futures to handle asynchronous action server interactions ## How It Works @@ -31,14 +30,12 @@ The Reference Filter provides smoother trajectories via a third-order model that The waypoint navigation process follows these steps: 1. **Waypoint Sequence Reception**: The Waypoint Manager receives a sequence of waypoints through its action server -2. **First Waypoint Dispatch**: It sends the first waypoint to the Reference Filter's action server -3. **Position Monitoring**: Continuously monitors Orca's position -4. **Threshold Checking**: Compares the distance to the current waypoint against the switching threshold -5. **Waypoint Advancement**: When within threshold, it: - - Cancels the current Reference Filter goal (if needed) - - Advances to the next waypoint +2. **Sequential Processing**: It sends each waypoint to the Reference Filter's action server one at a time +3. **Completion Monitoring**: Waits for the Reference Filter to report success or failure for each waypoint +4. **Waypoint Advancement**: When a waypoint is successfully reached, it: + - Advances to the next waypoint in the sequence - Sends the new waypoint to the Reference Filter -6. **Completion Notification**: When all waypoints are reached, the action is marked as succeeded +5. **Completion Notification**: When all waypoints are reached, the action is marked as succeeded ## Custom Action Types @@ -50,23 +47,20 @@ The Waypoint Manager uses a custom action definition: # Goal geometry_msgs/PoseStamped[] waypoints # Array of waypoints for Orca to visit string target_server # Name of the target action server -float64 switching_threshold # Distance threshold for switching waypoints +float64 switching_threshold # Distance threshold (reserved for future use) --- # Result bool success uint32 completed_waypoints # Number of waypoints completed --- # Feedback -geometry_msgs/Pose current_pose # Current pose of Orca uint32 current_waypoint_index # Index of the current waypoint -float64 distance_to_waypoint # Distance to the current waypoint ``` This action allows clients to: - Send multiple waypoints in a single request - Specify which server should handle the trajectory generation (currently only supporting "reference_filter") -- Set a custom threshold for when to consider a waypoint reached -- Receive feedback on progress including the current position, active waypoint, and distance remaining +- Receive feedback on progress including the active waypoint The Waypoint Manager in turn uses the Reference Filter's action server: @@ -91,7 +85,7 @@ vortex_msgs/ReferenceFilter feedback The Waypoint Manager uses a multi-threaded architecture to handle: - Main thread for ROS 2 callbacks - Separate execution thread for waypoint navigation -- Thread-safe communication using mutexes and promises/futures +- Thread-safe communication using mutexes, atomic variables, and promises/futures ### Error Handling @@ -111,19 +105,13 @@ The system includes robust error handling: - **`/orca/reference_filter`**: Sends individual waypoints to the Reference Filter node -### Subscriptions - -- **`/orca/pose`**: Orca's pose feedback (PoseWithCovarianceStamped) - ## Parameters | Parameter | Description | Default | |-----------|-------------|---------| -| `topics.pose` | Topic for Orca's pose | `/orca/pose` | | `action_servers.waypoint_manager` | Name of the waypoint manager action server | `waypoint_manager` | | `action_servers.reference_filter` | Name of the reference filter action server | `reference_filter` | - ## Dependencies - ROS 2 @@ -133,4 +121,3 @@ The system includes robust error handling: - vortex_msgs - tf2, tf2_geometry_msgs - spdlog (for logging) -- fmt (for string formatting) diff --git a/mission/waypoint_manager/include/waypoint_manager/waypoint_manager.hpp b/mission/waypoint_manager/include/waypoint_manager/waypoint_manager.hpp index 594771f0..067f6493 100644 --- a/mission/waypoint_manager/include/waypoint_manager/waypoint_manager.hpp +++ b/mission/waypoint_manager/include/waypoint_manager/waypoint_manager.hpp @@ -31,9 +31,9 @@ class WaypointManagerNode : public rclcpp::Node { reference_filter_client_; std::mutex queue_mutex_; + std::atomic running_{true}; std::condition_variable waypoint_cv_; - bool running_{false}; std::thread worker_thread_; rclcpp::CallbackGroup::SharedPtr server_cb_group_; diff --git a/mission/waypoint_manager/src/waypoint_manager.cpp b/mission/waypoint_manager/src/waypoint_manager.cpp index 690c2b97..89a1a5c4 100644 --- a/mission/waypoint_manager/src/waypoint_manager.cpp +++ b/mission/waypoint_manager/src/waypoint_manager.cpp @@ -24,7 +24,6 @@ WaypointManagerNode::WaypointManagerNode() : Node("waypoint_manager_node") { WaypointManagerNode::~WaypointManagerNode() { { - std::lock_guard lock(queue_mutex_); running_ = false; } waypoint_cv_.notify_all();