Feat/waypoint manager

guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_ros.hpp

@@ -74,8 +74,9 @@ class ReferenceFilterNode : public rclcpp::Node {
 
     Eigen::Vector18d fill_reference_state();
 
-    Eigen::Vector6d fill_reference_goal(
-        const geometry_msgs::msg::PoseStamped& goal);
+    Eigen::Vector6d fill_reference_goal(const geometry_msgs::msg::Pose& goal);
+
+    Eigen::Vector6d apply_mode_logic(const Eigen::Vector6d& r_in, uint8_t mode);
 
     vortex_msgs::msg::ReferenceFilter fill_reference_msg();
 

guidance/reference_filter_dp/src/reference_filter_ros.cpp

@@ -195,12 +195,12 @@ Eigen::Vector18d ReferenceFilterNode::fill_reference_state() {
 }
 
 Eigen::Vector6d ReferenceFilterNode::fill_reference_goal(
-    const geometry_msgs::msg::PoseStamped& goal) {
-    double x{goal.pose.position.x};
-    double y{goal.pose.position.y};
-    double z{goal.pose.position.z};
+    const geometry_msgs::msg::Pose& goal) {
+    double x{goal.position.x};
+    double y{goal.position.y};
+    double z{goal.position.z};
 
-    const auto& o = goal.pose.orientation;
+    const auto& o = goal.orientation;
     Eigen::Quaterniond q(o.w, o.x, o.y, o.z);
     Eigen::Vector3d euler_angles = vortex::utils::math::quat_to_euler(q);
     double roll{euler_angles(0)};
@@ -231,6 +231,43 @@ vortex_msgs::msg::ReferenceFilter ReferenceFilterNode::fill_reference_msg() {
     return feedback_msg;
 }
 
+Eigen::Vector6d ReferenceFilterNode::apply_mode_logic(
+    const Eigen::Vector6d& r_in,
+    uint8_t mode) {
+    Eigen::Vector6d r_out = r_in;
+
+    switch (mode) {
+        case vortex_msgs::msg::Waypoint::FULL_POSE:
+            break;
+
+        case vortex_msgs::msg::Waypoint::ONLY_POSITION:
+            r_out(3) = x_(3);
+            r_out(4) = x_(4);
+            r_out(5) = x_(5);
+            break;
+
+        case vortex_msgs::msg::Waypoint::FORWARD_HEADING: {
+            double dx = r_in(0) - x_(0);
+            double dy = r_in(1) - x_(1);
+
+            double forward_yaw = std::atan2(dy, dx);
+
+            r_out(3) = 0.0;
+            r_out(4) = 0.0;
+            r_out(5) = vortex::utils::math::ssa(forward_yaw);
+            break;
+        }
+
+        case vortex_msgs::msg::Waypoint::ONLY_ORIENTATION:
+            r_out(0) = x_(0);
+            r_out(1) = x_(1);
+            r_out(2) = x_(2);
+            break;
+    }
+
+    return r_out;
+}
+
 void ReferenceFilterNode::execute(
     const std::shared_ptr<rclcpp_action::ServerGoalHandle<
         vortex_msgs::action::ReferenceFilterWaypoint>> goal_handle) {
@@ -243,9 +280,21 @@ void ReferenceFilterNode::execute(
 
     x_ = fill_reference_state();
 
-    const geometry_msgs::msg::PoseStamped goal = goal_handle->get_goal()->goal;
+    const geometry_msgs::msg::Pose goal =
+        goal_handle->get_goal()->waypoint.pose;
+    uint8_t mode = goal_handle->get_goal()->waypoint.mode;
+    double convergence_threshold =
+        goal_handle->get_goal()->convergence_threshold;
+
+    if (convergence_threshold <= 0.0) {
+        convergence_threshold = 0.1;
+        spdlog::warn(
+            "ReferenceFilter: Invalid convergence_threshold received (<= 0). "
+            "Using default 0.1");
+    }
 
-    r_ = fill_reference_goal(goal);
+    Eigen::Vector6d r_temp = fill_reference_goal(goal);
+    r_ = apply_mode_logic(r_temp, mode);
 
     auto feedback = std::make_shared<
         vortex_msgs::action::ReferenceFilterWaypoint::Feedback>();
@@ -274,12 +323,12 @@ void ReferenceFilterNode::execute(
 
         vortex_msgs::msg::ReferenceFilter feedback_msg = fill_reference_msg();
 
-        feedback->feedback = feedback_msg;
+        feedback->reference = feedback_msg;
 
         goal_handle->publish_feedback(feedback);
         reference_pub_->publish(feedback_msg);
 
-        if ((x_.head(6) - r_.head(6)).norm() < 0.1) {
+        if ((x_.head(6) - r_.head(6)).norm() < convergence_threshold) {
             result->success = true;
             goal_handle->succeed(result);
             x_.head(6) = r_.head(6);

mission/waypoint_manager/CMakeLists.txt

@@ -0,0 +1,63 @@
+cmake_minimum_required(VERSION 3.8)
+project(waypoint_manager)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 20)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(vortex_msgs REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(spdlog REQUIRED)
+find_package(fmt REQUIRED)
+
+include_directories(include)
+
+set(LIB_NAME "${PROJECT_NAME}_component")
+
+add_library(${LIB_NAME} SHARED
+  src/waypoint_manager_ros.cpp)
+
+ament_target_dependencies(${LIB_NAME} PUBLIC
+  rclcpp
+  rclcpp_components
+  rclcpp_action
+  vortex_msgs
+  tf2_geometry_msgs
+  spdlog
+  fmt
+)
+
+rclcpp_components_register_node(
+  ${LIB_NAME}
+  PLUGIN "WaypointManagerNode"
+  EXECUTABLE ${PROJECT_NAME}_node
+)
+
+ament_export_targets(export_${LIB_NAME})
+
+install(TARGETS ${LIB_NAME}
+  EXPORT export_${LIB_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+install(DIRECTORY
+  launch
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+ament_package()

mission/waypoint_manager/README.md

@@ -0,0 +1,84 @@
+
+
+# Waypoint Manager — ROS 2 Node
+
+
+The **Waypoint Manager** node coordinates mission-level navigation by managing waypoint queues, forwarding them to a **Reference Filter** for trajectory generation, and exposing both an **action interface** (for mission planners) and a **service interface** (for perception-driven waypoint updates).
+
+
+
+# Interfaces
+
+* **[WaypointManager](https://github.com/vortexntnu/vortex-msgs/blob/main/action/WaypointManager.action) (action server)**
+* **[WaypointAddition](https://github.com/vortexntnu/vortex-msgs/blob/main/srv/WaypointAddition.srv) (service server)**
+* **[ReferenceFilterWaypoint](https://github.com/vortexntnu/vortex-msgs/blob/main/action/ReferenceFilterWaypoint.action) (action client)**
+
+
+# Behavior Summary
+
+### **Mission Start**
+
+* On receiving a new action goal, any existing mission is aborted.
+* Waypoints are stored, state is reset, and the first waypoint is sent to the reference filter.
+
+### **During Mission**
+
+* Each waypoint is executed sequentially.
+* Pose feedback from the reference filter is forwarded to the mission planner.
+* `convergence_threshold` determines when a waypoint is reached.
+* If `persistent = true`, the action does not end even when waypoints run out.
+
+### **Mission End**
+
+* If `persistent = false`, the action completes when the last waypoint is reached.
+* If cancelled externally, all state is cleared and reference filter goals are cancelled.
+
+### **Dynamic Waypoint Addition**
+
+Allowed only during **persistent** missions:
+
+* `overwrite = true` → clears old queue and restarts from new waypoints
+* `overwrite = false` → appends waypoints to queue
+* `priority = true` → turn on priority mode. In this mode additional service requests with `priority = false` are ignored. Priority mode is set to false again when there are no more waypoints in queue.
+
+---
+
+### Check available interfaces:
+
+```bash
+ros2 action list
+ros2 service list
+```
+beware of namespace!
+
+
+## Example Action Goal (CLI)
+
+```bash
+ros2 action send_goal /orca/waypoint_manager vortex_msgs/action/WaypointManager \
+'{
+  waypoints:[
+    {
+      pose:{position:{x:5.0,y:0.0,z:0.0},
+            orientation:{x:0,y:0,z:0,w:1}},
+      mode:1
+    }
+  ],
+  convergence_threshold:0.1,
+  persistent:false
+}'
+```
+
+
+## Example Waypoint Addition Service Call
+
+```bash
+ros2 service call /orca/waypoint_addition vortex_msgs/srv/WaypointAddition \
+'{
+  waypoints:[
+    {pose:{position:{x:2,y:3,z:0},orientation:{x:0,y:0,z:0,w:1}},mode:1}
+  ],
+  overwrite:false,
+  priority:true
+}'
+```

mission/waypoint_manager/include/waypoint_manager/waypoint_manager_ros.hpp

@@ -0,0 +1,125 @@
+#ifndef WAYPOINT_MANAGER__WAYPOINT_MANAGER_ROS_HPP_
+#define WAYPOINT_MANAGER__WAYPOINT_MANAGER_ROS_HPP_
+
+#include <memory>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+#include <vector>
+#include <vortex_msgs/action/reference_filter_waypoint.hpp>
+#include <vortex_msgs/action/waypoint_manager.hpp>
+#include <vortex_msgs/msg/waypoint.hpp>
+#include <vortex_msgs/srv/waypoint_addition.hpp>
+
+namespace vortex::mission {
+
+using WaypointManager = vortex_msgs::action::WaypointManager;
+using WaypointManagerGoalHandle =
+    rclcpp_action::ServerGoalHandle<WaypointManager>;
+
+using ReferenceFilterAction = vortex_msgs::action::ReferenceFilterWaypoint;
+using ReferenceFilterGoalHandle =
+    rclcpp_action::ClientGoalHandle<ReferenceFilterAction>;
+
+class WaypointManagerNode : public rclcpp::Node {
+   public:
+    explicit WaypointManagerNode(
+        const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
+
+   private:
+    // @brief Create the action server for WaypointManager.
+    void set_waypoint_action_server();
+
+    // @brief Create the action client for ReferenceFilterWaypoint.
+    void set_reference_action_client();
+
+    // @brief Create the service servers for WaypointAddition.
+    void set_waypoint_service_server();
+
+    // @brief Convert ReferenceFilter feedback to a Pose message
+    // @param fb The ReferenceFilter feedback message
+    // @return The corresponding Pose message
+    geometry_msgs::msg::Pose reference_to_pose(
+        const ReferenceFilterAction::Feedback& fb) const;
+
+    // @brief Construct the result message for the WaypointManager action
+    // @param success Whether the action was successful
+    // @return The constructed result message
+    std::shared_ptr<vortex_msgs::action::WaypointManager_Result>
+    construct_result(bool success) const;
+
+    // @brief Clean up the mission state after completion or cancellation of a
+    // waypoint action. Cancel active goals and reset internal variables. Make
+    // system ready for next action.
+    void cleanup_mission_state();
+
+    // @brief Send the next goal to the ReferenceFilter action server based on
+    // the current waypoint index or finish the waypoint action if all waypoints
+    // have been processed.
+    void send_next_reference_filter_goal();
+
+    // @brief Handle incoming action goal requests
+    // @param uuid The goal UUID
+    // @param goal The goal message
+    // @return The goal response
+    rclcpp_action::GoalResponse handle_waypoint_goal(
+        const rclcpp_action::GoalUUID& uuid,
+        std::shared_ptr<const vortex_msgs::action::WaypointManager::Goal>
+            goal_msg);
+
+    // @brief Handle requests to cancel the waypoint action
+    // @param goal_handle The goal handle
+    // @return The cancel response
+    rclcpp_action::CancelResponse handle_waypoint_cancel(
+        const std::shared_ptr<rclcpp_action::ServerGoalHandle<
+            vortex_msgs::action::WaypointManager>> goal_handle);
+
+    // @brief Handle the accepted goal request
+    // @param goal_handle The goal handle
+    void handle_waypoint_accepted(
+        const std::shared_ptr<rclcpp_action::ServerGoalHandle<
+            vortex_msgs::action::WaypointManager>> goal_handle);
+
+    // @brief Handle incoming waypoint addition service requests
+    //        Only accepted if waypoint action is running.
+    // @param request Incoming service request containing waypoint information.
+    // @param response Service response that should be populated and sent back
+    // to the caller.
+    void handle_waypoint_addition_service_request(
+        const std::shared_ptr<vortex_msgs::srv::WaypointAddition::Request>
+            request,
+        std::shared_ptr<vortex_msgs::srv::WaypointAddition::Response> response);
+
+    // @brief Send a goal to the reference filter
+    // @param goal_msg The action goal
+    void send_reference_filter_goal(
+        const vortex_msgs::action::ReferenceFilterWaypoint::Goal& goal_msg);
+
+    rclcpp_action::Client<vortex_msgs::action::ReferenceFilterWaypoint>::
+        SharedPtr reference_filter_client_;
+    rclcpp_action::Server<vortex_msgs::action::WaypointManager>::SharedPtr
+        waypoint_action_server_;
+    rclcpp::Service<vortex_msgs::srv::WaypointAddition>::SharedPtr
+        waypoint_service_server_;
+
+    std::vector<vortex_msgs::msg::Waypoint> waypoints_{};
+    std::size_t current_index_{0};
+    double convergence_threshold_{0.1};
+
+    bool persistent_action_mode_{false};
+    bool priority_mode_{false};
+
+    ReferenceFilterAction::Feedback latest_ref_feedback_;
+    bool have_reference_pose_{false};
+    bool cancel_in_progress_{false};
+
+    std::uint64_t mission_id_ = 0;
+
+    std::shared_ptr<ReferenceFilterGoalHandle> active_reference_filter_goal_;
+    std::shared_ptr<WaypointManagerGoalHandle> active_action_goal_;
+};
+
+}  // namespace vortex::mission
+
+#endif  // WAYPOINT_MANAGER__WAYPOINT_MANAGER_ROS_HPP_