From 07eb31cbb315634c6955e28a258d979c9a0d2f6a Mon Sep 17 00:00:00 2001 From: andreacelani Date: Sun, 24 Nov 2024 12:15:28 +0100 Subject: [PATCH] fixed cusp point navigation for RPP controller --- .../regulated_pure_pursuit_controller.hpp | 113 ++++++++++++++++- .../src/regulated_pure_pursuit_controller.cpp | 118 ++++++++++-------- .../test/test_regulated_pp.cpp | 38 ------ 3 files changed, 171 insertions(+), 98 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 7c244d93932..ad15369e5b2 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 @@ -33,6 +33,7 @@ namespace nav2_regulated_pure_pursuit_controller { + using PathIterator = std::vector::iterator; /** * @class nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController @@ -244,6 +245,44 @@ class RegulatedPurePursuitController : public nav2_core::Controller const geometry_msgs::msg::Point & p2, double r); + /** + * @brief normalize + * Normalizes the angle to be -M_PI circle to +M_PI circle + * It takes and returns radians. + * @param angles Angles to normalize + * @return normalized angles + */ + template + auto normalize_angles(const T & angles) + { + auto && theta = fmod(angles + M_PI, 2.0 * M_PI); + if (theta < 0) + theta += 360; + return theta - M_PI; + } + + + /** + * @brief shortest_angular_distance + * + * Given 2 angles, this returns the shortest angular + * difference. The inputs and ouputs are of course radians. + * + * The result + * would always be -pi <= result <= pi. Adding the result + * to "from" will always get you an equivelent angle to "to". + * @param from Start angle + * @param to End angle + * @return Shortest distance between angles + */ + template + auto shortest_angular_distance( + const F & from, + const T & to) + { + return normalize_angles(to - from); + } + /** * @brief Get lookahead point * @param lookahead_dist Optimal lookahead distance @@ -251,13 +290,73 @@ class RegulatedPurePursuitController : public nav2_core::Controller * @return Lookahead point */ geometry_msgs::msg::PoseStamped getLookAheadPoint(const double &, const nav_msgs::msg::Path &); - + + /** + * @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 existance of cusp, in the path, using the dot product. + float dot_product = (oa_x * ab_x) + (oa_y * ab_y); + if (dot_product < 0.0) { + return idx + 1; + } + } + + return path.poses.size(); + } + /** - * @brief checks for the cusp position - * @param pose Pose input to determine the cusp position - * @return robot distance from the cusp + * @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 */ - double findVelocitySignChange(const nav_msgs::msg::Path & transformed_plan); + 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 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); + + /** + * @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); /** * Get the greatest extent of the costmap in meters from the center. @@ -307,8 +406,12 @@ class RegulatedPurePursuitController : public nav2_core::Controller bool allow_reversing_; double max_robot_pose_search_dist_; bool use_interpolation_; + float inversion_xy_tolerance_{0.2}; + float inversion_yaw_tolerance_{0.4}; + unsigned int inversion_locale_{0u}; nav_msgs::msg::Path global_plan_; + nav_msgs::msg::Path global_plan_up_to_inversion_; std::shared_ptr> global_path_pub_; std::shared_ptr> carrot_pub_; 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 48703cc66dd..32c9831b7da 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 @@ -19,6 +19,7 @@ #include #include #include +#include #include "nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp" #include "nav2_core/exceptions.hpp" @@ -111,6 +112,10 @@ void RegulatedPurePursuitController::configure( node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2)); declare_parameter_if_not_declared( node, plugin_name_ + ".allow_reversing", 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)); declare_parameter_if_not_declared( node, plugin_name_ + ".max_robot_pose_search_dist", rclcpp::ParameterValue(getCostmapMaxExtent())); @@ -169,6 +174,9 @@ void RegulatedPurePursuitController::configure( node->get_parameter(plugin_name_ + ".rotate_to_heading_min_angle", rotate_to_heading_min_angle_); node->get_parameter(plugin_name_ + ".max_angular_accel", max_angular_accel_); node->get_parameter(plugin_name_ + ".allow_reversing", allow_reversing_); + node->get_parameter(plugin_name_ + "inversion_xy_tolerance", inversion_xy_tolerance_); + node->get_parameter(plugin_name_ + "inversion_yaw_tolerance", inversion_yaw_tolerance_); + inversion_locale_ = 0u; node->get_parameter("controller_frequency", control_frequency); node->get_parameter( plugin_name_ + ".max_robot_pose_search_dist", @@ -293,25 +301,16 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity } else { goal_dist_tol_ = pose_tolerance.position.x; } - + // Transform path to robot base frame auto transformed_plan = transformGlobalPlan(pose); // Find look ahead distance and point on path and publish double lookahead_dist = getLookAheadDistance(speed); - - // Check for reverse driving - if (allow_reversing_) { - // Cusp check - 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) { - lookahead_dist = dist_to_cusp; - } - } - - auto carrot_pose = getLookAheadPoint(lookahead_dist, transformed_plan); + + geometry_msgs::msg::PoseStamped carrot_pose; + carrot_pose = getLookAheadPoint(lookahead_dist, transformed_plan); + carrot_pub_->publish(createCarrotMsg(carrot_pose)); double linear_vel, angular_vel; @@ -669,6 +668,10 @@ void RegulatedPurePursuitController::applyConstraints( void RegulatedPurePursuitController::setPlan(const nav_msgs::msg::Path & path) { global_plan_ = path; + global_plan_up_to_inversion_ = global_plan_; + if (allow_reversing_) { + inversion_locale_ = removePosesAfterFirstInversion(global_plan_up_to_inversion_); + } } void RegulatedPurePursuitController::setSpeedLimit( @@ -692,40 +695,42 @@ void RegulatedPurePursuitController::setSpeedLimit( nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan( const geometry_msgs::msg::PoseStamped & pose) { - if (global_plan_.poses.empty()) { + if (global_plan_up_to_inversion_.poses.empty()) { throw nav2_core::PlannerException("Received plan with zero length"); } // let's get the pose of the robot in the frame of the plan geometry_msgs::msg::PoseStamped robot_pose; - if (!transformPose(global_plan_.header.frame_id, pose, robot_pose)) { + if (!transformPose(global_plan_up_to_inversion_.header.frame_id, pose, robot_pose)) { throw nav2_core::PlannerException("Unable to transform robot pose into global plan's frame"); } // We'll discard points on the plan that are outside the local costmap double max_costmap_extent = getCostmapMaxExtent(); - + + auto begin = global_plan_up_to_inversion_.poses.begin(); + 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, + begin, closest_pose_upper_bound, [&robot_pose](const geometry_msgs::msg::PoseStamped & ps) { return euclidean_distance(robot_pose, ps); }); // Find points up to max_transform_dist so we only transform them. auto transformation_end = std::find_if( - transformation_begin, global_plan_.poses.end(), + transformation_begin, global_plan_up_to_inversion_.poses.end(), [&](const auto & pose) { return euclidean_distance(pose, robot_pose) > max_costmap_extent; }); - + // 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; @@ -745,10 +750,18 @@ nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan( transformGlobalPoseToLocal); transformed_plan.header.frame_id = costmap_ros_->getBaseFrameID(); transformed_plan.header.stamp = robot_pose.header.stamp; - - // 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 (allow_reversing_ && inversion_locale_ != 0u) { + if (isWithinInversionTolerances(robot_pose)) { + // Remove the portion of the global plan from starting point up to inversion_locale_ (index) + prunePlan(global_plan_, global_plan_.poses.begin() + inversion_locale_); + global_plan_up_to_inversion_ = global_plan_; + inversion_locale_ = removePosesAfterFirstInversion(global_plan_up_to_inversion_); + } + } + global_path_pub_->publish(transformed_plan); if (transformed_plan.poses.empty()) { @@ -758,36 +771,6 @@ nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan( return transformed_plan; } -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 existance 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. */ - if ( (oa_x * ab_x) + (oa_y * ab_y) < 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); - } - } - - return std::numeric_limits::max(); -} - bool RegulatedPurePursuitController::transformPose( const std::string frame, const geometry_msgs::msg::PoseStamped & in_pose, @@ -868,6 +851,10 @@ RegulatedPurePursuitController::dynamicParametersCallback( max_angular_accel_ = parameter.as_double(); } else if (name == plugin_name_ + ".rotate_to_heading_min_angle") { rotate_to_heading_min_angle_ = parameter.as_double(); + } else if (name == plugin_name_ + ".inversion_xy_tolerance") { + inversion_xy_tolerance_ = parameter.as_double(); + } else if (name == plugin_name_ + ".inversion_yaw_tolerance") { + inversion_yaw_tolerance_ = parameter.as_double(); } } else if (type == ParameterType::PARAMETER_BOOL) { if (name == plugin_name_ + ".use_velocity_scaled_lookahead_dist") { @@ -899,6 +886,27 @@ RegulatedPurePursuitController::dynamicParametersCallback( result.successful = true; return result; } + +bool RegulatedPurePursuitController::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 = shortest_angular_distance( + tf2::getYaw(robot_pose.pose.orientation), + tf2::getYaw(last_pose.pose.orientation)); + + + return distance <= inversion_xy_tolerance_ && fabs(angle_distance) <= inversion_yaw_tolerance_; +} + + void RegulatedPurePursuitController::prunePlan(nav_msgs::msg::Path & plan, const PathIterator end) +{ + plan.poses.erase(plan.poses.begin(), end); +} } // namespace nav2_regulated_pure_pursuit_controller 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 83d814dda07..e95956461f4 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -101,12 +101,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) { @@ -166,38 +160,6 @@ TEST(RegulatedPurePursuitTest, createCarrotMsg) EXPECT_EQ(rtn->point.z, 0.01); } -TEST(RegulatedPurePursuitTest, findVelocitySignChange) -{ - auto ctrl = std::make_shared(); - auto node = std::make_shared("testRPPfindVelocitySignChange"); - 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()); -} - using CircleSegmentIntersectionParam = std::tuple< std::pair, std::pair,