Feat/auto refactor

guidance/reference_filter_dp/README.md

@@ -44,7 +44,9 @@ Each waypoint has a mode that determines which degrees of freedom are controlled
 | `FORWARD_HEADING` | x, y, z + yaw toward target (roll/pitch = 0) | Euclidean norm of position error and yaw error |
 | `ONLY_ORIENTATION` | roll, pitch, yaw (position holds current value) | Euclidean norm of angle errors |
 
-For all modes, convergence is reached when the error metric drops below the `convergence_threshold` specified in the action goal.
+For all modes, convergence is reached when the position error norm is below `position_tolerance` and the orientation error norm is below `orientation_tolerance` of the goal waypoint. A tolerance <= 0 is not checked, and a tolerance for a part the mode does not control (for example `orientation_tolerance` with `ONLY_POSITION`) has no effect, so a goal whose controlled tolerances are all <= 0 counts as converged immediately.
+
+The tolerances must hold continuously for the waypoint's `hold_time_sec` (default 0, converge at once) before the goal succeeds. A new goal sent while another is executing keeps the filter state and only changes the target and tolerances; the first goal after startup starts from the measured pose and twist.
 
 ## Action Server
 

guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_follower.hpp

@@ -2,6 +2,7 @@
 #define REFERENCE_FILTER_DP__LIB__WAYPOINT_FOLLOWER_HPP_
 
 #include <mutex>
+#include <optional>
 #include <vortex/utils/types.hpp>
 #include "reference_filter_dp/lib/eigen_typedefs.hpp"
 #include "reference_filter_dp/lib/reference_filter.hpp"
@@ -26,12 +27,22 @@ class WaypointFollower {
      * @param pose Current vehicle pose.
      * @param twist Current vehicle twist (body frame).
      * @param waypoint Target waypoint with mode.
-     * @param convergence_threshold Max error norm to consider target reached.
+     * @param tolerance Position and orientation tolerances to consider target
+     * reached.
      */
     void start(const PoseEuler& pose,
                const Twist& twist,
                const Waypoint& waypoint,
-               double convergence_threshold);
+               const ConvergenceTolerance& tolerance);
+
+    /**
+     * @brief Change the target and tolerances while keeping the filter state.
+     * @param waypoint New target waypoint with mode.
+     * @param tolerance Position and orientation tolerances to consider target
+     * reached.
+     */
+    void retarget(const Waypoint& waypoint,
+                  const ConvergenceTolerance& tolerance);
 
     /**
      * @brief Advance the filter by one time step.
@@ -42,10 +53,25 @@ class WaypointFollower {
     /**
      * @brief Check if the measured pose has converged to the reference goal.
      * @param measured_pose Current measured pose.
-     * @return True if the error norm is within the convergence threshold.
+     * @return True if the error norm is within the tolerances.
      */
     bool within_convergance(const Eigen::Vector6d& measured_pose) const;
 
+    /**
+     * @brief Check convergence, requiring the tolerances to hold continuously.
+     *
+     * The hold timer resets whenever the pose leaves the tolerances, and on
+     * start() and retarget().
+     * @param measured_pose Current measured pose.
+     * @param t_sec Current time in seconds.
+     * @param hold_time_sec Time the tolerances must hold; <= 0 converges on
+     * the first sample within the tolerances.
+     * @return True if the tolerances have held for hold_time_sec.
+     */
+    bool update_convergence(const Eigen::Vector6d& measured_pose,
+                            double t_sec,
+                            double hold_time_sec);
+
     /**
      * @brief Update the reference goal pose mid-sequence.
      * @param reference_goal_pose The new reference pose.
@@ -66,6 +92,9 @@ class WaypointFollower {
     Eigen::Vector6d reference() const;
 
    private:
+    /// @brief Convergence check for callers that already hold the mutex.
+    bool within_locked(const Eigen::Vector6d& measured_pose) const;
+
     Eigen::Vector18d compute_initial_state(const PoseEuler& pose,
                                            const Twist& twist);
 
@@ -75,7 +104,8 @@ class WaypointFollower {
     Eigen::Vector18d state_ = Eigen::Vector18d::Zero();
     Eigen::Vector6d reference_goal_ = Eigen::Vector6d::Zero();
     WaypointMode waypoint_mode_{WaypointMode::FULL_POSE};
-    double convergence_threshold_{0.1};
+    ConvergenceTolerance tolerance_{};
+    std::optional<double> inside_since_sec_;
 };
 
 }  // namespace vortex::guidance

guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_types.hpp

@@ -3,10 +3,13 @@
 
 #include <cstdint>
 #include <vortex/utils/types.hpp>
+#include <vortex/utils/waypoint_utils.hpp>
 
 namespace vortex::guidance {
 
 using vortex::utils::types::PoseEuler;
+using vortex::utils::waypoints::ControlledError;
+using vortex::utils::waypoints::ConvergenceTolerance;
 
 /**
  * @brief Determines which degrees of freedom the reference filter controls.

guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_utils.hpp

@@ -24,18 +24,20 @@ Eigen::Vector6d apply_mode_logic(const Eigen::Vector6d& r_in,
 /**
  * @brief Check whether the measured pose has converged to the reference.
  *
- * Only the DOFs relevant to the waypoint mode are included in the error norm.
+ * Only the DOFs relevant to the waypoint mode are included in the position
+ * and orientation error norms. A tolerance <= 0 is not checked.
  *
  * @param measured_pose The current measured pose (6D).
  * @param reference The reference goal pose (6D).
  * @param mode The waypoint mode.
- * @param convergence_threshold The maximum allowed error norm.
- * @return True if the error is below the threshold.
+ * @param tolerance The maximum allowed position [m] and orientation [rad]
+ * error norms.
+ * @return True if both errors are below their tolerances.
  */
 bool has_converged(const Eigen::Vector6d& measured_pose,
                    const Eigen::Vector6d& reference,
                    WaypointMode mode,
-                   double convergence_threshold);
+                   const ConvergenceTolerance& tolerance);
 
 }  // namespace vortex::guidance
 

guidance/reference_filter_dp/include/reference_filter_dp/ros/reference_filter_ros.hpp

@@ -55,7 +55,8 @@ class ReferenceFilterNode : public rclcpp::Node {
      * @param goal_handle The goal handle.
      */
     void execute(const std::shared_ptr<rclcpp_action::ServerGoalHandle<
-                     vortex_msgs::action::GuidanceWaypoint>> goal_handle);
+                     vortex_msgs::action::GuidanceWaypoint>> goal_handle,
+                 bool retarget);
 
     rclcpp_action::Server<vortex_msgs::action::GuidanceWaypoint>::SharedPtr
         action_server_;
@@ -87,6 +88,7 @@ class ReferenceFilterNode : public rclcpp::Node {
     std::mutex sensor_mutex_;
 
     std::atomic<bool> preempted_{false};
+    std::atomic<bool> executing_{false};
     std::mutex execute_mutex_;
     std::thread execute_thread_;
 };

guidance/reference_filter_dp/src/lib/waypoint_follower.cpp

@@ -12,11 +12,22 @@ WaypointFollower::WaypointFollower(const ReferenceFilterParams& params,
 void WaypointFollower::start(const PoseEuler& pose,
                              const Twist& twist,
                              const Waypoint& waypoint,
-                             double convergence_threshold) {
+                             const ConvergenceTolerance& tolerance) {
     std::lock_guard<std::mutex> lock(mutex_);
     state_ = compute_initial_state(pose, twist);
     waypoint_mode_ = waypoint.mode;
-    convergence_threshold_ = convergence_threshold;
+    tolerance_ = tolerance;
+    inside_since_sec_.reset();
+    reference_goal_ = apply_mode_logic(waypoint.pose.to_vector(),
+                                       waypoint_mode_, state_.head<6>());
+}
+
+void WaypointFollower::retarget(const Waypoint& waypoint,
+                                const ConvergenceTolerance& tolerance) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    waypoint_mode_ = waypoint.mode;
+    tolerance_ = tolerance;
+    inside_since_sec_.reset();
     reference_goal_ = apply_mode_logic(waypoint.pose.to_vector(),
                                        waypoint_mode_, state_.head<6>());
 }
@@ -42,11 +53,30 @@ Eigen::Vector18d WaypointFollower::step() {
     return state_;
 }
 
+bool WaypointFollower::within_locked(
+    const Eigen::Vector6d& measured_pose) const {
+    return has_converged(measured_pose, reference_goal_, waypoint_mode_,
+                         tolerance_);
+}
+
 bool WaypointFollower::within_convergance(
     const Eigen::Vector6d& measured_pose) const {
     std::lock_guard<std::mutex> lock(mutex_);
-    return has_converged(measured_pose, reference_goal_, waypoint_mode_,
-                         convergence_threshold_);
+    return within_locked(measured_pose);
+}
+
+bool WaypointFollower::update_convergence(const Eigen::Vector6d& measured_pose,
+                                          double t_sec,
+                                          double hold_time_sec) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (!within_locked(measured_pose)) {
+        inside_since_sec_.reset();
+        return false;
+    }
+    if (!inside_since_sec_) {
+        inside_since_sec_ = t_sec;
+    }
+    return t_sec - *inside_since_sec_ >= hold_time_sec;
 }
 
 void WaypointFollower::set_reference(const PoseEuler& reference_goal_pose) {

guidance/reference_filter_dp/src/lib/waypoint_utils.cpp

@@ -82,7 +82,7 @@ Eigen::Vector6d apply_mode_logic(const Eigen::Vector6d& reference_in,
 bool has_converged(const Eigen::Vector6d& measured_pose,
                    const Eigen::Vector6d& reference,
                    WaypointMode mode,
-                   double convergence_threshold) {
+                   const ConvergenceTolerance& tolerance) {
     using vortex::utils::math::ssa;
     const Eigen::Vector3d ep = measured_pose.head<3>() - reference.head<3>();
 
@@ -91,27 +91,30 @@ bool has_converged(const Eigen::Vector6d& measured_pose,
     ea(1) = ssa(measured_pose(4) - reference(4));
     ea(2) = ssa(measured_pose(5) - reference(5));
 
-    const double err = [&] {
+    const ControlledError err = [&]() -> ControlledError {
         switch (mode) {
             case WaypointMode::ONLY_POSITION:
-                return ep.norm();
+                return {ep.norm(), 0.0};
             case WaypointMode::ONLY_ORIENTATION:
-                return ea.norm();
+                return {0.0, 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));
+                return {ep.norm(), std::abs(ea(2))};
             case WaypointMode::XY_AND_YAW:
-                return std::sqrt(ep.head<2>().squaredNorm() + ea(2) * ea(2));
+                return {ep.head<2>().norm(), std::abs(ea(2))};
             case WaypointMode::XY_FORWARD_DIR:
-                return ep.head<2>().norm();
+                return {ep.head<2>().norm(), 0.0};
             case WaypointMode::FULL_POSE:
             default:
-                return std::sqrt(ep.squaredNorm() + ea.squaredNorm());
+                return {ep.norm(), ea.norm()};
         }
     }();
 
-    return err < convergence_threshold;
+    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;
 }
 
 }  // namespace vortex::guidance

guidance/reference_filter_dp/src/ros/reference_filter_ros.cpp

@@ -18,6 +18,10 @@ const auto start_message = R"(
 
 namespace vortex::guidance {
 
+namespace {
+constexpr double kDefaultTolerance = 0.1;
+}  // namespace
+
 ReferenceFilterNode::ReferenceFilterNode(const rclcpp::NodeOptions& options)
     : Node("reference_filter_node", options) {
     int time_step_ms = this->declare_parameter<int>("time_step_ms");
@@ -137,29 +141,33 @@ void ReferenceFilterNode::handle_accepted(
         rclcpp_action::ServerGoalHandle<vortex_msgs::action::GuidanceWaypoint>>
         goal_handle) {
     std::lock_guard<std::mutex> lock(execute_mutex_);
+    // Keep the filter state if a goal is still executing, else cold start.
+    const bool retarget = executing_.load();
     preempted_ = true;
     if (execute_thread_.joinable()) {
         execute_thread_.join();
     }
     preempted_ = false;
+    executing_ = true;
 
-    execute_thread_ =
-        std::thread([this, goal_handle]() { execute(goal_handle); });
+    execute_thread_ = std::thread(
+        [this, goal_handle, retarget]() { execute(goal_handle, retarget); });
 }
 
 void ReferenceFilterNode::execute(
-    const std::shared_ptr<
-        rclcpp_action::ServerGoalHandle<vortex_msgs::action::GuidanceWaypoint>>
-        goal_handle) {
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<
+        vortex_msgs::action::GuidanceWaypoint>> goal_handle,
+    bool retarget) {
     spdlog::info("Executing goal");
 
-    double convergence_threshold =
-        goal_handle->get_goal()->convergence_threshold;
+    const auto& waypoint_msg = goal_handle->get_goal()->waypoint;
+    ConvergenceTolerance tolerance{waypoint_msg.position_tolerance,
+                                   waypoint_msg.orientation_tolerance};
 
-    if (convergence_threshold <= 0.0) {
-        convergence_threshold = 0.1;
+    if (tolerance.position <= 0.0 && tolerance.orientation <= 0.0) {
+        tolerance = {kDefaultTolerance, kDefaultTolerance};
         spdlog::warn(
-            "ReferenceFilter: Invalid convergence_threshold received (<= 0). "
+            "ReferenceFilter: Invalid tolerances received (both <= 0). "
             "Using default 0.1");
     }
 
@@ -170,7 +178,11 @@ void ReferenceFilterNode::execute(
         return std::pair{current_pose_, current_twist_};
     }();
 
-    follower_->start(pose, twist, wp, convergence_threshold);
+    if (retarget) {
+        follower_->retarget(wp, tolerance);
+    } else {
+        follower_->start(pose, twist, wp, tolerance);
+    }
 
     auto result =
         std::make_shared<vortex_msgs::action::GuidanceWaypoint::Result>();
@@ -180,13 +192,15 @@ void ReferenceFilterNode::execute(
     while (rclcpp::ok()) {
         if (preempted_.load()) {
             result->success = false;
+            executing_ = false;
             goal_handle->abort(result);
             spdlog::info("Goal preempted by newer goal");
             return;
         }
 
         if (goal_handle->is_canceling()) {
             result->success = false;
+            executing_ = false;
             goal_handle->canceled(result);
             spdlog::info("Goal canceled");
             return;
@@ -199,8 +213,9 @@ void ReferenceFilterNode::execute(
             return current_pose_.to_vector();
         }();
 
-        bool target_reached =
-            follower_->within_convergance(current_pose_vector);
+        bool target_reached = follower_->update_convergence(
+            current_pose_vector, this->get_clock()->now().seconds(),
+            waypoint_msg.hold_time_sec);
 
         if (target_reached) {
             follower_->snap_state_to_reference();
@@ -212,6 +227,7 @@ void ReferenceFilterNode::execute(
             reference_pub_->publish(std::move(final_reference_msg));
 
             result->success = true;
+            executing_ = false;
             goal_handle->succeed(result);
             spdlog::info("Goal reached");
             return;
@@ -223,6 +239,7 @@ void ReferenceFilterNode::execute(
         reference_pub_->publish(std::move(reference_msg));
         loop_rate.sleep();
     }
+    executing_ = false;
     if (!rclcpp::ok() && goal_handle->is_active()) {
         auto result =
             std::make_shared<vortex_msgs::action::GuidanceWaypoint::Result>();