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))); +}