Feat/waypoint manager (#645)

* waypoint mode handling

* update tests with new message

* initial package setup

* ros interface function declaration

* node setup

* working prototype

* reentrant cb, multithreaded

* single threaded impl

* conv threshold action goal

* default thresholdref conv value

* removed switching logic

* removed timer execution

* sim test utils

* waypoint_manager_test setup

* no rendering test arg

* waypoint tests, timeout error

* test refactor

* format

* rename utils package

* test suite and description

* first waypoint test

* removed unused function

* renamed service field to priority. Added simple tests

* waypoint manager readme

* uniform attitude naming convention

* fix pr requests

* update tests with new service fields

* four corner test

* update util func name

* update with new action def

* removed failing build type

* test dependencies

* ignore failing yasmin package

* remove __init__ files

* quat_to_euler in make_pose helper

* added __init__ file

* removed sim deps for test packages

* added action shutdown handling

* removed waypoint manager set setup

* added waypoint manager node tests

* waypoint manager 4 corner sim test

* added missing launch testing test dependency

* add sleep for topic discovery

* fix action member field name

* removed unnecessary if here

Author: jorgenfj

.github/workflows/simulator-test.yml

@@ -16,5 +16,6 @@ jobs:
       setup_script: "tests/setup.sh"
       test_scripts: '[
         "tests/simulator_tests/waypoint_navigation/simulator_test.sh",
-        "tests/simulator_tests/los_test/simulator_test.sh"
+        "tests/simulator_tests/los_test/simulator_test.sh",
+        "tests/simulator_tests/waypoint_manager_test/simulator_test.sh"
       ]'

.gitignore

@@ -57,7 +57,4 @@ qtcreator-*
 # Emacs
 .#*
 
-# Catkin custom files
-COLCON_IGNORE
-
 # End of https://www.gitignore.io/api/ros

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_heading = std::atan2(dy, dx);
+
+            r_out(3) = 0.0;
+            r_out(4) = 0.0;
+            r_out(5) = vortex::utils::math::ssa(forward_heading);
+            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);
@@ -292,6 +341,17 @@ void ReferenceFilterNode::execute(
 
         loop_rate.sleep();
     }
+    if (!rclcpp::ok() && goal_handle->is_active()) {
+        auto result = std::make_shared<
+            vortex_msgs::action::ReferenceFilterWaypoint::Result>();
+        result->success = false;
+
+        try {
+            goal_handle->abort(result);
+        } catch (...) {
+            // Ignore exceptions during shutdown
+        }
+    }
 }
 
 RCLCPP_COMPONENTS_REGISTER_NODE(ReferenceFilterNode)

mission/FSM/docking/COLCON_IGNORE

@@ -0,0 +1,2 @@
+# Ignored temporarily to skip building while debugging yasmin upgrade
+# Remove this file to include the package in colcon builds again

mission/FSM/docking/src/docking.cpp

@@ -79,16 +79,18 @@ ApproachDockingStationState::create_goal_handler(
 
     blackboard->set<bool>("is_home", false);
 
-    docking_fsm::PoseStamped docking_offset_goal =
-        blackboard->get<docking_fsm::PoseStamped>("docking_offset_goal");
+    docking_fsm::Pose docking_offset_goal =
+        blackboard->get<docking_fsm::PoseStamped>("docking_offset_goal").pose;
+
+    vortex_msgs::msg::Waypoint waypoint;
+    waypoint.pose = docking_offset_goal;
 
-    goal.goal = docking_offset_goal;
+    goal.waypoint = waypoint;
 
     spdlog::info("Goal sent to action server:");
     spdlog::info("  Position: x = {}, y = {}, z = {}",
-                 docking_offset_goal.pose.position.x,
-                 docking_offset_goal.pose.position.y,
-                 docking_offset_goal.pose.position.z);
+                 docking_offset_goal.position.x, docking_offset_goal.position.y,
+                 docking_offset_goal.position.z);
 
     return goal;
 }
@@ -113,15 +115,15 @@ void ApproachDockingStationState::print_feedback(
     std::shared_ptr<const docking_fsm::ApproachDockingAction::Feedback>
         feedback) {
     docking_fsm::Pose current_pose = docking_fsm::Pose();
-    current_pose.position.x = feedback->feedback.x;
-    current_pose.position.y = feedback->feedback.y;
-    current_pose.position.z = feedback->feedback.z;
+    current_pose.position.x = feedback->reference.x;
+    current_pose.position.y = feedback->reference.y;
+    current_pose.position.z = feedback->reference.z;
 
     blackboard->set<docking_fsm::Pose>("current_pose", current_pose);
 
     spdlog::debug("Current position: x = {}, y = {}, z = {}",
-                  feedback->feedback.x, feedback->feedback.y,
-                  feedback->feedback.z);
+                  feedback->reference.x, feedback->reference.y,
+                  feedback->reference.z);
 }
 
 GoAboveDockingStationState::GoAboveDockingStationState(
@@ -145,7 +147,9 @@ GoAboveDockingStationState::create_goal_handler(
 
     auto docking_offset_goal =
         blackboard->get<docking_fsm::PoseStamped>("docking_offset_goal");
-    goal.goal = docking_offset_goal;
+    vortex_msgs::msg::Waypoint waypoint;
+    waypoint.pose = docking_offset_goal.pose;
+    goal.waypoint = waypoint;
 
     spdlog::info("Goal sent to action server:");
     spdlog::info("  Position: x = {}, y = {}, z = {}",
@@ -179,15 +183,15 @@ void GoAboveDockingStationState::print_feedback(
     std::shared_ptr<const docking_fsm::GoAboveDockingAction::Feedback>
         feedback) {
     docking_fsm::Pose current_pose = docking_fsm::Pose();
-    current_pose.position.x = feedback->feedback.x;
-    current_pose.position.y = feedback->feedback.y;
-    current_pose.position.z = feedback->feedback.z;
+    current_pose.position.x = feedback->reference.x;
+    current_pose.position.y = feedback->reference.y;
+    current_pose.position.z = feedback->reference.z;
 
     blackboard->set<docking_fsm::Pose>("current_pose", current_pose);
 
     spdlog::debug("Current position: x = {}, y = {}, z = {}",
-                  feedback->feedback.x, feedback->feedback.y,
-                  feedback->feedback.z);
+                  feedback->reference.x, feedback->reference.y,
+                  feedback->reference.z);
 }
 
 ConvergeDockingStationState::ConvergeDockingStationState(
@@ -213,7 +217,9 @@ ConvergeDockingStationState::create_goal_handler(
 
     docking_fsm::PoseStamped dock_pose =
         blackboard->get<docking_fsm::PoseStamped>("dock_pose");
-    goal.goal = dock_pose;
+    vortex_msgs::msg::Waypoint waypoint;
+    waypoint.pose = dock_pose.pose;
+    goal.waypoint = waypoint;
 
     spdlog::info("Goal sent to action server:");
     spdlog::info("  Position: x = {}, y = {}, z = {}",
@@ -242,14 +248,14 @@ void ConvergeDockingStationState::print_feedback(
     std::shared_ptr<const docking_fsm::ConvergeDockingAction::Feedback>
         feedback) {
     docking_fsm::Pose current_pose = docking_fsm::Pose();
-    current_pose.position.x = feedback->feedback.x;
-    current_pose.position.y = feedback->feedback.y;
-    current_pose.position.z = feedback->feedback.z;
+    current_pose.position.x = feedback->reference.x;
+    current_pose.position.y = feedback->reference.y;
+    current_pose.position.z = feedback->reference.z;
 
     blackboard->set<docking_fsm::Pose>("current_pose", current_pose);
     spdlog::debug("Current position: x = {}, y = {}, z = {}",
-                  feedback->feedback.x, feedback->feedback.y,
-                  feedback->feedback.z);
+                  feedback->reference.x, feedback->reference.y,
+                  feedback->reference.z);
 }
 
 std::string DockedState(
@@ -286,7 +292,9 @@ docking_fsm::ReturnHomeAction::Goal ReturnHomeState::create_goal_handler(
     docking_fsm::PoseStamped start_pose =
         blackboard->get<docking_fsm::PoseStamped>("start_pose");
 
-    goal.goal = start_pose;
+    vortex_msgs::msg::Waypoint waypoint;
+    waypoint.pose = start_pose.pose;
+    goal.waypoint = waypoint;
     spdlog::info("Goal sent to action server:");
     spdlog::info("  Position: x = {}, y = {}, z = {}",
                  start_pose.pose.position.x, start_pose.pose.position.y,
@@ -313,17 +321,17 @@ void ReturnHomeState::print_feedback(
     std::shared_ptr<yasmin::blackboard::Blackboard> blackboard,
     std::shared_ptr<const docking_fsm::ReturnHomeAction::Feedback> feedback) {
     docking_fsm::Pose current_pose = docking_fsm::Pose();
-    current_pose.position.x = feedback->feedback.x;
-    current_pose.position.y = feedback->feedback.y;
-    current_pose.position.z = feedback->feedback.z;
-    current_pose.orientation.x = feedback->feedback.roll;
-    current_pose.orientation.y = feedback->feedback.pitch;
-    current_pose.orientation.z = feedback->feedback.yaw;
+    current_pose.position.x = feedback->reference.x;
+    current_pose.position.y = feedback->reference.y;
+    current_pose.position.z = feedback->reference.z;
+    current_pose.orientation.x = feedback->reference.roll;
+    current_pose.orientation.y = feedback->reference.pitch;
+    current_pose.orientation.z = feedback->reference.yaw;
 
     blackboard->set<docking_fsm::Pose>("current_pose", current_pose);
     spdlog::debug("Current position: x = {}, y = {}, z = {}",
-                  feedback->feedback.x, feedback->feedback.y,
-                  feedback->feedback.z);
+                  feedback->reference.x, feedback->reference.y,
+                  feedback->reference.z);
 }
 
 std::string AbortState(

mission/waypoint_manager/CMakeLists.txt

@@ -0,0 +1,69 @@
+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(vortex_utils 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
+  vortex_utils
+  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}/
+)
+
+if(BUILD_TESTING)
+  add_subdirectory(test)
+endif()
+
+ament_package()