diff --git a/auv_setup/config/robots/orca.yaml b/auv_setup/config/robots/orca.yaml index 47039a4b9..12b747c89 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/guidance/reference_filter_dp/config/reference_filter_params.yaml b/guidance/reference_filter_dp/config/reference_filter_params.yaml index a9cf24aae..e3101d12a 100644 --- a/guidance/reference_filter_dp/config/reference_filter_params.yaml +++ b/guidance/reference_filter_dp/config/reference_filter_params.yaml @@ -1,4 +1,4 @@ /**: 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] diff --git a/mission/waypoint_manager/CMakeLists.txt b/mission/waypoint_manager/CMakeLists.txt new file mode 100644 index 000000000..854a0a604 --- /dev/null +++ b/mission/waypoint_manager/CMakeLists.txt @@ -0,0 +1,60 @@ +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) +find_package(spdlog REQUIRED) +find_package(fmt 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 + spdlog +) +target_link_libraries(waypoint_manager_node fmt::fmt) + +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/README.md b/mission/waypoint_manager/README.md new file mode 100644 index 000000000..60a32c44b --- /dev/null +++ b/mission/waypoint_manager/README.md @@ -0,0 +1,123 @@ +# 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 +- Processes waypoints sequentially +- Handles preemption of current navigation goals for new requests +- Provides feedback on 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. **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 + +The waypoint navigation process follows these steps: + +1. **Waypoint Sequence Reception**: The Waypoint Manager receives a sequence of waypoints through its action server +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 +5. **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 (reserved for future use) +--- +# Result +bool success +uint32 completed_waypoints # Number of waypoints completed +--- +# Feedback +uint32 current_waypoint_index # Index of 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") +- Receive feedback on progress including the active waypoint + +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, atomic variables, 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 + +## Parameters + +| Parameter | Description | Default | +|-----------|-------------|---------| +| `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) 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 000000000..601b7973b --- /dev/null +++ b/mission/waypoint_manager/config/waypoint_manager_params.yaml @@ -0,0 +1,7 @@ +/**: + 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 new file mode 100644 index 000000000..067f6493c --- /dev/null +++ b/mission/waypoint_manager/include/waypoint_manager/waypoint_manager.hpp @@ -0,0 +1,70 @@ +#ifndef WAYPOINT_MANAGER_HPP +#define WAYPOINT_MANAGER_HPP + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class WaypointManagerNode : public rclcpp::Node { + public: + explicit WaypointManagerNode(); + ~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_; + + std::mutex queue_mutex_; + std::atomic running_{true}; + std::condition_variable waypoint_cv_; + + std::thread worker_thread_; + + 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}; + size_t completed_waypoints_{0}; + double switching_threshold_{0.5}; + std::string target_server_{"reference_filter"}; + bool navigation_active_{false}; + rclcpp_action::GoalUUID preempted_goal_id_; + + void setup_action_server(); + void setup_action_clients(); + + 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 process_waypoints_thread(); + + bool 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 000000000..bc72549b7 --- /dev/null +++ b/mission/waypoint_manager/launch/waypoint_manager.launch.py @@ -0,0 +1,22 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + orca_config = os.path.join( + get_package_share_directory("auv_setup"), "config", "robots", "orca.yaml" + ) + + waypoint_manager_node = Node( + package='waypoint_manager', + executable='waypoint_manager_node', + name='waypoint_manager_node', + namespace='orca', + parameters=[orca_config], + 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 000000000..8ad32fd3f --- /dev/null +++ b/mission/waypoint_manager/package.xml @@ -0,0 +1,26 @@ + + + + waypoint_manager + 0.0.0 + Waypoint Manager for Orca task execution + Abubakar Aliyu Badawi + MIT + + ament_cmake + + rclcpp + rclcpp_action + geometry_msgs + nav_msgs + vortex_msgs + 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 new file mode 100644 index 000000000..89a1a5c49 --- /dev/null +++ b/mission/waypoint_manager/src/waypoint_manager.cpp @@ -0,0 +1,287 @@ +#include "waypoint_manager/waypoint_manager.hpp" +#include +#include +#include +#include + +using namespace std::placeholders; + +WaypointManagerNode::WaypointManagerNode() : Node("waypoint_manager_node") { + server_cb_group_ = + this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + client_cb_group_ = + this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + + setup_action_server(); + setup_action_clients(); + + running_ = true; + worker_thread_ = + std::thread(&WaypointManagerNode::process_waypoints_thread, this); + + spdlog::info("Waypoint Manager Node initialized"); +} + +WaypointManagerNode::~WaypointManagerNode() { + { + 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(); + + 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_); + + spdlog::info("Action server '{}' started", action_server_name); +} + +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(); + + reference_filter_client_ = + rclcpp_action::create_client( + this, reference_filter_server, client_cb_group_); + + spdlog::info("Action client created for '{}'", reference_filter_server); +} + +rclcpp_action::GoalResponse WaypointManagerNode::handle_goal( + const rclcpp_action::GoalUUID& /*uuid*/, + std::shared_ptr goal) { + if (goal->waypoints.empty()) { + spdlog::error("Received empty waypoint list"); + return rclcpp_action::GoalResponse::REJECT; + } + + if (goal->target_server != "reference_filter") { + spdlog::error( + "Invalid target server: {}. Only 'reference_filter' is supported", + goal->target_server); + return rclcpp_action::GoalResponse::REJECT; + } + + 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", + goal->waypoints.size()); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse WaypointManagerNode::handle_cancel( + const std::shared_ptr /*goal_handle*/) { + spdlog::info("Received cancel request"); + + waypoint_cv_.notify_all(); + + return rclcpp_action::CancelResponse::ACCEPT; +} + +void WaypointManagerNode::handle_accepted( + const std::shared_ptr goal_handle) { + std::lock_guard lock(queue_mutex_); + + current_goal_handle_ = goal_handle; + + auto goal = goal_handle->get_goal(); + + waypoints_ = goal->waypoints; + target_server_ = goal->target_server; + switching_threshold_ = goal->switching_threshold; + current_waypoint_index_ = 0; + completed_waypoints_ = 0; + navigation_active_ = true; + + waypoint_cv_.notify_all(); +} + +void WaypointManagerNode::process_waypoints_thread() { + auto feedback = std::make_shared(); + + while (true) { + std::shared_ptr goal_handle; + std::vector waypoints; + size_t current_index; + bool should_process = false; + + { + std::unique_lock lock(queue_mutex_); + + waypoint_cv_.wait(lock, [this]() { + return !running_ || + (navigation_active_ && + current_waypoint_index_ < waypoints_.size()); + }); + + if (!running_) { + break; + } + + 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; + } + + 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; + } + } + + if (navigation_active_ && + current_waypoint_index_ < waypoints_.size()) { + goal_handle = current_goal_handle_; + waypoints = waypoints_; + current_index = current_waypoint_index_; + should_process = true; + } + } + + if (should_process && goal_handle) { + geometry_msgs::msg::PoseStamped current_waypoint = + waypoints[current_index]; + + spdlog::info("Sending waypoint {} to target server", + current_index + 1); + bool waypoint_executed = + send_waypoint_to_target_server(current_waypoint); + + if (waypoint_executed) { + { + std::lock_guard lock(queue_mutex_); + feedback->current_waypoint_index = current_index; + goal_handle->publish_feedback(feedback); + } + + 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; + } + } + } +} + +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) {}; + + 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 + try { + return waypoint_future.get(); + } catch (const std::exception& e) { + spdlog::error("Exception while waiting for waypoint execution: {}", + e.what()); + return false; + } +} 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 000000000..6c6ef67b4 --- /dev/null +++ b/mission/waypoint_manager/src/waypoint_manager_node.cpp @@ -0,0 +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); + 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; +}