Feat/waypoint manager reference filter

dependencies.repos

@@ -2,3 +2,4 @@ repositories:
   vortex-msgs:
     type: git
     url: https://github.com/vortexntnu/vortex-msgs.git
+    version: development

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 {

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.

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};

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;
 }