From bfc1186294960dbae5c5d978c5bf13caa71cac7d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 15 Oct 2026 00:31:09 +0200 Subject: [PATCH] Regulated Pure Pursuit: approach the goal inside its tolerance MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Near the goal the controller stopped translating at the system's goal tolerance, the same threshold the system uses for success, and with localization noise it switched between turning to the goal heading and tracking the path, spinning in place. It now uses the strictest of its own and the system's tolerances, drives to half of it before turning to the goal heading, and keeps turning until the robot leaves the whole tolerance (reset on a new goal). Fixes #115 Signed-off-by: Francisco Martín Rico --- .../RegulatedPurePursuitController.hpp | 8 +- .../RegulatedPurePursuitController.cpp | 24 ++- .../tests/CMakeLists.txt | 5 + .../regulated_pp_goal_approach_tests.cpp | 170 ++++++++++++++++++ 4 files changed, 201 insertions(+), 6 deletions(-) create mode 100644 controllers/easynav_regulated_pp_controller/tests/regulated_pp_goal_approach_tests.cpp diff --git a/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp b/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp index 99d05e7e..736be5c0 100644 --- a/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp +++ b/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp @@ -118,9 +118,9 @@ class RegulatedPurePursuitController : public ControllerMethodBase /// Remaining-path distance at which to start slowing. double approach_velocity_scaling_dist_{1.0}; - // --- Goal tolerances (fallback values if "goal_tolerance.*" is not present in NavState) --- - double xy_goal_tolerance_{0.25}; ///< Fallback positional tolerance to the goal (m). - double yaw_goal_tolerance_{0.25}; ///< Fallback angular tolerance to the goal (rad). + // --- Goal tolerances: the strictest of these and the system's "goal_tolerance.*" applies --- + double xy_goal_tolerance_{0.25}; ///< Positional tolerance to the goal (m). + double yaw_goal_tolerance_{0.25}; ///< Angular tolerance to the goal (rad). rclcpp::Publisher::SharedPtr lookahead_point_pub_; rclcpp::Publisher::SharedPtr curvature_lookahead_point_pub_; @@ -133,6 +133,8 @@ class RegulatedPurePursuitController : public ControllerMethodBase double last_angular_vel_{0.0}; /// Whether the controller is currently rotating in place. bool is_rotating_to_heading_{false}; + bool aligning_to_goal_{false}; ///< Turning to the goal heading (with hysteresis). + geometry_msgs::msg::Pose aligning_goal_; ///< Goal that aligning_to_goal_ refers to. geometry_msgs::msg::TwistStamped cmd_vel_; ///< Current velocity command. /// \brief Publishes a zero-velocity command and resets the open-loop velocity feedback. diff --git a/controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp b/controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp index f6f7f775..6e59efd9 100644 --- a/controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp +++ b/controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp @@ -465,13 +465,14 @@ RegulatedPurePursuitController::update_rt(NavState & nav_state) const auto & goal_pose = path.poses.back().pose; + // The strictest of the controller's and the system's tolerances (the system decides success). double xy_tol = xy_goal_tolerance_; double yaw_tol = yaw_goal_tolerance_; if (nav_state.has("goal_tolerance.position")) { - xy_tol = nav_state.get_safe("goal_tolerance.position"); + xy_tol = std::min(xy_tol, nav_state.get_safe("goal_tolerance.position")); } if (nav_state.has("goal_tolerance.yaw")) { - yaw_tol = nav_state.get_safe("goal_tolerance.yaw"); + yaw_tol = std::min(yaw_tol, nav_state.get_safe("goal_tolerance.yaw")); } const double dist_to_goal = std::hypot( @@ -482,6 +483,23 @@ RegulatedPurePursuitController::update_rt(NavState & nav_state) const bool at_goal_xy = dist_to_goal <= xy_tol; + // Drive to half the tolerance before turning to the goal heading, and keep turning until + // the robot leaves the whole tolerance: localization noise at the border must not make it + // switch between both behaviors. + const bool new_goal = goal_pose.position.x != aligning_goal_.position.x || + goal_pose.position.y != aligning_goal_.position.y || + goal_pose.orientation.z != aligning_goal_.orientation.z || + goal_pose.orientation.w != aligning_goal_.orientation.w; + if (new_goal) { + aligning_to_goal_ = false; + aligning_goal_ = goal_pose; + } + if (dist_to_goal <= 0.5 * xy_tol) { + aligning_to_goal_ = true; + } else if (!at_goal_xy) { + aligning_to_goal_ = false; + } + if (at_goal_xy && std::fabs(e_theta_goal) <= yaw_tol) { stop(nav_state, header); return; @@ -494,7 +512,7 @@ RegulatedPurePursuitController::update_rt(NavState & nav_state) double linear_vel = max_linear_vel_; double angular_vel = 0.0; - if (use_rotate_to_heading_ && at_goal_xy) { + if (use_rotate_to_heading_ && aligning_to_goal_) { is_rotating_to_heading_ = true; rotateToHeading(linear_vel, angular_vel, e_theta_goal, dt); } else { diff --git a/controllers/easynav_regulated_pp_controller/tests/CMakeLists.txt b/controllers/easynav_regulated_pp_controller/tests/CMakeLists.txt index 233e7efd..d9448d34 100644 --- a/controllers/easynav_regulated_pp_controller/tests/CMakeLists.txt +++ b/controllers/easynav_regulated_pp_controller/tests/CMakeLists.txt @@ -41,3 +41,8 @@ target_link_libraries(regulated_pp_faulty_planner_tests easynav_controller::easynav_controller easynav_planner::faulty_planner ) + +ament_add_gtest(regulated_pp_goal_approach_tests regulated_pp_goal_approach_tests.cpp) +target_link_libraries(regulated_pp_goal_approach_tests + ${PROJECT_NAME} +) diff --git a/controllers/easynav_regulated_pp_controller/tests/regulated_pp_goal_approach_tests.cpp b/controllers/easynav_regulated_pp_controller/tests/regulated_pp_goal_approach_tests.cpp new file mode 100644 index 00000000..ad1f1e9f --- /dev/null +++ b/controllers/easynav_regulated_pp_controller/tests/regulated_pp_goal_approach_tests.cpp @@ -0,0 +1,170 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "gtest/gtest.h" + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "nav_msgs/msg/path.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "tf2/LinearMath/Quaternion.hpp" + +#include "easynav_common/types/NavState.hpp" +#include "easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp" + +namespace +{ + +// Path along +x ending at the goal (3, 0), whose heading is +y (90 deg). +nav_msgs::msg::Path path_to(double goal_x, double goal_yaw) +{ + nav_msgs::msg::Path path; + path.header.frame_id = "map"; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, goal_yaw); + for (double x = 0.0; x <= goal_x + 1e-9; x += 0.1) { + geometry_msgs::msg::PoseStamped p; + p.header.frame_id = "map"; + p.pose.position.x = x; + p.pose.orientation.z = q.z(); + p.pose.orientation.w = q.w(); + path.poses.push_back(p); + } + return path; +} + +class RppGoalApproachTest : public ::testing::Test +{ +protected: + static void SetUpTestSuite() {rclcpp::init(0, nullptr);} + static void TearDownTestSuite() {rclcpp::shutdown();} + + // The playgrounds' tolerances: the controller's own equal to the system's. + void SetUp() override + { + make({rclcpp::Parameter("rpp.xy_goal_tolerance", 0.3), + rclcpp::Parameter("rpp.yaw_goal_tolerance", 0.15)}); + } + + void make(const std::vector & overrides) + { + static int n = 0; + node_ = std::make_shared( + "rpp_goal_node_" + std::to_string(n++), rclcpp::NodeOptions().parameter_overrides(overrides)); + rpp_ = std::make_unique(); + rpp_->initialize(node_, "rpp"); + nav_state_ = std::make_unique(); + nav_state_->set("goal_tolerance.position", 0.3); + nav_state_->set("goal_tolerance.yaw", 0.15); + nav_state_->set("path", path_to(3.0, M_PI_2)); + } + + // Robot \p dist_to_goal m before the goal on the path, heading +x (the goal wants +y). + geometry_msgs::msg::Twist step(double dist_to_goal, double yaw = 0.0) + { + nav_msgs::msg::Odometry odom; + odom.pose.pose.position.x = 3.0 - dist_to_goal; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, yaw); + odom.pose.pose.orientation.z = q.z(); + odom.pose.pose.orientation.w = q.w(); + nav_state_->set("robot_pose", odom); + rpp_->update_rt(*nav_state_); + return nav_state_->get("cmd_vel").twist; + } + + static bool turning_in_place(const geometry_msgs::msg::Twist & t) + { + return t.linear.x == 0.0 && t.angular.z != 0.0; + } + + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + std::unique_ptr rpp_; + std::unique_ptr nav_state_; +}; + +} // namespace + +TEST_F(RppGoalApproachTest, InsideTheToleranceButNotHalfItKeepsDriving) +{ + const auto t = step(0.25); + EXPECT_GT(t.linear.x, 0.0); +} + +TEST_F(RppGoalApproachTest, WithinHalfTheToleranceTurnsToTheGoalHeading) +{ + const auto t = step(0.1); + EXPECT_TRUE(turning_in_place(t)); + EXPECT_GT(t.angular.z, 0.0); // towards +y +} + +TEST_F(RppGoalApproachTest, OnceTurningNoiseInsideTheToleranceDoesNotMakeItDriveAgain) +{ + EXPECT_TRUE(turning_in_place(step(0.1))); + for (double d : {0.2, 0.28, 0.12, 0.29, 0.25}) { + EXPECT_TRUE(turning_in_place(step(d))) << d; + } +} + +TEST_F(RppGoalApproachTest, LeavingTheWholeToleranceTracksThePathAgain) +{ + EXPECT_TRUE(turning_in_place(step(0.1))); + const auto out = step(0.4); + EXPECT_GT(out.linear.x, 0.0); + EXPECT_GT(step(0.25).linear.x, 0.0); // back inside, but not at half: still driving + EXPECT_TRUE(turning_in_place(step(0.1))); +} + +TEST_F(RppGoalApproachTest, ANewGoalStartsDrivingAgain) +{ + EXPECT_TRUE(turning_in_place(step(0.1))); + nav_state_->set("path", path_to(3.0, -M_PI_2)); // same place, another heading + EXPECT_TRUE(turning_in_place(step(0.1))); // still within half: turns, now to -y + nav_state_->set("path", path_to(3.5, M_PI_2)); // a goal 0.6 m farther + EXPECT_GT(step(0.25 + 0.5).linear.x, 0.0); +} + +TEST_F(RppGoalApproachTest, AtTheGoalWithTheRightHeadingItStops) +{ + const auto t = step(0.25, M_PI_2); // inside the tolerance, heading within 0.15 rad + EXPECT_EQ(t.linear.x, 0.0); + EXPECT_EQ(t.angular.z, 0.0); +} + +TEST_F(RppGoalApproachTest, AStricterOwnToleranceIsUsed) +{ + make({rclcpp::Parameter("rpp.xy_goal_tolerance", 0.1)}); + EXPECT_GT(step(0.08).linear.x, 0.0); // within half the system's, not half its own + EXPECT_TRUE(turning_in_place(step(0.04))); + // The system would accept it; the controller does not stop there. + const auto t = step(0.25, M_PI_2); + EXPECT_FALSE(t.linear.x == 0.0 && t.angular.z == 0.0); +} + +TEST_F(RppGoalApproachTest, WithoutSystemTolerancesItsOwnApply) +{ + make({rclcpp::Parameter("rpp.xy_goal_tolerance", 0.4)}); + nav_state_ = std::make_unique(); + nav_state_->set("path", path_to(3.0, M_PI_2)); + EXPECT_GT(step(0.3).linear.x, 0.0); + EXPECT_TRUE(turning_in_place(step(0.15))); +}