CurryMonsters · Oct 22, 2026 · Oct 5, 2026 · Oct 6, 2026 · Oct 6, 2026 · Oct 12, 2026
diff --git a/dependencies.repos b/dependencies.repos
@@ -2,3 +2,4 @@ repositories:
   vortex-msgs:
     type: git
     url: https://github.com/vortexntnu/vortex-msgs.git
+    version: development
diff --git a/vortex_utils/include/vortex/utils/types.hpp b/vortex_utils/include/vortex/utils/types.hpp
@@ -467,6 +467,9 @@ struct Waypoint {
     bool keep_altitude{false};
     double desired_altitude{0.0};
     bool require_altitude_convergence{false};
+    double position_tolerance{0.0};
+    double orientation_tolerance{0.0};
+    double hold_time_sec{0.0};
 };
 
 struct SonarInfo {

diff --git a/vortex_utils/include/vortex/utils/waypoint_utils.hpp b/vortex_utils/include/vortex/utils/waypoint_utils.hpp
@@ -11,12 +11,13 @@ using vortex::utils::types::WaypointMode;
 
 /**
  * @brief Struct to represent a waypoint goal, containing the target pose, the
- * waypoint mode, and the convergence threshold.
+ * waypoint mode, and the position [m] and orientation [rad] tolerances.
  */
 struct WaypointGoal {
     Pose pose;
     WaypointMode mode;
-    double convergence_threshold{0.1};
+    double position_tolerance{0.0};
+    double orientation_tolerance{0.0};
     bool keep_altitude{false};
     double desired_altitude{0.0};
     bool require_altitude_convergence{false};
@@ -70,6 +71,34 @@ Pose compute_waypoint_goal(const Pose& incoming_waypoint,
                            WaypointMode mode,
                            const Pose& current_state);
 
+/**
+ * @brief Position [m] and orientation [rad] error over the DOFs a waypoint
+ * mode controls.
+ */
+struct ControlledError {
+    double position{0.0};
+    double orientation{0.0};
+};
+
+/**
+ * @brief Position [m] and orientation [rad] tolerances. A value <= 0 is not
+ * checked.
+ */
+struct ConvergenceTolerance {
+    double position{0.0};
+    double orientation{0.0};
+};
+
+/**
+ * @brief Compute the controlled position and orientation error.
+ * @param state The current state pose.
+ * @param waypoint_goal The waypoint goal pose.
+ * @param mode The waypoint mode.
+ */
+ControlledError controlled_error(const Pose& state,
+                                 const Pose& waypoint_goal,
+                                 WaypointMode mode);
+
 /**
  * @brief Check whether the state has converged to the waypoint goal.
  *
@@ -86,6 +115,14 @@ bool has_converged(const Pose& state,
                    WaypointMode mode,
                    double convergence_threshold);
 
+/**
+ * @brief Check convergence with separate position and orientation tolerances.
+ */
+bool has_converged(const Pose& state,
+                   const Pose& waypoint_goal,
+                   WaypointMode mode,
+                   const ConvergenceTolerance& tolerance);
+
 /**
  * @brief Apply a pose offset to a base pose.
  *
@@ -133,9 +170,10 @@ Pose load_pose_from_yaml(const std::string& file_path,
  * "only_position") position:            # Required for FULL_POSE,
  * ONLY_POSITION, FORWARD_HEADING x: 1.0 y: 0.0 z: -0.5 orientation:         #
  * Required for FULL_POSE, ONLY_ORIENTATION roll: 0.0 pitch: 0.0 yaw: 3.14159
- *   convergence_threshold: 0.1  # Optional, default is 0.1
- *   keep_altitude: true          # Optional, default false
- *   desired_altitude: 1.5        # Required when keep_altitude is true
+ *   position_tolerance: 0.1          # Optional [m]
+ *   orientation_tolerance_deg: 5.0   # Optional [deg]
+ *   keep_altitude: true              # Optional, default false
+ *   desired_altitude: 1.5            # Required when keep_altitude is true
  * @endcode
  *
  * @param file_path Path to the YAML file.

diff --git a/vortex_utils/src/waypoint_utils.cpp b/vortex_utils/src/waypoint_utils.cpp
@@ -236,42 +236,72 @@ Pose compute_waypoint_goal(const Pose& incoming_waypoint,
     return waypoint_out;
 }
 
-bool has_converged(const Pose& state,
-                   const Pose& waypoint_goal,
-                   WaypointMode mode,
-                   double convergence_threshold) {
+namespace {
+
+struct ControlledErrorSq {
+    double position{0.0};
+    double orientation{0.0};
+};
+
+ControlledErrorSq controlled_error_sq(const Pose& state,
+                                      const Pose& waypoint_goal,
+                                      WaypointMode mode) {
     const Eigen::Vector3d ep = state.pos_vector() - waypoint_goal.pos_vector();
 
     const Eigen::Vector3d ea = vortex::utils::math::quaternion_error(
         state.ori_quaternion(), waypoint_goal.ori_quaternion());
 
-    const double err = [&] {
-        switch (mode) {
-            case WaypointMode::ONLY_POSITION:
-                return ep.norm();
-            case WaypointMode::ONLY_ORIENTATION:
-                return ea.norm();
-            case WaypointMode::FORWARD_HEADING:
-                return std::sqrt(ep.squaredNorm() + ea(2) * ea(2));
-            case WaypointMode::POSITION_AND_YAW:
-                return std::sqrt(ep.squaredNorm() + ea(2) * ea(2));
-            case WaypointMode::XY_AND_YAW:
-                return std::sqrt(ep.head<2>().squaredNorm() + ea(2) * ea(2));
-            case WaypointMode::XY_FORWARD_DIR:
-                return ep.head<2>().norm();
-            case WaypointMode::LEVEL_ORIENTATION:
-                return ea.head<2>().norm();
-            case WaypointMode::ONLY_Z:
-                return std::abs(ep(2));
-            case WaypointMode::POS_Z_LEVEL_ORIENTATION:
-                return std::sqrt(ep(2) * ep(2) + ea.head<2>().squaredNorm());
-            case WaypointMode::FULL_POSE:
-            default:
-                return std::sqrt(ep.squaredNorm() + ea.squaredNorm());
-        }
-    }();
+    switch (mode) {
+        case WaypointMode::ONLY_POSITION:
+            return {ep.squaredNorm(), 0.0};
+        case WaypointMode::ONLY_ORIENTATION:
+            return {0.0, ea.squaredNorm()};
+        case WaypointMode::FORWARD_HEADING:
+        case WaypointMode::POSITION_AND_YAW:
+            return {ep.squaredNorm(), ea(2) * ea(2)};
+        case WaypointMode::XY_AND_YAW:
+            return {ep.head<2>().squaredNorm(), ea(2) * ea(2)};
+        case WaypointMode::XY_FORWARD_DIR:
+            return {ep.head<2>().squaredNorm(), 0.0};
+        case WaypointMode::LEVEL_ORIENTATION:
+            return {0.0, ea.head<2>().squaredNorm()};
+        case WaypointMode::ONLY_Z:
+            return {ep(2) * ep(2), 0.0};
+        case WaypointMode::POS_Z_LEVEL_ORIENTATION:
+            return {ep(2) * ep(2), ea.head<2>().squaredNorm()};
+        case WaypointMode::FULL_POSE:
+        default:
+            return {ep.squaredNorm(), ea.squaredNorm()};
+    }
+}
+
+}  // namespace
 
-    return err < convergence_threshold;
+ControlledError controlled_error(const Pose& state,
+                                 const Pose& waypoint_goal,
+                                 WaypointMode mode) {
+    const auto sq = controlled_error_sq(state, waypoint_goal, mode);
+    return {std::sqrt(sq.position), std::sqrt(sq.orientation)};
+}
+
+bool has_converged(const Pose& state,
+                   const Pose& waypoint_goal,
+                   WaypointMode mode,
+                   double convergence_threshold) {
+    const auto sq = controlled_error_sq(state, waypoint_goal, mode);
+    return std::sqrt(sq.position + sq.orientation) < convergence_threshold;
+}
+
+bool has_converged(const Pose& state,
+                   const Pose& waypoint_goal,
+                   WaypointMode mode,
+                   const ConvergenceTolerance& tolerance) {
+    const auto err = controlled_error(state, waypoint_goal, mode);
+    const bool position_ok =
+        tolerance.position <= 0.0 || err.position < tolerance.position;
+    const bool orientation_ok =
+        tolerance.orientation <= 0.0 || err.orientation < tolerance.orientation;
+    return position_ok && orientation_ok;
 }
 
 Pose apply_pose_offset(const Pose& base, const Pose& offset) {
@@ -334,9 +364,14 @@ WaypointGoal load_waypoint_goal_from_yaml(const std::string& file_path,
     const auto mode = load_mode(wp);
     const Pose pose = load_pose_for_mode(wp, mode);
 
-    double convergence_threshold = 0.1;
-    if (wp["convergence_threshold"]) {
-        convergence_threshold = wp["convergence_threshold"].as<double>();
+    double position_tolerance = 0.0;
+    if (wp["position_tolerance"]) {
+        position_tolerance = wp["position_tolerance"].as<double>();
+    }
+    double orientation_tolerance = 0.0;
+    if (wp["orientation_tolerance_deg"]) {
+        orientation_tolerance =
+            wp["orientation_tolerance_deg"].as<double>() * (M_PI / 180.0);
     }
 
     bool keep_altitude = false;
@@ -361,7 +396,8 @@ WaypointGoal load_waypoint_goal_from_yaml(const std::string& file_path,
     return WaypointGoal{
         .pose = pose,
         .mode = mode,
-        .convergence_threshold = convergence_threshold,
+        .position_tolerance = position_tolerance,
+        .orientation_tolerance = orientation_tolerance,
         .keep_altitude = keep_altitude,
         .desired_altitude = desired_altitude,
         .require_altitude_convergence = require_altitude_convergence};

diff --git a/vortex_utils_ros/include/vortex/utils/ros/landmark_names.hpp b/vortex_utils_ros/include/vortex/utils/ros/landmark_names.hpp
@@ -0,0 +1,142 @@
+#ifndef VORTEX_UTILS_ROS_LANDMARK_NAMES_HPP
+#define VORTEX_UTILS_ROS_LANDMARK_NAMES_HPP
+
+#include <cstdint>
+#include <optional>
+#include <string_view>
+#include <utility>
+
+#include <vortex_msgs/msg/landmark_subtype.hpp>
+#include <vortex_msgs/msg/landmark_type.hpp>
+
+namespace vortex::utils::landmarks {
+
+/// A LandmarkType constant and its name.
+struct LandmarkTypeName {
+    std::string_view name;
+    std::uint16_t value;
+};
+
+/// A LandmarkSubtype constant, its name and the LandmarkType it belongs to.
+struct LandmarkSubtypeName {
+    std::string_view name;
+    std::uint16_t value;
+    std::uint16_t type;
+};
+
+using vortex_msgs::msg::LandmarkSubtype;
+using vortex_msgs::msg::LandmarkType;
+
+// Add new constants from LandmarkType.msg and LandmarkSubtype.msg here.
+inline constexpr LandmarkTypeName kLandmarkTypes[] = {
+    {"ARUCO_MARKER", LandmarkType::ARUCO_MARKER},
+    {"ARUCO_BOARD", LandmarkType::ARUCO_BOARD},
+    {"PIPELINE_START", LandmarkType::PIPELINE_START},
+    {"PIPELINE_END", LandmarkType::PIPELINE_END},
+    {"VALVE", LandmarkType::VALVE},
+    {"GATE", LandmarkType::GATE},
+    {"SLALOM_PIPE", LandmarkType::SLALOM_PIPE},
+    {"TORPEDO_BOARD", LandmarkType::TORPEDO_BOARD},
+    {"BIN", LandmarkType::BIN},
+    {"PATH_MARKER", LandmarkType::PATH_MARKER},
+    {"TABLE", LandmarkType::TABLE},
+    {"OCTAGON", LandmarkType::OCTAGON},
+    {"PINGER", LandmarkType::PINGER},
+};
+
+inline constexpr LandmarkSubtypeName kLandmarkSubtypes[] = {
+    {"ARUCO_BOARD_CAMERA", LandmarkSubtype::ARUCO_BOARD_CAMERA,
+     LandmarkType::ARUCO_BOARD},
+    {"ARUCO_BOARD_SONAR", LandmarkSubtype::ARUCO_BOARD_SONAR,
+     LandmarkType::ARUCO_BOARD},
+    {"ARUCO_BOARD_DETECTION", LandmarkSubtype::ARUCO_BOARD_DETECTION,
+     LandmarkType::ARUCO_BOARD},
+    {"VALVE_VERTICAL", LandmarkSubtype::VALVE_VERTICAL, LandmarkType::VALVE},
+    {"VALVE_HORIZONTAL", LandmarkSubtype::VALVE_HORIZONTAL,
+     LandmarkType::VALVE},
+    {"PIPELINE_START_CAMERA", LandmarkSubtype::PIPELINE_START_CAMERA,
+     LandmarkType::PIPELINE_START},
+    {"PIPELINE_START_SONAR", LandmarkSubtype::PIPELINE_START_SONAR,
+     LandmarkType::PIPELINE_START},
+    {"GATE_SEARCH_RESCUE", LandmarkSubtype::GATE_SEARCH_RESCUE,
+     LandmarkType::GATE},
+    {"GATE_SURVEY_REPAIR", LandmarkSubtype::GATE_SURVEY_REPAIR,
+     LandmarkType::GATE},
+    {"SLALOM_PIPE_WHITE", LandmarkSubtype::SLALOM_PIPE_WHITE,
+     LandmarkType::SLALOM_PIPE},
+    {"SLALOM_PIPE_RED", LandmarkSubtype::SLALOM_PIPE_RED,
+     LandmarkType::SLALOM_PIPE},
+    {"TORPEDO_BOARD_WHOLE", LandmarkSubtype::TORPEDO_BOARD_WHOLE,
+     LandmarkType::TORPEDO_BOARD},
+    {"TORPEDO_OPENING_LARGE_LEFT", LandmarkSubtype::TORPEDO_OPENING_LARGE_LEFT,
+     LandmarkType::TORPEDO_BOARD},
+    {"TORPEDO_OPENING_LARGE_RIGHT",
+     LandmarkSubtype::TORPEDO_OPENING_LARGE_RIGHT, LandmarkType::TORPEDO_BOARD},
+    {"TORPEDO_OPENING_SMALL_TOP", LandmarkSubtype::TORPEDO_OPENING_SMALL_TOP,
+     LandmarkType::TORPEDO_BOARD},
+    {"TORPEDO_OPENING_SMALL_BOTTOM",
+     LandmarkSubtype::TORPEDO_OPENING_SMALL_BOTTOM,
+     LandmarkType::TORPEDO_BOARD},
+    {"BIN_SEARCH_RESCUE", LandmarkSubtype::BIN_SEARCH_RESCUE,
+     LandmarkType::BIN},
+    {"BIN_SURVEY_REPAIR", LandmarkSubtype::BIN_SURVEY_REPAIR,
+     LandmarkType::BIN},
+    {"BIN_UNCLASSIFIED", LandmarkSubtype::BIN_UNCLASSIFIED, LandmarkType::BIN},
+    {"BIN_STRUCTURE", LandmarkSubtype::BIN_STRUCTURE, LandmarkType::BIN},
+    {"PATH_MARKER_WHOLE", LandmarkSubtype::PATH_MARKER_WHOLE,
+     LandmarkType::PATH_MARKER},
+    {"TABLE_WHOLE", LandmarkSubtype::TABLE_WHOLE, LandmarkType::TABLE},
+    {"TABLE_ITEM_NUTBOLT", LandmarkSubtype::TABLE_ITEM_NUTBOLT,
+     LandmarkType::TABLE},
+    {"TABLE_ITEM_ELECTRIC", LandmarkSubtype::TABLE_ITEM_ELECTRIC,
+     LandmarkType::TABLE},
+    {"TABLE_ITEM_PILL", LandmarkSubtype::TABLE_ITEM_PILL, LandmarkType::TABLE},
+    {"TABLE_ITEM_BANDAID", LandmarkSubtype::TABLE_ITEM_BANDAID,
+     LandmarkType::TABLE},
+    {"TABLE_BASKET_SURVEY_REPAIR", LandmarkSubtype::TABLE_BASKET_SURVEY_REPAIR,
+     LandmarkType::TABLE},
+    {"TABLE_BASKET_SEARCH_RESCUE", LandmarkSubtype::TABLE_BASKET_SEARCH_RESCUE,
+     LandmarkType::TABLE},
+    {"OCTAGON_WHOLE", LandmarkSubtype::OCTAGON_WHOLE, LandmarkType::OCTAGON},
+    {"OCTAGON_IMAGE_REPAIR", LandmarkSubtype::OCTAGON_IMAGE_REPAIR,
+     LandmarkType::OCTAGON},
+    {"OCTAGON_IMAGE_RESCUE", LandmarkSubtype::OCTAGON_IMAGE_RESCUE,
+     LandmarkType::OCTAGON},
+    {"OCTAGON_IMAGE_SEARCH", LandmarkSubtype::OCTAGON_IMAGE_SEARCH,
+     LandmarkType::OCTAGON},
+    {"OCTAGON_IMAGE_SURVEY", LandmarkSubtype::OCTAGON_IMAGE_SURVEY,
+     LandmarkType::OCTAGON},
+    {"PINGER_DEPLOY", LandmarkSubtype::PINGER_DEPLOY, LandmarkType::PINGER},
+    {"PINGER_RESTORE", LandmarkSubtype::PINGER_RESTORE, LandmarkType::PINGER},
+};
+
+namespace detail {
+
+template <typename Entry, std::size_t N>
+constexpr std::optional<std::uint16_t> value_of(const Entry (&table)[N],
+                                                std::string_view name) {
+    for (const auto& entry : table) {
+        if (entry.name == name) {
+            return entry.value;
+        }
+    }
+    return std::nullopt;
+}
+
+}  // namespace detail
+
+/// Value of the LandmarkType constant with this name, e.g. "GATE".
+constexpr std::optional<std::uint16_t> landmark_type_from_string(
+    std::string_view name) {
+    return detail::value_of(kLandmarkTypes, name);
+}
+
+/// Value of the LandmarkSubtype constant with this name.
+constexpr std::optional<std::uint16_t> landmark_subtype_from_string(
+    std::string_view name) {
+    return detail::value_of(kLandmarkSubtypes, name);
+}
+
+}  // namespace vortex::utils::landmarks
+
+#endif  // VORTEX_UTILS_ROS_LANDMARK_NAMES_HPP
diff --git a/vortex_utils_ros/include/vortex/utils/ros/waypoint_ros_conversions.hpp b/vortex_utils_ros/include/vortex/utils/ros/waypoint_ros_conversions.hpp
@@ -96,6 +96,9 @@ inline vortex::utils::types::Waypoint waypoint_from_ros(
     wp.keep_altitude = ros_wp.keep_altitude;
     wp.desired_altitude = ros_wp.desired_altitude;
     wp.require_altitude_convergence = ros_wp.require_altitude_convergence;
+    wp.position_tolerance = ros_wp.position_tolerance;
+    wp.orientation_tolerance = ros_wp.orientation_tolerance;
+    wp.hold_time_sec = ros_wp.hold_time_sec;
     return wp;
 }