Refactor/reference filter dp

[jorgenfj]

• Restructured the package into a standalone core library (lib/) and a ROS integration layer (ros/)
• Extracted waypoint following, mode logic, and convergence checking out of the ROS node
• Fixed action server concurrency race: replaced detached threads + generation counter with a join-before-spawn model guarded by execute_mutex_, ensuring only one thread accesses the follower at a time

[codecov]

Codecov Report

❌ Patch coverage is 83.64116% with 62 lines in your changes missing coverage. Please review.
✅ Project coverage is 33.47%. Comparing base (d53f2b8) to head (1442967).
⚠️ Report is 1 commits behind head on main.

Files with missing lines	Patch %	Lines
...e/reference_filter_dp/test/test_waypoint_utils.cpp	70.88%	0 Missing and 23 partials ⚠️
...ference_filter_dp/src/ros/reference_filter_ros.cpp	82.67%	16 Missing and 6 partials ⚠️
...rence_filter_dp/ros/reference_filter_ros_utils.hpp	72.72%	9 Missing ⚠️
...eference_filter_dp/test/test_waypoint_follower.cpp	88.33%	0 Missing and 7 partials ⚠️
...nce/reference_filter_dp/src/lib/waypoint_utils.cpp	97.61%	1 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main     #687      +/-   ##
==========================================
+ Coverage   31.54%   33.47%   +1.92%     
==========================================
  Files          63       68       +5     
  Lines        4710     4840     +130     
  Branches     1180     1245      +65     
==========================================
+ Hits         1486     1620     +134     
+ Misses       2957     2925      -32     
- Partials      267      295      +28     

Flag	Coverage Δ
unittests	33.47% <83.64%> (+1.92%)	⬆️

Flags with carried forward coverage won't be shown. Click here to find out more.

Files with missing lines	Coverage Δ
...e/reference_filter_dp/src/lib/reference_filter.cpp	100.00% <ø> (ø)
.../reference_filter_dp/src/lib/waypoint_follower.cpp	100.00% <100.00%> (ø)
...reference_filter_dp/test/test_reference_filter.cpp	88.88% <ø> (ø)
...nce/reference_filter_dp/src/lib/waypoint_utils.cpp	97.61% <97.61%> (ø)
...eference_filter_dp/test/test_waypoint_follower.cpp	88.33% <88.33%> (ø)
...rence_filter_dp/ros/reference_filter_ros_utils.hpp	72.72% <72.72%> (ø)
...ference_filter_dp/src/ros/reference_filter_ros.cpp	82.67% <82.67%> (ø)
...e/reference_filter_dp/test/test_waypoint_utils.cpp	70.88% <70.88%> (ø)

... and 1 file with indirect coverage changes

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[Andeshog]

Personally I think it looks better to comment out the unused parameters instead of void cast

rclcpp_action::GoalResponse ReferenceFilterNode::handle_goal(
    const rclcpp_action::GoalUUID& /*uuid*/,
    std::shared_ptr<const vortex_msgs::action::ReferenceFilterWaypoint::Goal> /*goal*/) 
{
    spdlog::info("Accepted goal request");
    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}

Ref https://isocpp.github.io/CppCoreGuidelines/CppCoreGuidelines#f9-unused-parameters-should-be-unnamed

[Andeshog]

rclcpp_action::CancelResponse ReferenceFilterNode::handle_cancel(
    const std::shared_ptr<rclcpp_action::ServerGoalHandle<
        vortex_msgs::action::ReferenceFilterWaypoint>> /*goal_handle*/) {
    spdlog::info("Received request to cancel goal");
    return rclcpp_action::CancelResponse::ACCEPT;
}

[Andeshog]

Could rewrite to something like

const auto wp = waypoint_from_ros(goal_handle->get_goal()->waypoint);

const auto [pose, twist] = [this] {
    std::lock_guard lock(sensor_mutex_);
    return std::pair{current_pose_, current_twist_};
}();

follower_->start(pose, twist, wp, convergence_threshold);

really just a cosmetic change though

[Andeshog]

Consider using an IIFE here as well

const auto current_pose_vector = [this] {
    std::lock_guard lock(sensor_mutex_);
    return current_pose_.to_vector();
}();

[Andeshog]

Could put a using vortex::utils::math::ssa in here just to reduce words

inline vortex_msgs::msg::ReferenceFilter fill_reference_msg(
    const Eigen::Vector18d& x) {
    using vortex::utils::math::ssa;
    vortex_msgs::msg::ReferenceFilter msg;
    msg.x = x(0);
    msg.y = x(1);
    msg.z = x(2);
    msg.roll = ssa(x(3));
    msg.pitch = ssa(x(4));
    msg.yaw = ssa(x(5));
    msg.x_dot = x(6);
    msg.y_dot = x(7);
    msg.z_dot = x(8);
    msg.roll_dot = x(9);
    msg.pitch_dot = x(10);
    msg.yaw_dot = x(11);
    return msg;
}

[Andeshog]

Could add a local using vortex::utils::math::ssa here too

[Andeshog]

Would drop the default, so that you get a warning/error if a new mode is added, but not supported here.
Also, instead of the "initialize then assign" pattern, you could wrap the switch in an IIFE to assign err once

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::FULL_POSE:
            return std::sqrt(ep.squaredNorm() + ea.squaredNorm());
    }
}();

[Andeshog]

Prefer lambdas over bind 😀

[Andeshog]

For example like this

action_server_ = rclcpp_action::create_server
    vortex_msgs::action::ReferenceFilterWaypoint>(
    this, action_server_name,
    [this](const auto& uuid, auto goal) {
        return handle_goal(uuid, std::move(goal));
    },
    [this](auto& goal_handle) {
        return handle_cancel(std::move(goal_handle));
    },
    [this](auto& goal_handle) {
        handle_accepted(std::move(goal_handle));
    },
    rcl_action_server_get_default_options());

[github-actions]

🎉 This PR is included in version 2.7.0 🎉

The release is available on GitHub release

Your semantic-release bot 📦🚀