Waypoint node

auv_setup/config/robots/orca.yaml

@@ -134,6 +134,7 @@
 
     action_servers:
       reference_filter: "reference_filter"
+      waypoint_manager: "waypoint_manager"
       los: "los_guidance"
 
     fsm:

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"

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()

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)

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"

mission/waypoint_manager/include/waypoint_manager/waypoint_manager.hpp

@@ -0,0 +1,93 @@
+#ifndef WAYPOINT_MANAGER_HPP
+#define WAYPOINT_MANAGER_HPP
+
+#include <spdlog/spdlog.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <condition_variable>
+#include <eigen3/Eigen/Dense>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <mutex>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <thread>
+#include <vortex_msgs/action/reference_filter_waypoint.hpp>
+#include <vortex_msgs/action/waypoint_manager.hpp>
+
+class WaypointManagerNode : public rclcpp::Node {
+   public:
+    explicit WaypointManagerNode();
+    ~WaypointManagerNode();
+
+   private:
+    using WaypointManagerAction = vortex_msgs::action::WaypointManager;
+    using GoalHandleWaypointManager =
+        rclcpp_action::ServerGoalHandle<WaypointManagerAction>;
+    using ReferenceFilterAction = vortex_msgs::action::ReferenceFilterWaypoint;
+
+    // Action server and client
+    rclcpp_action::Server<WaypointManagerAction>::SharedPtr action_server_;
+    rclcpp_action::Client<ReferenceFilterAction>::SharedPtr
+        reference_filter_client_;
+
+    // Subscription
+    rclcpp::Subscription<
+        geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_;
+
+    // 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
+    std::shared_ptr<GoalHandleWaypointManager> current_goal_handle_;
+    std::vector<geometry_msgs::msg::PoseStamped> 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_;
+
+    // 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*/,
+        std::shared_ptr<const WaypointManagerAction::Goal> goal);
+
+    rclcpp_action::CancelResponse handle_cancel(
+        const std::shared_ptr<GoalHandleWaypointManager> /*goal_handle*/);
+
+    void handle_accepted(
+        const std::shared_ptr<GoalHandleWaypointManager> 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);
+
+    bool send_waypoint_to_target_server(
+        const geometry_msgs::msg::PoseStamped& waypoint);
+};
+
+#endif  // WAYPOINT_MANAGER_HPP

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])

mission/waypoint_manager/package.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>waypoint_manager</name>
+  <version>0.0.0</version>
+  <description>Waypoint Manager for Orca task execution</description>
+  <maintainer email="abubakb@stud.ntnu.no">Abubakar Aliyu Badawi</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>vortex_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>eigen</depend>
+  <depend>spdlog</depend>
+  <depend>fmt</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>