From be7d59ef33eecab3e66e1dd07ebe3a4852cf9b42 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Mon, 2 Feb 2026 20:44:58 +0100 Subject: [PATCH 1/4] reference filter mode dependant pose convergence --- .../reference_filter_ros.hpp | 7 ++ .../src/reference_filter_ros.cpp | 65 ++++++++++++++++++- 2 files changed, 71 insertions(+), 1 deletion(-) diff --git a/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_ros.hpp b/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_ros.hpp index 6fa179291..945a31df3 100644 --- a/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_ros.hpp +++ b/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_ros.hpp @@ -76,8 +76,15 @@ class ReferenceFilterNode : public rclcpp::Node { Eigen::Vector6d fill_reference_goal(const geometry_msgs::msg::Pose& goal); + Eigen::Vector6d measured_pose_vector6(); + Eigen::Vector6d apply_mode_logic(const Eigen::Vector6d& r_in, uint8_t mode); + bool has_converged_against_pose(const Eigen::Vector6d& y, + const Eigen::Vector6d& r, + uint8_t mode, + double convergence_threshold) const; + void publish_hold_reference(); vortex_msgs::msg::ReferenceFilter fill_reference_msg(); diff --git a/guidance/reference_filter_dp/src/reference_filter_ros.cpp b/guidance/reference_filter_dp/src/reference_filter_ros.cpp index 148c94dc7..2cd25af11 100644 --- a/guidance/reference_filter_dp/src/reference_filter_ros.cpp +++ b/guidance/reference_filter_dp/src/reference_filter_ros.cpp @@ -197,6 +197,31 @@ Eigen::Vector18d ReferenceFilterNode::fill_reference_state() { return x; } +Eigen::Vector6d ReferenceFilterNode::measured_pose_vector6() { + geometry_msgs::msg::PoseWithCovarianceStamped pose_msg; + { + std::lock_guard lock(mutex_); + pose_msg = current_pose_; + } + + const auto& p = pose_msg.pose.pose.position; + const auto& o = pose_msg.pose.pose.orientation; + + Eigen::Quaterniond q(o.w, o.x, o.y, o.z); + if (q.norm() < 1e-9) { + q = Eigen::Quaterniond::Identity(); + } else { + q.normalize(); + } + + Eigen::Vector3d euler = vortex::utils::math::quat_to_euler(q); + + Eigen::Vector6d y; + y << p.x, p.y, p.z, vortex::utils::math::ssa(euler(0)), + vortex::utils::math::ssa(euler(1)), vortex::utils::math::ssa(euler(2)); + return y; +} + Eigen::Vector6d ReferenceFilterNode::fill_reference_goal( const geometry_msgs::msg::Pose& goal) { double x{goal.position.x}; @@ -271,6 +296,42 @@ Eigen::Vector6d ReferenceFilterNode::apply_mode_logic( return r_out; } +bool ReferenceFilterNode::has_converged_against_pose( + const Eigen::Vector6d& y, + const Eigen::Vector6d& r, + uint8_t mode, + double convergence_threshold) const { + const Eigen::Vector3d ep = y.head<3>() - r.head<3>(); + + Eigen::Vector3d ea; + ea(0) = vortex::utils::math::ssa(y(3) - r(3)); + ea(1) = vortex::utils::math::ssa(y(4) - r(4)); + ea(2) = vortex::utils::math::ssa(y(5) - r(5)); + + double err = 0.0; + + switch (mode) { + case vortex_msgs::msg::Waypoint::ONLY_POSITION: + err = ep.norm(); + break; + + case vortex_msgs::msg::Waypoint::ONLY_ORIENTATION: + err = ea.norm(); + break; + + case vortex_msgs::msg::Waypoint::FORWARD_HEADING: + err = std::sqrt(ep.squaredNorm() + ea(2) * ea(2)); + break; + + case vortex_msgs::msg::Waypoint::FULL_POSE: + default: + err = std::sqrt(ep.squaredNorm() + ea.squaredNorm()); + break; + } + + return err < convergence_threshold; +} + void ReferenceFilterNode::publish_hold_reference() { if (!reference_pub_) { return; @@ -371,7 +432,9 @@ void ReferenceFilterNode::execute( goal_handle->publish_feedback(feedback); reference_pub_->publish(feedback_msg); - if ((x_.head(6) - r_.head(6)).norm() < convergence_threshold) { + Eigen::Vector6d y = measured_pose_vector6(); + + if (has_converged_against_pose(y, r_, mode, convergence_threshold)) { result->success = true; goal_handle->succeed(result); x_.head(6) = r_.head(6); From 29396c9b0d97ae8edd058420e25fae66d3951377 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Thu, 5 Feb 2026 12:00:11 +0100 Subject: [PATCH 2/4] reference action detached thread, fixed tests --- .../src/reference_filter_ros.cpp | 7 +++--- .../test/test_single_waypoint.py | 25 +++++++++++++------ 2 files changed, 20 insertions(+), 12 deletions(-) diff --git a/guidance/reference_filter_dp/src/reference_filter_ros.cpp b/guidance/reference_filter_dp/src/reference_filter_ros.cpp index 2cd25af11..b9f591d94 100644 --- a/guidance/reference_filter_dp/src/reference_filter_ros.cpp +++ b/guidance/reference_filter_dp/src/reference_filter_ros.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include #include #include @@ -67,8 +68,6 @@ void ReferenceFilterNode::set_action_server() { this->declare_parameter("action_servers.reference_filter"); std::string action_server_name = this->get_parameter("action_servers.reference_filter").as_string(); - cb_group_ = - this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); action_server_ = rclcpp_action::create_server< vortex_msgs::action::ReferenceFilterWaypoint>( @@ -79,7 +78,7 @@ void ReferenceFilterNode::set_action_server() { std::placeholders::_1), std::bind(&ReferenceFilterNode::handle_accepted, this, std::placeholders::_1), - rcl_action_server_get_default_options(), cb_group_); + rcl_action_server_get_default_options()); } void ReferenceFilterNode::set_refererence_filter() { @@ -152,7 +151,7 @@ rclcpp_action::CancelResponse ReferenceFilterNode::handle_cancel( void ReferenceFilterNode::handle_accepted( const std::shared_ptr> goal_handle) { - execute(goal_handle); + std::thread([this, goal_handle]() { execute(goal_handle); }).detach(); } Eigen::Vector18d ReferenceFilterNode::fill_reference_state() { diff --git a/mission/waypoint_manager/test/test_single_waypoint.py b/mission/waypoint_manager/test/test_single_waypoint.py index 35e02b79e..12d7b78aa 100644 --- a/mission/waypoint_manager/test/test_single_waypoint.py +++ b/mission/waypoint_manager/test/test_single_waypoint.py @@ -9,7 +9,7 @@ import launch_testing.actions import rclpy from ament_index_python.packages import get_package_share_directory -from nav_msgs.msg import Odometry +from geometry_msgs.msg import PoseWithCovarianceStamped from rclpy.action import ActionClient from rclpy.qos import qos_profile_sensor_data from vortex_msgs.action import WaypointManager @@ -66,20 +66,28 @@ def setUp(self): def tearDown(self): self.node.destroy_node() - def _publish_fake_odom(self, x, y, z, duration_sec=5.0, rate_hz=10.0): + def _publish_fake_pose(self, x, y, z, duration_sec=5.0, rate_hz=10.0): pub = self.node.create_publisher( - Odometry, - '/orca/odom', + PoseWithCovarianceStamped, + '/orca/pose', qos_profile_sensor_data, ) - msg = Odometry() + msg = PoseWithCovarianceStamped() msg.header.frame_id = 'odom' - msg.child_frame_id = 'base_link' + msg.pose.pose.position.x = x msg.pose.pose.position.y = y msg.pose.pose.position.z = z + # Valid orientation + msg.pose.pose.orientation.w = 1.0 + msg.pose.pose.orientation.x = 0.0 + msg.pose.pose.orientation.y = 0.0 + msg.pose.pose.orientation.z = 0.0 + + # Covariance can be left as all zeros (default) for this test. + end_time = time.time() + duration_sec period = 1.0 / rate_hz @@ -100,9 +108,11 @@ def test_accepts_and_executes_goal(self): goal_msg = WaypointManager.Goal() wp = Waypoint() + wp.mode = Waypoint.FULL_POSE wp.pose.position.x = 0.0 wp.pose.position.y = 0.0 wp.pose.position.z = 1.0 + wp.pose.orientation.w = 1.0 # valid quaternion goal_msg.waypoints = [wp] goal_msg.persistent = False @@ -120,8 +130,7 @@ def test_accepts_and_executes_goal(self): goal_handle = send_fut.result() assert goal_handle.accepted, 'Goal was rejected' - # Publish fake odometry at the goal position - self._publish_fake_odom( + self._publish_fake_pose( x=wp.pose.position.x, y=wp.pose.position.y, z=wp.pose.position.z, From 684cbb1146bdb1258678557697e3894da46b10d1 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Thu, 5 Feb 2026 14:23:59 +0100 Subject: [PATCH 3/4] fix: waypoint manager bug --- guidance/reference_filter_dp/src/reference_filter_ros.cpp | 3 ++- mission/waypoint_manager/src/waypoint_manager_ros.cpp | 1 + 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/guidance/reference_filter_dp/src/reference_filter_ros.cpp b/guidance/reference_filter_dp/src/reference_filter_ros.cpp index b9f591d94..367d8e28e 100644 --- a/guidance/reference_filter_dp/src/reference_filter_ros.cpp +++ b/guidance/reference_filter_dp/src/reference_filter_ros.cpp @@ -404,7 +404,8 @@ void ReferenceFilterNode::execute( while (rclcpp::ok()) { { std::lock_guard lock(mutex_); - if (goal_handle->get_goal_id() == preempted_goal_id_) { + if (goal_handle->get_goal_id() == preempted_goal_id_ && + !goal_handle->is_canceling()) { publish_hold_reference(); result->success = false; goal_handle->abort(result); diff --git a/mission/waypoint_manager/src/waypoint_manager_ros.cpp b/mission/waypoint_manager/src/waypoint_manager_ros.cpp index 93a9da5d2..dc46c5eb0 100644 --- a/mission/waypoint_manager/src/waypoint_manager_ros.cpp +++ b/mission/waypoint_manager/src/waypoint_manager_ros.cpp @@ -205,6 +205,7 @@ void WaypointManagerNode::handle_send_waypoints_service_request( priority_mode_active_ = request->take_priority; if (request->overwrite_prior_waypoints) { + mission_id_++; waypoints_ = request->waypoints; current_index_ = 0; has_reference_pose_ = false; From bc251847479a142a34684c6543cdf5450adc2d1d Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Thu, 5 Feb 2026 15:12:31 +0100 Subject: [PATCH 4/4] remove hold reference pub --- .../src/reference_filter_ros.cpp | 37 ------------------- 1 file changed, 37 deletions(-) diff --git a/guidance/reference_filter_dp/src/reference_filter_ros.cpp b/guidance/reference_filter_dp/src/reference_filter_ros.cpp index 367d8e28e..d1dd5bc87 100644 --- a/guidance/reference_filter_dp/src/reference_filter_ros.cpp +++ b/guidance/reference_filter_dp/src/reference_filter_ros.cpp @@ -331,41 +331,6 @@ bool ReferenceFilterNode::has_converged_against_pose( return err < convergence_threshold; } -void ReferenceFilterNode::publish_hold_reference() { - if (!reference_pub_) { - return; - } - - const auto& p = current_pose_.pose.pose.position; - const auto& o = current_pose_.pose.pose.orientation; - - Eigen::Quaterniond q(o.w, o.x, o.y, o.z); - if (q.norm() < 1e-9) { - q = Eigen::Quaterniond::Identity(); - } else { - q.normalize(); - } - - Eigen::Vector3d euler_angles = vortex::utils::math::quat_to_euler(q); - - vortex_msgs::msg::ReferenceFilter hold_msg; - hold_msg.x = p.x; - hold_msg.y = p.y; - hold_msg.z = p.z; - hold_msg.roll = vortex::utils::math::ssa(euler_angles(0)); - hold_msg.pitch = vortex::utils::math::ssa(euler_angles(1)); - hold_msg.yaw = vortex::utils::math::ssa(euler_angles(2)); - - hold_msg.x_dot = 0.0; - hold_msg.y_dot = 0.0; - hold_msg.z_dot = 0.0; - hold_msg.roll_dot = 0.0; - hold_msg.pitch_dot = 0.0; - hold_msg.yaw_dot = 0.0; - - reference_pub_->publish(hold_msg); -} - void ReferenceFilterNode::execute( const std::shared_ptr> goal_handle) { @@ -406,7 +371,6 @@ void ReferenceFilterNode::execute( std::lock_guard lock(mutex_); if (goal_handle->get_goal_id() == preempted_goal_id_ && !goal_handle->is_canceling()) { - publish_hold_reference(); result->success = false; goal_handle->abort(result); return; @@ -415,7 +379,6 @@ void ReferenceFilterNode::execute( { std::lock_guard lock(mutex_); if (goal_handle->is_canceling()) { - publish_hold_reference(); result->success = false; goal_handle->canceled(result); spdlog::info("Goal canceled");