update to new waypointmode msg (#691)

* update to new waypointmode msg

* update to new waypoint mode msg

* update waypoint mode def in node test

Author: jorgenfj

guidance/reference_filter_dp/include/reference_filter_dp/ros/reference_filter_ros_utils.hpp

@@ -7,26 +7,30 @@
 #include <vortex/utils/types.hpp>
 #include <vortex_msgs/msg/reference_filter.hpp>
 #include <vortex_msgs/msg/waypoint.hpp>
+#include <vortex_msgs/msg/waypoint_mode.hpp>
 #include "reference_filter_dp/lib/eigen_typedefs.hpp"
 #include "reference_filter_dp/lib/waypoint_types.hpp"
 
 namespace vortex::guidance {
 
-/// @brief Convert a ROS waypoint mode to a WaypointMode enum.
-/// @throws std::invalid_argument if the mode value is not recognized.
-inline WaypointMode waypoint_mode_from_ros(uint8_t mode) {
-    switch (mode) {
-        case vortex_msgs::msg::Waypoint::FULL_POSE:
+/**
+ * @brief Convert a ROS waypoint mode to a WaypointMode enum.
+ * @throws std::invalid_argument if the mode value is not recognized.
+ */
+inline WaypointMode waypoint_mode_from_ros(
+    const vortex_msgs::msg::WaypointMode& mode_msg) {
+    switch (mode_msg.mode) {
+        case vortex_msgs::msg::WaypointMode::FULL_POSE:
             return WaypointMode::FULL_POSE;
-        case vortex_msgs::msg::Waypoint::ONLY_POSITION:
+        case vortex_msgs::msg::WaypointMode::ONLY_POSITION:
             return WaypointMode::ONLY_POSITION;
-        case vortex_msgs::msg::Waypoint::FORWARD_HEADING:
+        case vortex_msgs::msg::WaypointMode::FORWARD_HEADING:
             return WaypointMode::FORWARD_HEADING;
-        case vortex_msgs::msg::Waypoint::ONLY_ORIENTATION:
+        case vortex_msgs::msg::WaypointMode::ONLY_ORIENTATION:
             return WaypointMode::ONLY_ORIENTATION;
         default:
             throw std::invalid_argument("Invalid ROS waypoint mode: " +
-                                        std::to_string(mode));
+                                        std::to_string(mode_msg.mode));
     }
 }
 
@@ -36,7 +40,7 @@ inline vortex::guidance::Waypoint waypoint_from_ros(
     Waypoint wp;
     wp.pose =
         vortex::utils::ros_conversions::ros_pose_to_pose_euler(ros_wp.pose);
-    wp.mode = waypoint_mode_from_ros(ros_wp.mode);
+    wp.mode = waypoint_mode_from_ros(ros_wp.waypoint_mode);
     return wp;
 }
 

mission/landmark_server/include/landmark_server/landmark_server_ros.hpp

@@ -21,6 +21,7 @@
 #include <vortex_msgs/action/reference_filter_waypoint.hpp>
 #include <vortex_msgs/msg/landmark_array.hpp>
 #include <vortex_msgs/msg/landmark_track_array.hpp>
+#include <vortex_msgs/msg/waypoint_mode.hpp>
 
 #include <pose_filtering/lib/pose_track_manager.hpp>
 
@@ -183,7 +184,7 @@ class LandmarkServerNode : public rclcpp::Node {
     uint64_t convergence_session_id_{0};
     bool convergence_active_{false};
     bool convergence_dead_reckoning_handoff_{false};
-    uint8_t convergence_mode_{0};
+    vortex_msgs::msg::WaypointMode convergence_mode_;
     std::optional<vortex::filtering::Track> convergence_last_known_track_;
 
     bool convergence_track_lost_{false};

mission/landmark_server/src/landmark_server_convergence.cpp

@@ -2,6 +2,7 @@
 #include <rclcpp_action/client.hpp>
 #include <vortex/utils/ros/ros_conversions.hpp>
 #include <vortex_msgs/msg/waypoint.hpp>
+#include <vortex_msgs/msg/waypoint_mode.hpp>
 #include "landmark_server/landmark_server_ros.hpp"
 
 namespace vortex::mission {
@@ -98,7 +99,7 @@ void LandmarkServerNode::handle_landmark_convergence_accepted(
         convergence_goal()->subtype.value,
         convergence_goal()->convergence_threshold,
         convergence_goal()->dead_reckoning_threshold,
-        convergence_goal()->track_loss_timeout_sec, convergence_mode_);
+        convergence_goal()->track_loss_timeout_sec, convergence_mode_.mode);
 
     const auto track = get_convergence_track();
     if (!track) {
@@ -291,7 +292,7 @@ LandmarkServerNode::make_rf_goal(const geometry_msgs::msg::Pose& target,
     vortex_msgs::action::ReferenceFilterWaypoint::Goal rf_goal;
     vortex_msgs::msg::Waypoint wp;
     wp.pose = target;
-    wp.mode = convergence_mode_;
+    wp.waypoint_mode = convergence_mode_;
     rf_goal.waypoint = wp;
     rf_goal.convergence_threshold = convergence_threshold;
     return rf_goal;
@@ -379,7 +380,7 @@ void LandmarkServerNode::handle_rf_result(
                      convergence_session_id_, static_cast<int>(resultCode));
         active_landmark_convergence_goal_->canceled(make_result(false));
         convergence_active_ = false;
-        convergence_mode_ = vortex_msgs::msg::Waypoint::FULL_POSE;
+        convergence_mode_.mode = vortex_msgs::msg::WaypointMode::FULL_POSE;
         return;
     }
 
@@ -411,7 +412,7 @@ void LandmarkServerNode::handle_rf_result(
     }
 
     convergence_active_ = false;
-    convergence_mode_ = vortex_msgs::msg::Waypoint::FULL_POSE;
+    convergence_mode_.mode = vortex_msgs::msg::WaypointMode::FULL_POSE;
 }
 
 std::optional<vortex::filtering::Track>

mission/waypoint_manager/test/test_single_waypoint.py

@@ -15,7 +15,7 @@
 from rclpy.action import ActionClient
 from rclpy.qos import qos_profile_sensor_data
 from vortex_msgs.action import WaypointManager
-from vortex_msgs.msg import Waypoint
+from vortex_msgs.msg import Waypoint, WaypointMode
 
 from auv_setup.launch_arg_common import (
     declare_drone_and_namespace_args,
@@ -131,7 +131,7 @@ def test_accepts_and_executes_goal(self):
 
         goal_msg = WaypointManager.Goal()
         wp = Waypoint()
-        wp.mode = Waypoint.FULL_POSE
+        wp.waypoint_mode.mode = WaypointMode.FULL_POSE
         wp.pose.position.x = 0.0
         wp.pose.position.y = 0.0
         wp.pose.position.z = 1.0

tests/ros_node_tests/reference_filter_node_test.sh

@@ -29,7 +29,7 @@ fi
 # Send action goal
 echo "Sending goal..."
 ros2 action send_goal /orca/reference_filter vortex_msgs/action/ReferenceFilterWaypoint \
-    "{waypoint: {pose: {position: {x: 1.0,y: 0.0,z: 0.0}, orientation:{x: 0,y: 0,z: 0,w: 1}}, mode: 0}}" &
+    "{waypoint: {pose: {position: {x: 1.0,y: 0.0,z: 0.0}, orientation:{x: 0,y: 0,z: 0,w: 1}}, waypoint_mode: {mode: 0}}}" &
 
 sleep 2