Refactor/reference filter dp (#687)

* refactor package structure

* waypoint follower class

* time step param

* use vortex types, moved initial state function

* pose as reference api type. Renamed variables

* added reference filter ros utils

* added mutex to WaypointFollower

* move waypoint mode ownership to WaypointFollower

* removed measured_pose_vector6 function

* clean up step result handling

* renamed filter variable from 'x' to state

* update documentation

* fixed action concurrency issues

* join thread in destructor

* separate step and convergance check

* added local using for ssa util function

* added lambdas

* comment out unused function args

Author: jorgenfj

guidance/reference_filter_dp/CMakeLists.txt

@@ -24,36 +24,57 @@ find_package(fmt REQUIRED)
 
 include_directories(include)
 
-set(LIB_NAME "${PROJECT_NAME}_component")
+set(CORE_LIB_NAME "${PROJECT_NAME}")
 
-add_library(${LIB_NAME} SHARED
-  src/reference_filter.cpp
-  src/reference_filter_ros.cpp)
+add_library(${CORE_LIB_NAME} SHARED
+  src/lib/reference_filter.cpp
+  src/lib/waypoint_utils.cpp
+  src/lib/waypoint_follower.cpp
+)
+
+target_include_directories(${CORE_LIB_NAME}
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+)
+
+ament_target_dependencies(${CORE_LIB_NAME}
+  Eigen3
+  vortex_utils
+)
+
+set(COMPONENT_LIB_NAME "${PROJECT_NAME}_component")
+
+add_library(${COMPONENT_LIB_NAME} SHARED
+  src/ros/reference_filter_ros.cpp
+)
 
-ament_target_dependencies(${LIB_NAME} PUBLIC
+ament_target_dependencies(${COMPONENT_LIB_NAME}
   rclcpp
   rclcpp_components
   rclcpp_action
   geometry_msgs
   nav_msgs
-  Eigen3
   vortex_msgs
   vortex_utils
   vortex_utils_ros
   spdlog
   fmt
 )
 
+target_link_libraries(${COMPONENT_LIB_NAME} ${CORE_LIB_NAME})
+
 rclcpp_components_register_node(
-  ${LIB_NAME}
+  ${COMPONENT_LIB_NAME}
   PLUGIN "ReferenceFilterNode"
   EXECUTABLE ${PROJECT_NAME}_node
 )
 
-ament_export_targets(export_${LIB_NAME})
-
-install(TARGETS ${LIB_NAME}
-  EXPORT export_${LIB_NAME}
+install(
+  TARGETS
+    ${CORE_LIB_NAME}
+    ${COMPONENT_LIB_NAME}
+  EXPORT export_${PROJECT_NAME}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
@@ -70,6 +91,14 @@ install(DIRECTORY
   DESTINATION share/${PROJECT_NAME}/
 )
 
+ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
+ament_export_dependencies(
+  Eigen3
+  vortex_utils
+)
+ament_export_include_directories(include)
+ament_export_libraries(${CORE_LIB_NAME})
+
 if(BUILD_TESTING)
   add_subdirectory(test)
 endif()

guidance/reference_filter_dp/README.md

@@ -33,10 +33,28 @@ The state at the next time step is calculated using forward Euler integration
 x_{i+1} = x_i + \dot{x}_i * dt
 ```
 
+## Waypoint modes
+
+Each waypoint has a mode that determines which degrees of freedom are controlled by the reference filter. The mode also determines how convergence is measured.
+
+| Mode | Controlled DOFs | Convergence metric |
+|---|---|---|
+| `FULL_POSE` | All 6 DOF (position + orientation) | Euclidean norm of position and angle errors |
+| `ONLY_POSITION` | x, y, z (orientation holds current value) | Euclidean norm of position error |
+| `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.
+
 ## Action Server
-The action server is responsible for handling goal requests and publishing guidance commands. The server will always prioritize new goal request, and will abort ongoing request when getting a new request. The action definition can be found [here](https://github.com/vortexntnu/vortex-msgs/blob/main/action/ReferenceFilterWaypoint.action).
+
+The action server is responsible for handling goal requests and publishing guidance commands. The server will always prioritize new goal requests, and will abort ongoing requests when getting a new request. The action definition can be found [here](https://github.com/vortexntnu/vortex-msgs/blob/main/action/ReferenceFilterWaypoint.action).
 
 - Action name: /reference_filter
 - Goal type: PoseStamped
 - Result type: bool
 - Guidance topic: /dp/reference
+
+### Overwriting the reference during an action
+
+While an action is executing, the reference goal can be updated at any time by publishing a `geometry_msgs/msg/PoseStamped` to the reference pose topic. This allows external nodes to adjust the target pose mid-action without canceling and resending the goal. The convergence check will use the updated reference, so the action completes when the vehicle reaches the latest reference.

guidance/reference_filter_dp/config/reference_filter_params.yaml

@@ -2,3 +2,4 @@
   ros__parameters:
     zeta: [1., 1., 1., 1., 1., 1.]
     omega: [0.2, 0.2, 0.2, 0.2, 0.2, 0.2]
+    time_step_ms: 10

…e/reference_filter_dp/eigen_typedefs.hpp …ference_filter_dp/lib/eigen_typedefs.hppguidance/reference_filter_dp/include/reference_filter_dp/eigen_typedefs.hpp renamed to guidance/reference_filter_dp/include/reference_filter_dp/lib/eigen_typedefs.hpp guidance/reference_filter_dp/include/reference_filter_dp/eigen_typedefs.hpp renamed to guidance/reference_filter_dp/include/reference_filter_dp/lib/eigen_typedefs.hpp

@@ -3,8 +3,8 @@
  * @brief Contains Eigen typedefs used in this package.
  */
 
-#ifndef REFERENCE_FILTER_DP__EIGEN_TYPEDEFS_HPP_
-#define REFERENCE_FILTER_DP__EIGEN_TYPEDEFS_HPP_
+#ifndef REFERENCE_FILTER_DP__LIB__EIGEN_TYPEDEFS_HPP_
+#define REFERENCE_FILTER_DP__LIB__EIGEN_TYPEDEFS_HPP_
 
 #include <eigen3/Eigen/Dense>
 
@@ -18,4 +18,4 @@ typedef Eigen::Matrix<double, 18, 1> Vector18d;
 
 }  // namespace Eigen
 
-#endif  // REFERENCE_FILTER_DP__EIGEN_TYPEDEFS_HPP_
+#endif  // REFERENCE_FILTER_DP__LIB__EIGEN_TYPEDEFS_HPP_

…reference_filter_dp/reference_filter.hpp …rence_filter_dp/lib/reference_filter.hppguidance/reference_filter_dp/include/reference_filter_dp/reference_filter.hpp renamed to guidance/reference_filter_dp/include/reference_filter_dp/lib/reference_filter.hpp guidance/reference_filter_dp/include/reference_filter_dp/reference_filter.hpp renamed to guidance/reference_filter_dp/include/reference_filter_dp/lib/reference_filter.hpp

@@ -1,7 +1,7 @@
-#ifndef REFERENCE_FILTER_DP__REFERENCE_FILTER_HPP_
-#define REFERENCE_FILTER_DP__REFERENCE_FILTER_HPP_
+#ifndef REFERENCE_FILTER_DP__LIB__REFERENCE_FILTER_HPP_
+#define REFERENCE_FILTER_DP__LIB__REFERENCE_FILTER_HPP_
 
-#include "reference_filter_dp/eigen_typedefs.hpp"
+#include "reference_filter_dp/lib/eigen_typedefs.hpp"
 
 namespace vortex::guidance {
 
@@ -10,6 +10,12 @@ struct ReferenceFilterParams {
     Eigen::Vector6d zeta = Eigen::Vector6d::Zero();
 };
 
+/**
+ * @brief Stateless third-order reference filter.
+ *
+ * Computes the state derivative x_dot = Ad * x + Bd * r, where the state
+ * x = [eta, eta_dot, eta_ddot] is 18-dimensional and r is the 6D reference.
+ */
 class ReferenceFilter {
    public:
     explicit ReferenceFilter(const ReferenceFilterParams& params);
@@ -41,4 +47,4 @@ class ReferenceFilter {
 
 }  // namespace vortex::guidance
 
-#endif  // REFERENCE_FILTER_DP__REFERENCE_FILTER_HPP_
+#endif  // REFERENCE_FILTER_DP__LIB__REFERENCE_FILTER_HPP_

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

@@ -0,0 +1,83 @@
+#ifndef REFERENCE_FILTER_DP__LIB__WAYPOINT_FOLLOWER_HPP_
+#define REFERENCE_FILTER_DP__LIB__WAYPOINT_FOLLOWER_HPP_
+
+#include <mutex>
+#include <vortex/utils/types.hpp>
+#include "reference_filter_dp/lib/eigen_typedefs.hpp"
+#include "reference_filter_dp/lib/reference_filter.hpp"
+#include "reference_filter_dp/lib/waypoint_types.hpp"
+
+namespace vortex::guidance {
+
+using vortex::utils::types::PoseEuler;
+using vortex::utils::types::Twist;
+
+/**
+ * @brief Manages reference filter state and waypoint following logic.
+ *
+ * Thread-safe: all public methods acquire a mutex.
+ */
+class WaypointFollower {
+   public:
+    WaypointFollower(const ReferenceFilterParams& params, double dt_seconds);
+
+    /**
+     * @brief Initialize the follower with the current vehicle state and target.
+     * @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.
+     */
+    void start(const PoseEuler& pose,
+               const Twist& twist,
+               const Waypoint& waypoint,
+               double convergence_threshold);
+
+    /**
+     * @brief Advance the filter by one time step.
+     * @return  The updated filter state.
+     */
+    Eigen::Vector18d step();
+
+    /**
+     * @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.
+     */
+    bool within_convergance(const Eigen::Vector6d& measured_pose) const;
+
+    /**
+     * @brief Update the reference goal pose mid-sequence.
+     * @param reference_goal_pose The new reference pose.
+     */
+    void set_reference(const PoseEuler& reference_goal_pose);
+
+    /**
+     * @brief Snap the position component of the filter state to the reference.
+     *
+     * Useful after convergence to eliminate any remaining steady-state offset
+     */
+    void snap_state_to_reference();
+
+    /// @brief Get the current 18D filter state.
+    Eigen::Vector18d state() const;
+
+    /// @brief Get the current 6D reference goal pose.
+    Eigen::Vector6d reference() const;
+
+   private:
+    Eigen::Vector18d compute_initial_state(const PoseEuler& pose,
+                                           const Twist& twist);
+
+    mutable std::mutex mutex_;
+    ReferenceFilter filter_;
+    double dt_seconds_{0.01};
+    Eigen::Vector18d state_ = Eigen::Vector18d::Zero();
+    Eigen::Vector6d reference_goal_ = Eigen::Vector6d::Zero();
+    WaypointMode waypoint_mode_{WaypointMode::FULL_POSE};
+    double convergence_threshold_{0.1};
+};
+
+}  // namespace vortex::guidance
+
+#endif  // REFERENCE_FILTER_DP__LIB__WAYPOINT_FOLLOWER_HPP_

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

@@ -0,0 +1,34 @@
+#ifndef REFERENCE_FILTER_DP__LIB__WAYPOINT_TYPES_HPP_
+#define REFERENCE_FILTER_DP__LIB__WAYPOINT_TYPES_HPP_
+
+#include <cstdint>
+#include <vortex/utils/types.hpp>
+
+namespace vortex::guidance {
+
+using vortex::utils::types::PoseEuler;
+
+/**
+ * @brief Determines which degrees of freedom the reference filter controls.
+ *
+ * The mode affects both the reference goal computation (via apply_mode_logic)
+ * and the convergence check (via has_converged).
+ */
+enum class WaypointMode : uint8_t {
+    FULL_POSE = 0,         ///< Control all 6 DOF.
+    ONLY_POSITION = 1,     ///< Control x, y, z; hold current orientation.
+    FORWARD_HEADING = 2,   ///< Control x, y, z with yaw toward target.
+    ONLY_ORIENTATION = 3,  ///< Control roll, pitch, yaw; hold current position.
+};
+
+/**
+ * @brief A target pose with an associated waypoint mode.
+ */
+struct Waypoint {
+    PoseEuler pose{};
+    WaypointMode mode = WaypointMode::FULL_POSE;
+};
+
+}  // namespace vortex::guidance
+
+#endif  // REFERENCE_FILTER_DP__LIB__WAYPOINT_TYPES_HPP_

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

@@ -0,0 +1,42 @@
+#ifndef REFERENCE_FILTER_DP__LIB__WAYPOINT_UTILS_HPP_
+#define REFERENCE_FILTER_DP__LIB__WAYPOINT_UTILS_HPP_
+
+#include "reference_filter_dp/lib/eigen_typedefs.hpp"
+#include "reference_filter_dp/lib/waypoint_types.hpp"
+
+namespace vortex::guidance {
+
+/**
+ * @brief Apply waypoint mode logic to a reference pose.
+ *
+ * For modes that don't control all DOFs, the uncontrolled components are
+ * replaced with values from @p current_state so the filter holds them steady.
+ *
+ * @param r_in The raw reference pose (6D).
+ * @param mode The waypoint mode.
+ * @param current_state The current filter state pose (6D).
+ * @return The adjusted reference pose.
+ */
+Eigen::Vector6d apply_mode_logic(const Eigen::Vector6d& r_in,
+                                 WaypointMode mode,
+                                 const Eigen::Vector6d& current_state);
+
+/**
+ * @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.
+ *
+ * @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.
+ */
+bool has_converged(const Eigen::Vector6d& measured_pose,
+                   const Eigen::Vector6d& reference,
+                   WaypointMode mode,
+                   double convergence_threshold);
+
+}  // namespace vortex::guidance
+
+#endif  // REFERENCE_FILTER_DP__LIB__WAYPOINT_UTILS_HPP_