From 7a759f5fa19946bc443c05ecfa9c2ca475182e54 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Thu, 28 Aug 2025 17:04:41 +0000 Subject: [PATCH 1/4] Add exact path following for RPP Signed-off-by: mini-1235 --- .../nav2_mppi_controller/tools/utils.hpp | 53 --------------- nav2_mppi_controller/src/path_handler.cpp | 5 +- nav2_mppi_controller/test/utils_test.cpp | 63 ----------------- .../parameter_handler.hpp | 3 + .../path_handler.hpp | 24 ++++++- .../src/parameter_handler.cpp | 18 ++++- .../src/path_handler.cpp | 67 ++++++++++++++++--- .../src/regulated_pure_pursuit_controller.cpp | 4 +- .../include/nav2_util/controller_utils.hpp | 53 +++++++++++++++ nav2_util/test/test_controller_utils.cpp | 63 +++++++++++++++++ 10 files changed, 220 insertions(+), 133 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index f518f16e169..9e7d77a0ae6 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -585,59 +585,6 @@ inline void savitskyGolayFilter( control_sequence.wz(offset)}; } -/** - * @brief Find the iterator of the first pose at which there is an inversion on the path, - * @param path to check for inversion - * @return the first point after the inversion found in the path - */ -inline unsigned int findFirstPathInversion(nav_msgs::msg::Path & path) -{ - // At least 3 poses for a possible inversion - if (path.poses.size() < 3) { - return path.poses.size(); - } - - // Iterating through the path to determine the position of the path inversion - for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) { - // We have two vectors for the dot product OA and AB. Determining the vectors. - float oa_x = path.poses[idx].pose.position.x - - path.poses[idx - 1].pose.position.x; - float oa_y = path.poses[idx].pose.position.y - - path.poses[idx - 1].pose.position.y; - float ab_x = path.poses[idx + 1].pose.position.x - - path.poses[idx].pose.position.x; - float ab_y = path.poses[idx + 1].pose.position.y - - path.poses[idx].pose.position.y; - - // Checking for the existence of cusp, in the path, using the dot product. - float dot_product = (oa_x * ab_x) + (oa_y * ab_y); - if (dot_product < 0.0f) { - return idx + 1; - } - } - - return path.poses.size(); -} - -/** - * @brief Find and remove poses after the first inversion in the path - * @param path to check for inversion - * @return The location of the inversion, return 0 if none exist - */ -inline unsigned int removePosesAfterFirstInversion(nav_msgs::msg::Path & path) -{ - nav_msgs::msg::Path cropped_path = path; - const unsigned int first_after_inversion = findFirstPathInversion(cropped_path); - if (first_after_inversion == path.poses.size()) { - return 0u; - } - - cropped_path.poses.erase( - cropped_path.poses.begin() + first_after_inversion, cropped_path.poses.end()); - path = cropped_path; - return first_after_inversion; -} - /** * @brief Compare to trajectory points to find closest path point along integrated distances * @param vec Vect to check diff --git a/nav2_mppi_controller/src/path_handler.cpp b/nav2_mppi_controller/src/path_handler.cpp index 55931f6bc62..63780cb049a 100644 --- a/nav2_mppi_controller/src/path_handler.cpp +++ b/nav2_mppi_controller/src/path_handler.cpp @@ -18,6 +18,7 @@ #include "nav2_mppi_controller/tools/utils.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "nav2_util/robot_utils.hpp" +#include "nav2_util/controller_utils.hpp" namespace mppi { @@ -135,7 +136,7 @@ nav_msgs::msg::Path PathHandler::transformPath( if (isWithinInversionTolerances(global_pose)) { prunePlan(global_plan_, global_plan_.poses.begin() + inversion_locale_); global_plan_up_to_inversion_ = global_plan_; - inversion_locale_ = utils::removePosesAfterFirstInversion(global_plan_up_to_inversion_); + inversion_locale_ = nav2_util::removePosesAfterFirstInversion(global_plan_up_to_inversion_); } } @@ -158,7 +159,7 @@ void PathHandler::setPath(const nav_msgs::msg::Path & plan) global_plan_ = plan; global_plan_up_to_inversion_ = global_plan_; if (enforce_path_inversion_) { - inversion_locale_ = utils::removePosesAfterFirstInversion(global_plan_up_to_inversion_); + inversion_locale_ = nav2_util::removePosesAfterFirstInversion(global_plan_up_to_inversion_); } } diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp index 6f70e19cfa5..264b18d6e48 100644 --- a/nav2_mppi_controller/test/utils_test.cpp +++ b/nav2_mppi_controller/test/utils_test.cpp @@ -390,69 +390,6 @@ TEST(UtilsTests, SmootherTest) EXPECT_LT(smoothed_val, original_val); } -TEST(UtilsTests, FindPathInversionTest) -{ - // Straight path, no inversions to be found - nav_msgs::msg::Path path; - for (unsigned int i = 0; i != 10; i++) { - geometry_msgs::msg::PoseStamped pose; - pose.pose.position.x = i; - path.poses.push_back(pose); - } - EXPECT_EQ(utils::findFirstPathInversion(path), 10u); - - // To short to process - path.poses.erase(path.poses.begin(), path.poses.begin() + 7); - EXPECT_EQ(utils::findFirstPathInversion(path), 3u); - - // Has inversion at index 10, so should return 11 for the first point afterwards - // 0 1 2 3 4 5 6 7 8 9 10 **9** 8 7 6 5 4 3 2 1 - path.poses.clear(); - for (unsigned int i = 0; i != 10; i++) { - geometry_msgs::msg::PoseStamped pose; - pose.pose.position.x = i; - path.poses.push_back(pose); - } - for (unsigned int i = 0; i != 10; i++) { - geometry_msgs::msg::PoseStamped pose; - pose.pose.position.x = 10 - i; - path.poses.push_back(pose); - } - EXPECT_EQ(utils::findFirstPathInversion(path), 11u); -} - -TEST(UtilsTests, RemovePosesAfterPathInversionTest) -{ - nav_msgs::msg::Path path; - // straight path - for (unsigned int i = 0; i != 10; i++) { - geometry_msgs::msg::PoseStamped pose; - pose.pose.position.x = i; - path.poses.push_back(pose); - } - EXPECT_EQ(utils::removePosesAfterFirstInversion(path), 0u); - - // try empty path - path.poses.clear(); - EXPECT_EQ(utils::removePosesAfterFirstInversion(path), 0u); - - // cusping path - for (unsigned int i = 0; i != 10; i++) { - geometry_msgs::msg::PoseStamped pose; - pose.pose.position.x = i; - path.poses.push_back(pose); - } - for (unsigned int i = 0; i != 10; i++) { - geometry_msgs::msg::PoseStamped pose; - pose.pose.position.x = 10 - i; - path.poses.push_back(pose); - } - EXPECT_EQ(utils::removePosesAfterFirstInversion(path), 11u); - // Check to see if removed - EXPECT_EQ(path.poses.size(), 11u); - EXPECT_EQ(path.poses.back().pose.position.x, 10); -} - TEST(UtilsTests, ShiftColumnsByOnePlaceTest) { // Try with scalar value diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp index 2b03443f5fb..5400b63edd7 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp @@ -63,6 +63,9 @@ struct Parameters bool use_collision_detection; double transform_tolerance; bool stateful; + bool enforce_path_inversion; + float inversion_xy_tolerance; + float inversion_yaw_tolerance; }; /** diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp index 135a304dc26..0460edd4d86 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp @@ -27,10 +27,12 @@ #include "nav2_util/odometry_utils.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_core/controller_exceptions.hpp" +#include "nav2_regulated_pure_pursuit_controller/parameter_handler.hpp" #include "geometry_msgs/msg/pose.hpp" namespace nav2_regulated_pure_pursuit_controller { +using PathIterator = std::vector::iterator; /** * @class nav2_regulated_pure_pursuit_controller::PathHandler @@ -43,7 +45,7 @@ class PathHandler * @brief Constructor for nav2_regulated_pure_pursuit_controller::PathHandler */ PathHandler( - double transform_tolerance, + Parameters * params, std::shared_ptr tf, std::shared_ptr costmap_ros); @@ -65,10 +67,24 @@ class PathHandler const geometry_msgs::msg::PoseStamped & pose, double max_robot_pose_search_dist, bool reject_unit_path = false); - void setPlan(const nav_msgs::msg::Path & path) {global_plan_ = path;} + void setPlan(const nav_msgs::msg::Path & path); nav_msgs::msg::Path getPlan() {return global_plan_;} + /** + * @brief Check if the robot pose is within the set inversion tolerances + * @param robot_pose Robot's current pose to check + * @return bool If the robot pose is within the set inversion tolerances + */ + bool isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose); + + /** + * @brief Prune a path to only interesting portions + * @param plan Plan to prune + * @param end Final path iterator + */ + void prunePlan(nav_msgs::msg::Path & plan, const PathIterator end); + protected: /** * Get the greatest extent of the costmap in meters from the center. @@ -81,6 +97,10 @@ class PathHandler std::shared_ptr tf_; std::shared_ptr costmap_ros_; nav_msgs::msg::Path global_plan_; + nav_msgs::msg::Path global_plan_up_to_inversion_; + unsigned int inversion_locale_{0u}; + Parameters * params_; + }; } // namespace nav2_regulated_pure_pursuit_controller diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp index 5bd1f2a7eeb..f48c7e0e53e 100644 --- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp @@ -105,7 +105,13 @@ ParameterHandler::ParameterHandler( node, plugin_name_ + ".use_collision_detection", rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( - node, plugin_name_ + ".stateful", rclcpp::ParameterValue(true)); + node, plugin_name_ + ".stateful", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".enforce_path_inversion", rclcpp::ParameterValue(false)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".inversion_xy_tolerance", rclcpp::ParameterValue(0.2)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".inversion_yaw_tolerance", rclcpp::ParameterValue(0.4)); node->get_parameter(plugin_name_ + ".desired_linear_vel", params_.desired_linear_vel); params_.base_desired_linear_vel = params_.desired_linear_vel; @@ -190,6 +196,10 @@ ParameterHandler::ParameterHandler( plugin_name_ + ".use_collision_detection", params_.use_collision_detection); node->get_parameter(plugin_name_ + ".stateful", params_.stateful); + node->get_parameter(plugin_name_ + ".enforce_path_inversion", params_.enforce_path_inversion); + node->get_parameter(plugin_name_ + ".inversion_xy_tolerance", params_.inversion_xy_tolerance); + node->get_parameter(plugin_name_ + ".inversion_yaw_tolerance", params_.inversion_yaw_tolerance); + if (params_.inflation_cost_scaling_factor <= 0.0) { RCLCPP_WARN( @@ -311,6 +321,10 @@ ParameterHandler::updateParametersCallback( params_.transform_tolerance = parameter.as_double(); } else if (param_name == plugin_name_ + ".max_robot_pose_search_dist") { params_.max_robot_pose_search_dist = parameter.as_double(); + } else if (param_name == plugin_name_ + ".inversion_xy_tolerance") { + params_.inversion_xy_tolerance = parameter.as_double(); + } else if (param_name == plugin_name_ + ".inversion_yaw_tolerance") { + params_.inversion_yaw_tolerance = parameter.as_double(); } } else if (param_type == ParameterType::PARAMETER_BOOL) { if (param_name == plugin_name_ + ".use_velocity_scaled_lookahead_dist") { @@ -333,6 +347,8 @@ ParameterHandler::updateParametersCallback( params_.allow_reversing = parameter.as_bool(); } else if (param_name == plugin_name_ + ".interpolate_curvature_after_goal") { params_.interpolate_curvature_after_goal = parameter.as_bool(); + } else if (param_name == plugin_name_ + ".enforce_path_inversion") { + params_.enforce_path_inversion = parameter.as_bool(); } } } diff --git a/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp index c91bb8dc8f7..7072bbc1843 100644 --- a/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp @@ -24,6 +24,8 @@ #include "nav2_ros_common/node_utils.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_util/robot_utils.hpp" +#include "nav2_util/controller_utils.hpp" +#include "angles/angles.h" namespace nav2_regulated_pure_pursuit_controller { @@ -31,11 +33,17 @@ namespace nav2_regulated_pure_pursuit_controller using nav2_util::geometry_utils::euclidean_distance; PathHandler::PathHandler( - double transform_tolerance, + Parameters * params, std::shared_ptr tf, std::shared_ptr costmap_ros) -: transform_tolerance_(transform_tolerance), tf_(tf), costmap_ros_(costmap_ros) { + params_ = params; + costmap_ros_ = costmap_ros; + tf_ = tf; + + if(params_->enforce_path_inversion){ + inversion_locale_ = 0u; + } } double PathHandler::getCostmapMaxExtent() const @@ -46,22 +54,51 @@ double PathHandler::getCostmapMaxExtent() const return max_costmap_dim_meters / 2.0; } +void PathHandler::prunePlan(nav_msgs::msg::Path & plan, const PathIterator end) +{ + plan.poses.erase(plan.poses.begin(), end); +} + +bool PathHandler::isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose) +{ + // Keep full path if we are within tolerance of the inversion pose + const auto last_pose = global_plan_up_to_inversion_.poses.back(); + float distance = hypotf( + robot_pose.pose.position.x - last_pose.pose.position.x, + robot_pose.pose.position.y - last_pose.pose.position.y); + + float angle_distance = angles::shortest_angular_distance( + tf2::getYaw(robot_pose.pose.orientation), + tf2::getYaw(last_pose.pose.orientation)); + + return distance <= params_->inversion_xy_tolerance && fabs(angle_distance) <= params_->inversion_yaw_tolerance; +} + +void PathHandler::setPlan(const nav_msgs::msg::Path & path) +{ + global_plan_ = path; + global_plan_up_to_inversion_ = global_plan_; + if(params_->enforce_path_inversion){ + inversion_locale_ = nav2_util::removePosesAfterFirstInversion(global_plan_up_to_inversion_); + } +} + nav_msgs::msg::Path PathHandler::transformGlobalPlan( const geometry_msgs::msg::PoseStamped & pose, double max_robot_pose_search_dist, bool reject_unit_path) { - if (global_plan_.poses.empty()) { + if (global_plan_up_to_inversion_.poses.empty()) { throw nav2_core::InvalidPath("Received plan with zero length"); } - if (reject_unit_path && global_plan_.poses.size() == 1) { + if (reject_unit_path && global_plan_up_to_inversion_.poses.size() == 1) { throw nav2_core::InvalidPath("Received plan with length of one"); } // let's get the pose of the robot in the frame of the plan geometry_msgs::msg::PoseStamped robot_pose; - if (!nav2_util::transformPoseInTargetFrame(pose, robot_pose, *tf_, global_plan_.header.frame_id, + if (!nav2_util::transformPoseInTargetFrame(pose, robot_pose, *tf_, global_plan_up_to_inversion_.header.frame_id, transform_tolerance_)) { throw nav2_core::ControllerTFError("Unable to transform robot pose into global plan's frame"); @@ -69,14 +106,14 @@ nav_msgs::msg::Path PathHandler::transformGlobalPlan( auto closest_pose_upper_bound = nav2_util::geometry_utils::first_after_integrated_distance( - global_plan_.poses.begin(), global_plan_.poses.end(), max_robot_pose_search_dist); + global_plan_up_to_inversion_.poses.begin(), global_plan_up_to_inversion_.poses.end(), max_robot_pose_search_dist); // First find the closest pose on the path to the robot // bounded by when the path turns around (if it does) so we don't get a pose from a later // portion of the path auto transformation_begin = nav2_util::geometry_utils::min_by( - global_plan_.poses.begin(), closest_pose_upper_bound, + global_plan_up_to_inversion_.poses.begin(), closest_pose_upper_bound, [&robot_pose](const geometry_msgs::msg::PoseStamped & ps) { return euclidean_distance(robot_pose, ps); }); @@ -84,7 +121,7 @@ nav_msgs::msg::Path PathHandler::transformGlobalPlan( // Make sure we always have at least 2 points on the transformed plan and that we don't prune // the global plan below 2 points in order to have always enough point to interpolate the // end of path direction - if (global_plan_.poses.begin() != closest_pose_upper_bound && global_plan_.poses.size() > 1 && + if (global_plan_up_to_inversion_.poses.begin() != closest_pose_upper_bound && global_plan_up_to_inversion_.poses.size() > 1 && transformation_begin == std::prev(closest_pose_upper_bound)) { transformation_begin = std::prev(std::prev(closest_pose_upper_bound)); @@ -93,7 +130,7 @@ nav_msgs::msg::Path PathHandler::transformGlobalPlan( // We'll discard points on the plan that are outside the local costmap const double max_costmap_extent = getCostmapMaxExtent(); auto transformation_end = std::find_if( - transformation_begin, global_plan_.poses.end(), + transformation_begin, global_plan_up_to_inversion_.poses.end(), [&](const auto & global_plan_pose) { return euclidean_distance(global_plan_pose, robot_pose) > max_costmap_extent; }); @@ -101,7 +138,7 @@ nav_msgs::msg::Path PathHandler::transformGlobalPlan( // Lambda to transform a PoseStamped from global frame to local auto transformGlobalPoseToLocal = [&](const auto & global_plan_pose) { geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; - stamped_pose.header.frame_id = global_plan_.header.frame_id; + stamped_pose.header.frame_id = global_plan_up_to_inversion_.header.frame_id; stamped_pose.header.stamp = robot_pose.header.stamp; stamped_pose.pose = global_plan_pose.pose; if (!nav2_util::transformPoseInTargetFrame(stamped_pose, transformed_pose, *tf_, @@ -124,7 +161,15 @@ nav_msgs::msg::Path PathHandler::transformGlobalPlan( // Remove the portion of the global plan that we've already passed so we don't // process it on the next iteration (this is called path pruning) - global_plan_.poses.erase(begin(global_plan_.poses), transformation_begin); + prunePlan(global_plan_up_to_inversion_, transformation_begin); + + if (params_->enforce_path_inversion && inversion_locale_ != 0u) { + if (isWithinInversionTolerances(robot_pose)) { + prunePlan(global_plan_, global_plan_.poses.begin() + inversion_locale_); + global_plan_up_to_inversion_ = global_plan_; + inversion_locale_ = nav2_util::removePosesAfterFirstInversion(global_plan_up_to_inversion_); + } + } if (transformed_plan.poses.empty()) { throw nav2_core::InvalidPath("Resulting plan has 0 poses in it."); diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index fced1dff734..ee2d0f66fd2 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -62,7 +62,7 @@ void RegulatedPurePursuitController::configure( // Handles global path transformations path_handler_ = std::make_unique( - params_->transform_tolerance, tf_, costmap_ros_); + params_, tf_, costmap_ros_); // Checks for imminent collisions collision_checker_ = std::make_unique(node, costmap_ros_, params_); @@ -197,9 +197,11 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity // if the lookahead distance is further than the cusp, use the cusp distance instead if (dist_to_cusp < lookahead_dist) { + std::cout << "11111111" << std::endl; lookahead_dist = dist_to_cusp; } if (dist_to_cusp < curv_lookahead_dist) { + std::cout << "2222222" << std::endl; curv_lookahead_dist = dist_to_cusp; } } diff --git a/nav2_util/include/nav2_util/controller_utils.hpp b/nav2_util/include/nav2_util/controller_utils.hpp index dbe04b1f2f3..b8a526157fd 100644 --- a/nav2_util/include/nav2_util/controller_utils.hpp +++ b/nav2_util/include/nav2_util/controller_utils.hpp @@ -49,6 +49,59 @@ geometry_msgs::msg::PoseStamped getLookAheadPoint( double &, const nav_msgs::msg::Path &, const bool interpolate_after_goal = false); +/** + * @brief Find the iterator of the first pose at which there is an inversion on the path, + * @param path to check for inversion + * @return the first point after the inversion found in the path + */ +inline unsigned int findFirstPathInversion(nav_msgs::msg::Path & path) +{ + // At least 3 poses for a possible inversion + if (path.poses.size() < 3) { + return path.poses.size(); + } + + // Iterating through the path to determine the position of the path inversion + for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) { + // We have two vectors for the dot product OA and AB. Determining the vectors. + float oa_x = path.poses[idx].pose.position.x - + path.poses[idx - 1].pose.position.x; + float oa_y = path.poses[idx].pose.position.y - + path.poses[idx - 1].pose.position.y; + float ab_x = path.poses[idx + 1].pose.position.x - + path.poses[idx].pose.position.x; + float ab_y = path.poses[idx + 1].pose.position.y - + path.poses[idx].pose.position.y; + + // Checking for the existence of cusp, in the path, using the dot product. + float dot_product = (oa_x * ab_x) + (oa_y * ab_y); + if (dot_product < 0.0f) { + return idx + 1; + } + } + + return path.poses.size(); +} + +/** + * @brief Find and remove poses after the first inversion in the path + * @param path to check for inversion + * @return The location of the inversion, return 0 if none exist + */ +inline unsigned int removePosesAfterFirstInversion(nav_msgs::msg::Path & path) +{ + nav_msgs::msg::Path cropped_path = path; + const unsigned int first_after_inversion = findFirstPathInversion(cropped_path); + if (first_after_inversion == path.poses.size()) { + return 0u; + } + + cropped_path.poses.erase( + cropped_path.poses.begin() + first_after_inversion, cropped_path.poses.end()); + path = cropped_path; + return first_after_inversion; +} + } // namespace nav2_util #endif // NAV2_UTIL__CONTROLLER_UTILS_HPP_ diff --git a/nav2_util/test/test_controller_utils.cpp b/nav2_util/test/test_controller_utils.cpp index 3c13d8e9bbc..50bfc010ce8 100644 --- a/nav2_util/test/test_controller_utils.cpp +++ b/nav2_util/test/test_controller_utils.cpp @@ -263,3 +263,66 @@ TEST(InterpolationUtils, lookaheadExtrapolation) EXPECT_NEAR(pt.pose.position.x, cos(-45.0 * M_PI / 180) * 10.0, EPSILON); EXPECT_NEAR(pt.pose.position.y, sin(-45.0 * M_PI / 180) * 10.0, EPSILON); } + +TEST(UtilsTests, FindPathInversionTest) +{ + // Straight path, no inversions to be found + nav_msgs::msg::Path path; + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = i; + path.poses.push_back(pose); + } + EXPECT_EQ(nav2_util::findFirstPathInversion(path), 10u); + + // To short to process + path.poses.erase(path.poses.begin(), path.poses.begin() + 7); + EXPECT_EQ(nav2_util::findFirstPathInversion(path), 3u); + + // Has inversion at index 10, so should return 11 for the first point afterwards + // 0 1 2 3 4 5 6 7 8 9 10 **9** 8 7 6 5 4 3 2 1 + path.poses.clear(); + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = i; + path.poses.push_back(pose); + } + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = 10 - i; + path.poses.push_back(pose); + } + EXPECT_EQ(nav2_util::findFirstPathInversion(path), 11u); +} + +TEST(UtilsTests, RemovePosesAfterPathInversionTest) +{ + nav_msgs::msg::Path path; + // straight path + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = i; + path.poses.push_back(pose); + } + EXPECT_EQ(nav2_util::removePosesAfterFirstInversion(path), 0u); + + // try empty path + path.poses.clear(); + EXPECT_EQ(nav2_util::removePosesAfterFirstInversion(path), 0u); + + // cusping path + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = i; + path.poses.push_back(pose); + } + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = 10 - i; + path.poses.push_back(pose); + } + EXPECT_EQ(nav2_util::removePosesAfterFirstInversion(path), 11u); + // Check to see if removed + EXPECT_EQ(path.poses.size(), 11u); + EXPECT_EQ(path.poses.back().pose.position.x, 10); +} \ No newline at end of file From 4326ef93e734a33e7cc0856c9c684e449aab8a0f Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Fri, 29 Aug 2025 02:06:38 +0800 Subject: [PATCH 2/4] Fix transform tolerance Signed-off-by: mini-1235 --- .../path_handler.hpp | 1 - .../src/path_handler.cpp | 4 ++-- .../src/regulated_pure_pursuit_controller.cpp | 16 ---------------- 3 files changed, 2 insertions(+), 19 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp index 0460edd4d86..804f950a5eb 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp @@ -93,7 +93,6 @@ class PathHandler double getCostmapMaxExtent() const; rclcpp::Logger logger_ {rclcpp::get_logger("RPPPathHandler")}; - double transform_tolerance_; std::shared_ptr tf_; std::shared_ptr costmap_ros_; nav_msgs::msg::Path global_plan_; diff --git a/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp index 7072bbc1843..3d5644ff19a 100644 --- a/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp @@ -99,7 +99,7 @@ nav_msgs::msg::Path PathHandler::transformGlobalPlan( // let's get the pose of the robot in the frame of the plan geometry_msgs::msg::PoseStamped robot_pose; if (!nav2_util::transformPoseInTargetFrame(pose, robot_pose, *tf_, global_plan_up_to_inversion_.header.frame_id, - transform_tolerance_)) + params_->transform_tolerance)) { throw nav2_core::ControllerTFError("Unable to transform robot pose into global plan's frame"); } @@ -142,7 +142,7 @@ nav_msgs::msg::Path PathHandler::transformGlobalPlan( stamped_pose.header.stamp = robot_pose.header.stamp; stamped_pose.pose = global_plan_pose.pose; if (!nav2_util::transformPoseInTargetFrame(stamped_pose, transformed_pose, *tf_, - costmap_ros_->getBaseFrameID(), transform_tolerance_)) + costmap_ros_->getBaseFrameID(), params_->transform_tolerance)) { throw nav2_core::ControllerTFError("Unable to transform plan pose into local frame"); } diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index ee2d0f66fd2..7b353444826 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -190,22 +190,6 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity double lookahead_dist = getLookAheadDistance(speed); double curv_lookahead_dist = params_->curvature_lookahead_dist; - // Check for reverse driving - if (params_->allow_reversing) { - // Cusp check - const double dist_to_cusp = findVelocitySignChange(transformed_plan); - - // if the lookahead distance is further than the cusp, use the cusp distance instead - if (dist_to_cusp < lookahead_dist) { - std::cout << "11111111" << std::endl; - lookahead_dist = dist_to_cusp; - } - if (dist_to_cusp < curv_lookahead_dist) { - std::cout << "2222222" << std::endl; - curv_lookahead_dist = dist_to_cusp; - } - } - // Get the particular point on the path at the lookahead distance auto carrot_pose = nav2_util::getLookAheadPoint(lookahead_dist, transformed_plan); auto rotate_to_path_carrot_pose = carrot_pose; From e035275afaa99b6c64506db40741eb333ac5780f Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Thu, 28 Aug 2025 18:33:14 +0000 Subject: [PATCH 3/4] Linting Signed-off-by: mini-1235 --- .../path_handler.hpp | 1 - .../src/path_handler.cpp | 18 +++++++++++------- nav2_util/test/test_controller_utils.cpp | 2 +- 3 files changed, 12 insertions(+), 9 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp index 804f950a5eb..d17bf497df4 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp @@ -99,7 +99,6 @@ class PathHandler nav_msgs::msg::Path global_plan_up_to_inversion_; unsigned int inversion_locale_{0u}; Parameters * params_; - }; } // namespace nav2_regulated_pure_pursuit_controller diff --git a/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp index 3d5644ff19a..fc33b6448e3 100644 --- a/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp @@ -41,7 +41,7 @@ PathHandler::PathHandler( costmap_ros_ = costmap_ros; tf_ = tf; - if(params_->enforce_path_inversion){ + if(params_->enforce_path_inversion) { inversion_locale_ = 0u; } } @@ -71,14 +71,15 @@ bool PathHandler::isWithinInversionTolerances(const geometry_msgs::msg::PoseStam tf2::getYaw(robot_pose.pose.orientation), tf2::getYaw(last_pose.pose.orientation)); - return distance <= params_->inversion_xy_tolerance && fabs(angle_distance) <= params_->inversion_yaw_tolerance; + return distance <= params_->inversion_xy_tolerance && + fabs(angle_distance) <= params_->inversion_yaw_tolerance; } -void PathHandler::setPlan(const nav_msgs::msg::Path & path) +void PathHandler::setPlan(const nav_msgs::msg::Path & path) { global_plan_ = path; global_plan_up_to_inversion_ = global_plan_; - if(params_->enforce_path_inversion){ + if(params_->enforce_path_inversion) { inversion_locale_ = nav2_util::removePosesAfterFirstInversion(global_plan_up_to_inversion_); } } @@ -98,7 +99,8 @@ nav_msgs::msg::Path PathHandler::transformGlobalPlan( // let's get the pose of the robot in the frame of the plan geometry_msgs::msg::PoseStamped robot_pose; - if (!nav2_util::transformPoseInTargetFrame(pose, robot_pose, *tf_, global_plan_up_to_inversion_.header.frame_id, + if (!nav2_util::transformPoseInTargetFrame(pose, robot_pose, *tf_, + global_plan_up_to_inversion_.header.frame_id, params_->transform_tolerance)) { throw nav2_core::ControllerTFError("Unable to transform robot pose into global plan's frame"); @@ -106,7 +108,8 @@ nav_msgs::msg::Path PathHandler::transformGlobalPlan( auto closest_pose_upper_bound = nav2_util::geometry_utils::first_after_integrated_distance( - global_plan_up_to_inversion_.poses.begin(), global_plan_up_to_inversion_.poses.end(), max_robot_pose_search_dist); + global_plan_up_to_inversion_.poses.begin(), global_plan_up_to_inversion_.poses.end(), + max_robot_pose_search_dist); // First find the closest pose on the path to the robot // bounded by when the path turns around (if it does) so we don't get a pose from a later @@ -121,7 +124,8 @@ nav_msgs::msg::Path PathHandler::transformGlobalPlan( // Make sure we always have at least 2 points on the transformed plan and that we don't prune // the global plan below 2 points in order to have always enough point to interpolate the // end of path direction - if (global_plan_up_to_inversion_.poses.begin() != closest_pose_upper_bound && global_plan_up_to_inversion_.poses.size() > 1 && + if (global_plan_up_to_inversion_.poses.begin() != closest_pose_upper_bound && + global_plan_up_to_inversion_.poses.size() > 1 && transformation_begin == std::prev(closest_pose_upper_bound)) { transformation_begin = std::prev(std::prev(closest_pose_upper_bound)); diff --git a/nav2_util/test/test_controller_utils.cpp b/nav2_util/test/test_controller_utils.cpp index 50bfc010ce8..a4620a58b79 100644 --- a/nav2_util/test/test_controller_utils.cpp +++ b/nav2_util/test/test_controller_utils.cpp @@ -325,4 +325,4 @@ TEST(UtilsTests, RemovePosesAfterPathInversionTest) // Check to see if removed EXPECT_EQ(path.poses.size(), 11u); EXPECT_EQ(path.poses.back().pose.position.x, 10); -} \ No newline at end of file +} From c40fa8ee1b17757c52b377dd2ee33b53a48b21ec Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Sat, 6 Sep 2025 21:26:18 +0800 Subject: [PATCH 4/4] Add in place rotation for enforce path inversion Signed-off-by: mini-1235 --- .../regulated_pure_pursuit_controller.hpp | 7 --- .../src/regulated_pure_pursuit_controller.cpp | 47 ------------------- .../test/test_regulated_pp.cpp | 46 ------------------ .../include/nav2_util/controller_utils.hpp | 14 ++++++ 4 files changed, 14 insertions(+), 100 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index fd79208d291..c16aff2a0c1 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -174,13 +174,6 @@ class RegulatedPurePursuitController : public nav2_core::Controller const double & pose_cost, const nav_msgs::msg::Path & path, double & linear_vel, double & sign); - /** - * @brief checks for the cusp position - * @param pose Pose input to determine the cusp position - * @return robot distance from the cusp - */ - double findVelocitySignChange(const nav_msgs::msg::Path & transformed_plan); - nav2::LifecycleNode::WeakPtr node_; std::shared_ptr tf_; std::string plugin_name_; diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index 7b353444826..26939679520 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -408,53 +408,6 @@ void RegulatedPurePursuitController::reset() finished_cancelling_ = false; has_reached_xy_tolerance_ = false; } - -double RegulatedPurePursuitController::findVelocitySignChange( - const nav_msgs::msg::Path & transformed_plan) -{ - // Iterating through the transformed global path to determine the position of the cusp - for (unsigned int pose_id = 1; pose_id < transformed_plan.poses.size() - 1; ++pose_id) { - // We have two vectors for the dot product OA and AB. Determining the vectors. - double oa_x = transformed_plan.poses[pose_id].pose.position.x - - transformed_plan.poses[pose_id - 1].pose.position.x; - double oa_y = transformed_plan.poses[pose_id].pose.position.y - - transformed_plan.poses[pose_id - 1].pose.position.y; - double ab_x = transformed_plan.poses[pose_id + 1].pose.position.x - - transformed_plan.poses[pose_id].pose.position.x; - double ab_y = transformed_plan.poses[pose_id + 1].pose.position.y - - transformed_plan.poses[pose_id].pose.position.y; - - /* Checking for the existence of cusp, in the path, using the dot product - and determine it's distance from the robot. If there is no cusp in the path, - then just determine the distance to the goal location. */ - const double dot_prod = (oa_x * ab_x) + (oa_y * ab_y); - if (dot_prod < 0.0) { - // returning the distance if there is a cusp - // The transformed path is in the robots frame, so robot is at the origin - return hypot( - transformed_plan.poses[pose_id].pose.position.x, - transformed_plan.poses[pose_id].pose.position.y); - } - - if ( - (hypot(oa_x, oa_y) == 0.0 && - transformed_plan.poses[pose_id - 1].pose.orientation != - transformed_plan.poses[pose_id].pose.orientation) - || - (hypot(ab_x, ab_y) == 0.0 && - transformed_plan.poses[pose_id].pose.orientation != - transformed_plan.poses[pose_id + 1].pose.orientation)) - { - // returning the distance since the points overlap - // but are not simply duplicate points (e.g. in place rotation) - return hypot( - transformed_plan.poses[pose_id].pose.position.x, - transformed_plan.poses[pose_id].pose.position.y); - } - } - - return std::numeric_limits::max(); -} } // namespace nav2_regulated_pure_pursuit_controller // Register this controller as a nav2_core plugin diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp index 4b3dbb905f8..8e8ec9ff6ec 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -80,12 +80,6 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure linear_vel, sign); } - double findVelocitySignChangeWrapper( - const nav_msgs::msg::Path & transformed_plan) - { - return findVelocitySignChange(transformed_plan); - } - nav_msgs::msg::Path transformGlobalPlanWrapper( const geometry_msgs::msg::PoseStamped & pose) { @@ -145,46 +139,6 @@ TEST(RegulatedPurePursuitTest, createCarrotMsg) EXPECT_EQ(rtn->point.z, 0.01); } -TEST(RegulatedPurePursuitTest, findVelocitySignChange) -{ - auto node = std::make_shared("testRPPfindVelocitySignChange"); - auto ctrl = std::make_shared(); - - std::string name = "PathFollower"; - auto tf = std::make_shared(node->get_clock()); - auto costmap = std::make_shared("fake_costmap"); - rclcpp_lifecycle::State state; - costmap->on_configure(state); - ctrl->configure(node, name, tf, costmap); - - geometry_msgs::msg::PoseStamped pose; - pose.header.frame_id = "smb"; - auto time = node->get_clock()->now(); - pose.header.stamp = time; - pose.pose.position.x = 1.0; - pose.pose.position.y = 0.0; - - nav_msgs::msg::Path path; - path.poses.resize(3); - path.header.frame_id = "smb"; - path.header.stamp = pose.header.stamp; - path.poses[0].pose.position.x = 1.0; - path.poses[0].pose.position.y = 1.0; - path.poses[1].pose.position.x = 2.0; - path.poses[1].pose.position.y = 2.0; - path.poses[2].pose.position.x = -1.0; - path.poses[2].pose.position.y = -1.0; - ctrl->setPlan(path); - auto rtn = ctrl->findVelocitySignChangeWrapper(path); - EXPECT_EQ(rtn, sqrt(8.0)); - - path.poses[2].pose.position.x = 3.0; - path.poses[2].pose.position.y = 3.0; - ctrl->setPlan(path); - rtn = ctrl->findVelocitySignChangeWrapper(path); - EXPECT_EQ(rtn, std::numeric_limits::max()); -} - TEST(RegulatedPurePursuitTest, lookaheadAPI) { auto ctrl = std::make_shared(); diff --git a/nav2_util/include/nav2_util/controller_utils.hpp b/nav2_util/include/nav2_util/controller_utils.hpp index b8a526157fd..46168d4c351 100644 --- a/nav2_util/include/nav2_util/controller_utils.hpp +++ b/nav2_util/include/nav2_util/controller_utils.hpp @@ -78,6 +78,20 @@ inline unsigned int findFirstPathInversion(nav_msgs::msg::Path & path) if (dot_product < 0.0f) { return idx + 1; } + + if ( + (hypot(oa_x, oa_y) == 0.0 && + path.poses[idx - 1].pose.orientation != + path.poses[idx].pose.orientation) + || + (hypot(ab_x, ab_y) == 0.0 && + path.poses[idx].pose.orientation != + path.poses[idx + 1].pose.orientation)) + { + // returning the distance since the points overlap + // but are not simply duplicate points (e.g. in place rotation) + return idx + 1; + } } return path.poses.size();