From 8a87ee0f169f305ac71f5d47e292f00ec5b229e6 Mon Sep 17 00:00:00 2001 From: Hinthujan Thiagarajah Date: Mon, 5 Oct 2026 21:12:47 +0200 Subject: [PATCH 1/7] Separate position and orientation convergence tolerances - controlled_error(): position and orientation error over the DOFs a waypoint mode controls. - has_converged() overload with ConvergenceTolerance; the threshold version keeps its results. - Waypoint gets convergence_threshold, position_tolerance, orientation_tolerance and hold_time_sec, converted from the message. - dependencies.repos: vortex-msgs from development. --- dependencies.repos | 1 + vortex_utils/include/vortex/utils/types.hpp | 4 + .../include/vortex/utils/waypoint_utils.hpp | 36 ++++++++ vortex_utils/src/waypoint_utils.cpp | 91 +++++++++++++------ .../utils/ros/waypoint_ros_conversions.hpp | 4 + 5 files changed, 106 insertions(+), 30 deletions(-) diff --git a/dependencies.repos b/dependencies.repos index 4475316..0379f46 100644 --- 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 index baadd2b..c3f8f7a 100644 --- a/vortex_utils/include/vortex/utils/types.hpp +++ b/vortex_utils/include/vortex/utils/types.hpp @@ -467,6 +467,10 @@ struct Waypoint { bool keep_altitude{false}; double desired_altitude{0.0}; bool require_altitude_convergence{false}; + double convergence_threshold{0.0}; + 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 index 0b6ee0c..3b6f109 100644 --- a/vortex_utils/include/vortex/utils/waypoint_utils.hpp +++ b/vortex_utils/include/vortex/utils/waypoint_utils.hpp @@ -70,6 +70,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 +114,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. * diff --git a/vortex_utils/src/waypoint_utils.cpp b/vortex_utils/src/waypoint_utils.cpp index 318a746..d2f2230 100644 --- a/vortex_utils/src/waypoint_utils.cpp +++ b/vortex_utils/src/waypoint_utils.cpp @@ -236,42 +236,73 @@ 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 { + +/// Squared position and orientation error over the DOFs controlled by @p mode. +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()}; + } +} - return err < convergence_threshold; +} // namespace + +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) { 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 index 4ad731a..75013fb 100644 --- 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,10 @@ 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.convergence_threshold = ros_wp.convergence_threshold; + 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; } From 3207627818249f8393a0c49ea2d5e991a35549cb Mon Sep 17 00:00:00 2001 From: Hinthujan Thiagarajah Date: Tue, 6 Oct 2026 13:01:59 +0200 Subject: [PATCH 2/7] Tolerances replace convergence_threshold on waypoints - Waypoint loses convergence_threshold and its conversion. - WaypointGoal: position_tolerance and orientation_tolerance instead of convergence_threshold; YAML keys position_tolerance and orientation_tolerance_deg. --- vortex_utils/include/vortex/utils/types.hpp | 1 - .../include/vortex/utils/waypoint_utils.hpp | 12 +++++++----- vortex_utils/src/waypoint_utils.cpp | 14 ++++++++++---- .../vortex/utils/ros/waypoint_ros_conversions.hpp | 1 - 4 files changed, 17 insertions(+), 11 deletions(-) diff --git a/vortex_utils/include/vortex/utils/types.hpp b/vortex_utils/include/vortex/utils/types.hpp index c3f8f7a..cdadaf6 100644 --- a/vortex_utils/include/vortex/utils/types.hpp +++ b/vortex_utils/include/vortex/utils/types.hpp @@ -467,7 +467,6 @@ struct Waypoint { bool keep_altitude{false}; double desired_altitude{0.0}; bool require_altitude_convergence{false}; - double convergence_threshold{0.0}; double position_tolerance{0.0}; double orientation_tolerance{0.0}; double hold_time_sec{0.0}; diff --git a/vortex_utils/include/vortex/utils/waypoint_utils.hpp b/vortex_utils/include/vortex/utils/waypoint_utils.hpp index 3b6f109..ebc0649 100644 --- 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}; @@ -169,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 index d2f2230..596e5b2 100644 --- a/vortex_utils/src/waypoint_utils.cpp +++ b/vortex_utils/src/waypoint_utils.cpp @@ -365,9 +365,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 position_tolerance = 0.0; + if (wp["position_tolerance"]) { + position_tolerance = wp["position_tolerance"].as(); + } + double orientation_tolerance = 0.0; + if (wp["orientation_tolerance_deg"]) { + orientation_tolerance = + wp["orientation_tolerance_deg"].as() * (M_PI / 180.0); } bool keep_altitude = false; @@ -392,7 +397,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/waypoint_ros_conversions.hpp b/vortex_utils_ros/include/vortex/utils/ros/waypoint_ros_conversions.hpp index 75013fb..65e5c0c 100644 --- 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,7 +96,6 @@ 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.convergence_threshold = ros_wp.convergence_threshold; wp.position_tolerance = ros_wp.position_tolerance; wp.orientation_tolerance = ros_wp.orientation_tolerance; wp.hold_time_sec = ros_wp.hold_time_sec; From 33118c4da5e0a237803f622abe22da83a452d891 Mon Sep 17 00:00:00 2001 From: Hinthujan Thiagarajah Date: Tue, 6 Oct 2026 13:05:19 +0200 Subject: [PATCH 3/7] Remove comments that repeat the code --- vortex_utils/src/waypoint_utils.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/vortex_utils/src/waypoint_utils.cpp b/vortex_utils/src/waypoint_utils.cpp index 596e5b2..d8510b0 100644 --- a/vortex_utils/src/waypoint_utils.cpp +++ b/vortex_utils/src/waypoint_utils.cpp @@ -238,7 +238,6 @@ Pose compute_waypoint_goal(const Pose& incoming_waypoint, namespace { -/// Squared position and orientation error over the DOFs controlled by @p mode. struct ControlledErrorSq { double position{0.0}; double orientation{0.0}; From 54d55cd7975d175e081914bfefcc64b91906b3b6 Mon Sep 17 00:00:00 2001 From: Hinthujan Thiagarajah Date: Mon, 12 Oct 2026 17:58:57 +0200 Subject: [PATCH 4/7] Landmark type and subtype values from their constant names --- .../vortex/utils/ros/landmark_names.hpp | 113 ++++++++++++++++++ 1 file changed, 113 insertions(+) create mode 100644 vortex_utils_ros/include/vortex/utils/ros/landmark_names.hpp diff --git a/vortex_utils_ros/include/vortex/utils/ros/landmark_names.hpp b/vortex_utils_ros/include/vortex/utils/ros/landmark_names.hpp new file mode 100644 index 0000000..7a41e6b --- /dev/null +++ b/vortex_utils_ros/include/vortex/utils/ros/landmark_names.hpp @@ -0,0 +1,113 @@ +#ifndef VORTEX_UTILS_ROS_LANDMARK_NAMES_HPP +#define VORTEX_UTILS_ROS_LANDMARK_NAMES_HPP + +#include +#include +#include +#include + +#include +#include + +namespace vortex::utils::landmarks { + +namespace detail { + +using NameValue = std::pair; +using vortex_msgs::msg::LandmarkSubtype; +using vortex_msgs::msg::LandmarkType; + +// Add new constants from LandmarkType.msg and LandmarkSubtype.msg here. +inline constexpr NameValue kTypes[] = { + {"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 NameValue kSubtypes[] = { + {"ARUCO_BOARD_CAMERA", LandmarkSubtype::ARUCO_BOARD_CAMERA}, + {"ARUCO_BOARD_SONAR", LandmarkSubtype::ARUCO_BOARD_SONAR}, + {"ARUCO_BOARD_DETECTION", LandmarkSubtype::ARUCO_BOARD_DETECTION}, + {"VALVE_VERTICAL", LandmarkSubtype::VALVE_VERTICAL}, + {"VALVE_HORIZONTAL", LandmarkSubtype::VALVE_HORIZONTAL}, + {"PIPELINE_START_CAMERA", LandmarkSubtype::PIPELINE_START_CAMERA}, + {"PIPELINE_START_SONAR", LandmarkSubtype::PIPELINE_START_SONAR}, + {"GATE_SEARCH_RESCUE", LandmarkSubtype::GATE_SEARCH_RESCUE}, + {"GATE_SURVEY_REPAIR", LandmarkSubtype::GATE_SURVEY_REPAIR}, + {"SLALOM_PIPE_WHITE", LandmarkSubtype::SLALOM_PIPE_WHITE}, + {"SLALOM_PIPE_RED", LandmarkSubtype::SLALOM_PIPE_RED}, + {"TORPEDO_BOARD_WHOLE", LandmarkSubtype::TORPEDO_BOARD_WHOLE}, + {"TORPEDO_TARGET_LARGE_SEARCH_RESCUE", + LandmarkSubtype::TORPEDO_TARGET_LARGE_SEARCH_RESCUE}, + {"TORPEDO_TARGET_LARGE_SURVEY_REPAIR", + LandmarkSubtype::TORPEDO_TARGET_LARGE_SURVEY_REPAIR}, + {"TORPEDO_TARGET_SMALL_SEARCH_RESCUE", + LandmarkSubtype::TORPEDO_TARGET_SMALL_SEARCH_RESCUE}, + {"TORPEDO_TARGET_SMALL_SURVEY_REPAIR", + LandmarkSubtype::TORPEDO_TARGET_SMALL_SURVEY_REPAIR}, + {"BIN_SEARCH_RESCUE", LandmarkSubtype::BIN_SEARCH_RESCUE}, + {"BIN_SURVEY_REPAIR", LandmarkSubtype::BIN_SURVEY_REPAIR}, + {"GATE_WHOLE", LandmarkSubtype::GATE_WHOLE}, + {"GATE_POLE_EDGE", LandmarkSubtype::GATE_POLE_EDGE}, + {"GATE_POLE_MIDDLE", LandmarkSubtype::GATE_POLE_MIDDLE}, + {"TORPEDO_ICON_FIRE", LandmarkSubtype::TORPEDO_ICON_FIRE}, + {"TORPEDO_ICON_BLOOD", LandmarkSubtype::TORPEDO_ICON_BLOOD}, + {"TORPEDO_ICON_FIRETRUCK", LandmarkSubtype::TORPEDO_ICON_FIRETRUCK}, + {"TORPEDO_ICON_AMBULANCE", LandmarkSubtype::TORPEDO_ICON_AMBULANCE}, + {"BIN_UNCLASSIFIED", LandmarkSubtype::BIN_UNCLASSIFIED}, + {"BIN_STRUCTURE", LandmarkSubtype::BIN_STRUCTURE}, + {"PATH_MARKER_WHOLE", LandmarkSubtype::PATH_MARKER_WHOLE}, + {"TABLE_WHOLE", LandmarkSubtype::TABLE_WHOLE}, + {"TABLE_ITEM_NUTBOLT", LandmarkSubtype::TABLE_ITEM_NUTBOLT}, + {"TABLE_ITEM_ELECTRIC", LandmarkSubtype::TABLE_ITEM_ELECTRIC}, + {"TABLE_ITEM_PILL", LandmarkSubtype::TABLE_ITEM_PILL}, + {"TABLE_ITEM_BANDAID", LandmarkSubtype::TABLE_ITEM_BANDAID}, + {"TABLE_BASKET_SURVEY_REPAIR", LandmarkSubtype::TABLE_BASKET_SURVEY_REPAIR}, + {"TABLE_BASKET_SEARCH_RESCUE", LandmarkSubtype::TABLE_BASKET_SEARCH_RESCUE}, + {"OCTAGON_WHOLE", LandmarkSubtype::OCTAGON_WHOLE}, + {"OCTAGON_IMAGE_REPAIR", LandmarkSubtype::OCTAGON_IMAGE_REPAIR}, + {"OCTAGON_IMAGE_RESCUE", LandmarkSubtype::OCTAGON_IMAGE_RESCUE}, + {"OCTAGON_IMAGE_SEARCH", LandmarkSubtype::OCTAGON_IMAGE_SEARCH}, + {"OCTAGON_IMAGE_SURVEY", LandmarkSubtype::OCTAGON_IMAGE_SURVEY}, + {"PINGER_DEPLOY", LandmarkSubtype::PINGER_DEPLOY}, + {"PINGER_RESTORE", LandmarkSubtype::PINGER_RESTORE}, +}; + +template +constexpr std::optional value_of(const NameValue (&table)[N], + std::string_view name) { + for (const auto& [key, value] : table) { + if (key == name) { + return value; + } + } + return std::nullopt; +} + +} // namespace detail + +/// Value of the LandmarkType constant with this name, e.g. "GATE". +constexpr std::optional landmark_type_from_string( + std::string_view name) { + return detail::value_of(detail::kTypes, name); +} + +/// Value of the LandmarkSubtype constant with this name. +constexpr std::optional landmark_subtype_from_string( + std::string_view name) { + return detail::value_of(detail::kSubtypes, name); +} + +} // namespace vortex::utils::landmarks + +#endif // VORTEX_UTILS_ROS_LANDMARK_NAMES_HPP From 7402504418ba49be9302e021e326981c67511533 Mon Sep 17 00:00:00 2001 From: Hinthujan Thiagarajah Date: Tue, 13 Oct 2026 11:31:34 +0200 Subject: [PATCH 5/7] Landmark names: drop gate whole and gate pole --- vortex_utils_ros/include/vortex/utils/ros/landmark_names.hpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/vortex_utils_ros/include/vortex/utils/ros/landmark_names.hpp b/vortex_utils_ros/include/vortex/utils/ros/landmark_names.hpp index 7a41e6b..6ad6164 100644 --- a/vortex_utils_ros/include/vortex/utils/ros/landmark_names.hpp +++ b/vortex_utils_ros/include/vortex/utils/ros/landmark_names.hpp @@ -57,9 +57,6 @@ inline constexpr NameValue kSubtypes[] = { LandmarkSubtype::TORPEDO_TARGET_SMALL_SURVEY_REPAIR}, {"BIN_SEARCH_RESCUE", LandmarkSubtype::BIN_SEARCH_RESCUE}, {"BIN_SURVEY_REPAIR", LandmarkSubtype::BIN_SURVEY_REPAIR}, - {"GATE_WHOLE", LandmarkSubtype::GATE_WHOLE}, - {"GATE_POLE_EDGE", LandmarkSubtype::GATE_POLE_EDGE}, - {"GATE_POLE_MIDDLE", LandmarkSubtype::GATE_POLE_MIDDLE}, {"TORPEDO_ICON_FIRE", LandmarkSubtype::TORPEDO_ICON_FIRE}, {"TORPEDO_ICON_BLOOD", LandmarkSubtype::TORPEDO_ICON_BLOOD}, {"TORPEDO_ICON_FIRETRUCK", LandmarkSubtype::TORPEDO_ICON_FIRETRUCK}, From cf1d7456cc9f671c3a30fa421af22caee35aa392 Mon Sep 17 00:00:00 2001 From: Hinthujan Thiagarajah Date: Thu, 22 Oct 2026 09:05:24 +0200 Subject: [PATCH 6/7] Landmark names: torpedo openings instead of icons and role targets --- .../include/vortex/utils/ros/landmark_names.hpp | 16 ++++------------ 1 file changed, 4 insertions(+), 12 deletions(-) diff --git a/vortex_utils_ros/include/vortex/utils/ros/landmark_names.hpp b/vortex_utils_ros/include/vortex/utils/ros/landmark_names.hpp index 6ad6164..0253475 100644 --- a/vortex_utils_ros/include/vortex/utils/ros/landmark_names.hpp +++ b/vortex_utils_ros/include/vortex/utils/ros/landmark_names.hpp @@ -47,20 +47,12 @@ inline constexpr NameValue kSubtypes[] = { {"SLALOM_PIPE_WHITE", LandmarkSubtype::SLALOM_PIPE_WHITE}, {"SLALOM_PIPE_RED", LandmarkSubtype::SLALOM_PIPE_RED}, {"TORPEDO_BOARD_WHOLE", LandmarkSubtype::TORPEDO_BOARD_WHOLE}, - {"TORPEDO_TARGET_LARGE_SEARCH_RESCUE", - LandmarkSubtype::TORPEDO_TARGET_LARGE_SEARCH_RESCUE}, - {"TORPEDO_TARGET_LARGE_SURVEY_REPAIR", - LandmarkSubtype::TORPEDO_TARGET_LARGE_SURVEY_REPAIR}, - {"TORPEDO_TARGET_SMALL_SEARCH_RESCUE", - LandmarkSubtype::TORPEDO_TARGET_SMALL_SEARCH_RESCUE}, - {"TORPEDO_TARGET_SMALL_SURVEY_REPAIR", - LandmarkSubtype::TORPEDO_TARGET_SMALL_SURVEY_REPAIR}, + {"TORPEDO_OPENING_LARGE_LEFT", LandmarkSubtype::TORPEDO_OPENING_LARGE_LEFT}, + {"TORPEDO_OPENING_LARGE_RIGHT", LandmarkSubtype::TORPEDO_OPENING_LARGE_RIGHT}, + {"TORPEDO_OPENING_SMALL_TOP", LandmarkSubtype::TORPEDO_OPENING_SMALL_TOP}, + {"TORPEDO_OPENING_SMALL_BOTTOM", LandmarkSubtype::TORPEDO_OPENING_SMALL_BOTTOM}, {"BIN_SEARCH_RESCUE", LandmarkSubtype::BIN_SEARCH_RESCUE}, {"BIN_SURVEY_REPAIR", LandmarkSubtype::BIN_SURVEY_REPAIR}, - {"TORPEDO_ICON_FIRE", LandmarkSubtype::TORPEDO_ICON_FIRE}, - {"TORPEDO_ICON_BLOOD", LandmarkSubtype::TORPEDO_ICON_BLOOD}, - {"TORPEDO_ICON_FIRETRUCK", LandmarkSubtype::TORPEDO_ICON_FIRETRUCK}, - {"TORPEDO_ICON_AMBULANCE", LandmarkSubtype::TORPEDO_ICON_AMBULANCE}, {"BIN_UNCLASSIFIED", LandmarkSubtype::BIN_UNCLASSIFIED}, {"BIN_STRUCTURE", LandmarkSubtype::BIN_STRUCTURE}, {"PATH_MARKER_WHOLE", LandmarkSubtype::PATH_MARKER_WHOLE}, From 8a500f1b9b8e0c76d7b7c2f918da0a2a9965f01a Mon Sep 17 00:00:00 2001 From: Hinthujan Thiagarajah Date: Thu, 22 Oct 2026 09:13:49 +0200 Subject: [PATCH 7/7] Landmark names: each subtype with its type, tables public kLandmarkTypes and kLandmarkSubtypes (with the LandmarkType each subtype belongs to) can be listed, so tools like the simulator use this one table instead of a copy. landmark_type_from_string and landmark_subtype_from_string are unchanged. --- .../vortex/utils/ros/landmark_names.hpp | 132 ++++++++++++------ 1 file changed, 86 insertions(+), 46 deletions(-) diff --git a/vortex_utils_ros/include/vortex/utils/ros/landmark_names.hpp b/vortex_utils_ros/include/vortex/utils/ros/landmark_names.hpp index 0253475..7e4980f 100644 --- a/vortex_utils_ros/include/vortex/utils/ros/landmark_names.hpp +++ b/vortex_utils_ros/include/vortex/utils/ros/landmark_names.hpp @@ -11,14 +11,24 @@ namespace vortex::utils::landmarks { -namespace detail { +/// 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 NameValue = std::pair; using vortex_msgs::msg::LandmarkSubtype; using vortex_msgs::msg::LandmarkType; // Add new constants from LandmarkType.msg and LandmarkSubtype.msg here. -inline constexpr NameValue kTypes[] = { +inline constexpr LandmarkTypeName kLandmarkTypes[] = { {"ARUCO_MARKER", LandmarkType::ARUCO_MARKER}, {"ARUCO_BOARD", LandmarkType::ARUCO_BOARD}, {"PIPELINE_START", LandmarkType::PIPELINE_START}, @@ -34,50 +44,80 @@ inline constexpr NameValue kTypes[] = { {"PINGER", LandmarkType::PINGER}, }; -inline constexpr NameValue kSubtypes[] = { - {"ARUCO_BOARD_CAMERA", LandmarkSubtype::ARUCO_BOARD_CAMERA}, - {"ARUCO_BOARD_SONAR", LandmarkSubtype::ARUCO_BOARD_SONAR}, - {"ARUCO_BOARD_DETECTION", LandmarkSubtype::ARUCO_BOARD_DETECTION}, - {"VALVE_VERTICAL", LandmarkSubtype::VALVE_VERTICAL}, - {"VALVE_HORIZONTAL", LandmarkSubtype::VALVE_HORIZONTAL}, - {"PIPELINE_START_CAMERA", LandmarkSubtype::PIPELINE_START_CAMERA}, - {"PIPELINE_START_SONAR", LandmarkSubtype::PIPELINE_START_SONAR}, - {"GATE_SEARCH_RESCUE", LandmarkSubtype::GATE_SEARCH_RESCUE}, - {"GATE_SURVEY_REPAIR", LandmarkSubtype::GATE_SURVEY_REPAIR}, - {"SLALOM_PIPE_WHITE", LandmarkSubtype::SLALOM_PIPE_WHITE}, - {"SLALOM_PIPE_RED", LandmarkSubtype::SLALOM_PIPE_RED}, - {"TORPEDO_BOARD_WHOLE", LandmarkSubtype::TORPEDO_BOARD_WHOLE}, - {"TORPEDO_OPENING_LARGE_LEFT", LandmarkSubtype::TORPEDO_OPENING_LARGE_LEFT}, - {"TORPEDO_OPENING_LARGE_RIGHT", LandmarkSubtype::TORPEDO_OPENING_LARGE_RIGHT}, - {"TORPEDO_OPENING_SMALL_TOP", LandmarkSubtype::TORPEDO_OPENING_SMALL_TOP}, - {"TORPEDO_OPENING_SMALL_BOTTOM", LandmarkSubtype::TORPEDO_OPENING_SMALL_BOTTOM}, - {"BIN_SEARCH_RESCUE", LandmarkSubtype::BIN_SEARCH_RESCUE}, - {"BIN_SURVEY_REPAIR", LandmarkSubtype::BIN_SURVEY_REPAIR}, - {"BIN_UNCLASSIFIED", LandmarkSubtype::BIN_UNCLASSIFIED}, - {"BIN_STRUCTURE", LandmarkSubtype::BIN_STRUCTURE}, - {"PATH_MARKER_WHOLE", LandmarkSubtype::PATH_MARKER_WHOLE}, - {"TABLE_WHOLE", LandmarkSubtype::TABLE_WHOLE}, - {"TABLE_ITEM_NUTBOLT", LandmarkSubtype::TABLE_ITEM_NUTBOLT}, - {"TABLE_ITEM_ELECTRIC", LandmarkSubtype::TABLE_ITEM_ELECTRIC}, - {"TABLE_ITEM_PILL", LandmarkSubtype::TABLE_ITEM_PILL}, - {"TABLE_ITEM_BANDAID", LandmarkSubtype::TABLE_ITEM_BANDAID}, - {"TABLE_BASKET_SURVEY_REPAIR", LandmarkSubtype::TABLE_BASKET_SURVEY_REPAIR}, - {"TABLE_BASKET_SEARCH_RESCUE", LandmarkSubtype::TABLE_BASKET_SEARCH_RESCUE}, - {"OCTAGON_WHOLE", LandmarkSubtype::OCTAGON_WHOLE}, - {"OCTAGON_IMAGE_REPAIR", LandmarkSubtype::OCTAGON_IMAGE_REPAIR}, - {"OCTAGON_IMAGE_RESCUE", LandmarkSubtype::OCTAGON_IMAGE_RESCUE}, - {"OCTAGON_IMAGE_SEARCH", LandmarkSubtype::OCTAGON_IMAGE_SEARCH}, - {"OCTAGON_IMAGE_SURVEY", LandmarkSubtype::OCTAGON_IMAGE_SURVEY}, - {"PINGER_DEPLOY", LandmarkSubtype::PINGER_DEPLOY}, - {"PINGER_RESTORE", LandmarkSubtype::PINGER_RESTORE}, +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}, }; -template -constexpr std::optional value_of(const NameValue (&table)[N], +namespace detail { + +template +constexpr std::optional value_of(const Entry (&table)[N], std::string_view name) { - for (const auto& [key, value] : table) { - if (key == name) { - return value; + for (const auto& entry : table) { + if (entry.name == name) { + return entry.value; } } return std::nullopt; @@ -88,13 +128,13 @@ constexpr std::optional value_of(const NameValue (&table)[N], /// Value of the LandmarkType constant with this name, e.g. "GATE". constexpr std::optional landmark_type_from_string( std::string_view name) { - return detail::value_of(detail::kTypes, name); + return detail::value_of(kLandmarkTypes, name); } /// Value of the LandmarkSubtype constant with this name. constexpr std::optional landmark_subtype_from_string( std::string_view name) { - return detail::value_of(detail::kSubtypes, name); + return detail::value_of(kLandmarkSubtypes, name); } } // namespace vortex::utils::landmarks