From 8a87ee0f169f305ac71f5d47e292f00ec5b229e6 Mon Sep 17 00:00:00 2001 From: Hinthujan Thiagarajah Date: Mon, 5 Oct 2026 21:12:47 +0200 Subject: [PATCH 1/3] 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/3] 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/3] 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};