diff --git a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt index d64353d94a..7c2be316ee 100644 --- a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt +++ b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt @@ -11,6 +11,8 @@ find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(pluginlib REQUIRED) find_package(tf2 REQUIRED) +find_package(ruckig REQUIRED) +find_package(angles REQUIRED) nav2_package() set(CMAKE_CXX_STANDARD 17) @@ -28,13 +30,15 @@ set(dependencies nav2_util nav2_core tf2 + ruckig + angles ) set(library_name nav2_regulated_pure_pursuit_controller) add_library(${library_name} SHARED src/regulated_pure_pursuit_controller.cpp) - +target_link_libraries(${library_name} ruckig::ruckig) ament_target_dependencies(${library_name} ${dependencies} ) diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index 6ef3260d6b..579432f169 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -36,7 +36,7 @@ The Regulated Pure Pursuit controller implements active collision detection. We The regulated pure pursuit algorithm also makes use of the common variations on the pure pursuit algorithm. We implement the adaptive pure pursuit's main contribution of having velocity-scaled lookahead point distances. This helps make the controller more stable over a larger range of potential linear velocities. There are parameters for setting the lookahead gain (or lookahead time) and thresholded values for minimum and maximum. -The final minor improvement we make is slowing on approach to the goal. Knowing that the optimal lookahead distance is `X`, we can take the difference in `X` and the actual distance of the lookahead point found to find the lookahead point error. During operations, the variation in this error should be exceptionally small and won't be triggered. However, at the end of the path, there are no more points at a lookahead distance away from the robot, so it uses the last point on the path. So as the robot approaches a target, its error will grow and the robot's velocity will be reduced proportional to this error until a minimum threshold. This is also tracked by the kinematic speed limits to ensure drivability. +The final minor improvement we make is slowing on approach to the goal. Knowing the remaining path length we are able to calculate motion profile that brings us to stop on the goal point. In case of robots that are able to rotate in place, there is a proportional angle controller implemented in order to precisely track the angle setpoint. The major improvements that this work implements is the regulations on the linear velocity based on some cost functions. They were selected to remove long-standing bad behavior within the pure pursuit algorithm. Normal Pure Pursuit has an issue with overshoot and poor handling in particularly high curvature (or extremely rapidly changing curvature) environments. It is commonly known that this will cause the robot to overshoot from the path and potentially collide with the environment. These cost functions in the Regulated Pure Pursuit algorithm were also chosen based on common requirements and needs of mobile robots uses in service, commercial, and industrial use-cases; scaling by curvature creates intuitive behavior of slowing the robot when making sharp turns and slowing when its near a potential collision so that small variations don't clip obstacles. This is also really useful when working in partially observable environments (like turning in and out of aisles / hallways often) so that you slow before a sharp turn into an unknown dynamic environment to be more conservative in case something is in the way immediately requiring a stop. @@ -52,9 +52,12 @@ Note: The maximum allowed time to collision is thresholded by the lookahead poin | Parameter | Description | |-----|----| -| `desired_linear_vel` | The desired maximum linear velocity to use. | -| `max_linear_accel` | Acceleration for linear velocity | -| `max_linear_decel` | Deceleration for linear velocity | +| `desired_linear_vel` | The desired maximum linear velocity to use. | +| `max_linear_accel` | Acceleration for linear velocity | +| `max_linear_decel` | Deceleration for linear velocity | +| `max_linear_jerk` | Jerk for linear velocity | +| `max_angular_jerk` | Jerk for angular velocity | +| `kp_angle` | Proportional regulator gain while rotating in place | | `lookahead_dist` | The lookahead distance to use to find the lookahead point | | `min_lookahead_dist` | The minimum lookahead distance threshold when using velocity scaled lookahead distances | | `max_lookahead_dist` | The maximum lookahead distance threshold when using velocity scaled lookahead distances | @@ -62,8 +65,6 @@ Note: The maximum allowed time to collision is thresholded by the lookahead poin | `rotate_to_heading_angular_vel` | If rotate to heading is used, this is the angular velocity to use. | | `transform_tolerance` | The TF transform tolerance | | `use_velocity_scaled_lookahead_dist` | Whether to use the velocity scaled lookahead distances or constant `lookahead_distance` | -| `min_approach_linear_velocity` | The minimum velocity threshold to apply when approaching the goal | -| `use_approach_linear_velocity_scaling` | Whether to scale the linear velocity down on approach to the goal for a smooth stop | | `max_allowed_time_to_collision_up_to_carrot` | The time to project a velocity command to check for collisions, limited to maximum distance of lookahead distance selected | | `use_regulated_linear_velocity_scaling` | Whether to use the regulated features for curvature | | `use_cost_regulated_linear_velocity_scaling` | Whether to use the regulated features for proximity to obstacles | @@ -106,6 +107,9 @@ controller_server: desired_linear_vel: 0.5 max_linear_accel: 2.5 max_linear_decel: 2.5 + max_linear_jerk: 10000.0 + max_angular_jerk: 10000.0 + kp_angle: 3.0 lookahead_dist: 0.6 min_lookahead_dist: 0.3 max_lookahead_dist: 0.9 @@ -113,8 +117,6 @@ controller_server: rotate_to_heading_angular_vel: 1.8 transform_tolerance: 0.1 use_velocity_scaled_lookahead_dist: false - min_approach_linear_velocity: 0.05 - use_approach_linear_velocity_scaling: true max_allowed_time_to_collision_up_to_carrot: 1.0 use_regulated_linear_velocity_scaling: true use_cost_regulated_linear_velocity_scaling: false @@ -150,3 +152,5 @@ To tune to get Pure Pursuit behaviors, set all boolean parameters to false and m Currently, there is no rotate to goal behaviors, so it is expected that the path approach orientations are the orientations of the goal or the goal checker has been set with a generous `min_theta_velocity_threshold`. Implementations for rotating to goal heading are on the way. The choice of lookahead distances are highly dependent on robot size, responsiveness, controller update rate, and speed. Please make sure to tune this for your platform, although the `regulated` features do largely make heavy tuning of this value unnecessary. If you see wiggling, increase the distance or scale. If it's not converging as fast to the path as you'd like, decrease it. + +Default jerk limits are set to high values. This essentially generates trapezoidal velocity profiles and fits most of users. Advanced users can change these values but should be aware that too low jerk limits can lead to poor path tracking and oscillations. 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 15500cf056..1c1ee42ce9 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 @@ -30,6 +30,7 @@ #include "nav2_util/odometry_utils.hpp" #include "nav2_util/geometry_utils.hpp" #include "geometry_msgs/msg/pose2_d.hpp" +#include "ruckig/ruckig.hpp" namespace nav2_regulated_pure_pursuit_controller { @@ -160,9 +161,11 @@ class RegulatedPurePursuitController : public nav2_core::Controller /** * @brief Whether robot should rotate to final goal orientation * @param carrot_pose current lookahead point + * @param lookahead_dist current lookahead distance * @return Whether should rotate to goal heading */ - bool shouldRotateToGoalHeading(const geometry_msgs::msg::PoseStamped & carrot_pose); + bool shouldRotateToGoalHeading( + const geometry_msgs::msg::PoseStamped & carrot_pose, const double & lookahead_dist); /** * @brief Create a smooth and kinematically smoothed rotation command @@ -240,9 +243,11 @@ class RegulatedPurePursuitController : public nav2_core::Controller * @brief Get lookahead point * @param lookahead_dist Optimal lookahead distance * @param path Current global path + * @param remaining_path_length path length from lookahead point to the end of pruned path * @return Lookahead point */ - geometry_msgs::msg::PoseStamped getLookAheadPoint(const double &, const nav_msgs::msg::Path &); + geometry_msgs::msg::PoseStamped getLookAheadPoint( + const double &, const nav_msgs::msg::Path &, double & remaining_path_length); /** * @brief checks for the cusp position @@ -273,6 +278,8 @@ class RegulatedPurePursuitController : public nav2_core::Controller rclcpp::Clock::SharedPtr clock_; double desired_linear_vel_, base_desired_linear_vel_; + double max_linear_accel_; + double max_linear_decel_; double lookahead_dist_; double rotate_to_heading_angular_vel_; double max_lookahead_dist_; @@ -280,7 +287,6 @@ class RegulatedPurePursuitController : public nav2_core::Controller double lookahead_time_; bool use_velocity_scaled_lookahead_dist_; tf2::Duration transform_tolerance_; - double min_approach_linear_velocity_; double control_duration_; double max_allowed_time_to_collision_up_to_carrot_; bool use_regulated_linear_velocity_scaling_; @@ -295,6 +301,11 @@ class RegulatedPurePursuitController : public nav2_core::Controller double rotate_to_heading_min_angle_; double goal_dist_tol_; bool allow_reversing_; + double robot_angle_; + double kp_angle_; + double max_linear_jerk_; + double max_angular_jerk_; + rclcpp::Time system_time_; double max_robot_pose_search_dist_; bool use_interpolation_; @@ -306,6 +317,16 @@ class RegulatedPurePursuitController : public nav2_core::Controller std::unique_ptr> collision_checker_; + std::unique_ptr> distance_profile_; + ruckig::InputParameter<1> distance_profile_input_; + ruckig::OutputParameter<1> distance_profile_output_; + ruckig::Result distance_profile_result_; + + std::unique_ptr> angle_profile_; + ruckig::InputParameter<1> angle_profile_input_; + ruckig::OutputParameter<1> angle_profile_output_; + ruckig::Result angle_profile_result_; + // Dynamic parameters handler std::mutex mutex_; rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index 0420ba04d2..8cea88ea4a 100644 --- a/nav2_regulated_pure_pursuit_controller/package.xml +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -19,6 +19,8 @@ nav2_msgs pluginlib tf2 + ruckig + angles ament_cmake_gtest ament_lint_common 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 9441f2d69f..1f92f8cef2 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 @@ -25,6 +25,7 @@ #include "nav2_util/node_utils.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_costmap_2d/costmap_filters/filter_values.hpp" +#include "angles/angles/angles.h" using std::hypot; using std::min; @@ -62,6 +63,16 @@ void RegulatedPurePursuitController::configure( declare_parameter_if_not_declared( node, plugin_name_ + ".desired_linear_vel", rclcpp::ParameterValue(0.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_linear_accel", rclcpp::ParameterValue(2.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_linear_decel", rclcpp::ParameterValue(2.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_linear_jerk", rclcpp::ParameterValue(10000.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_angular_jerk", rclcpp::ParameterValue(10000.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".kp_angle", rclcpp::ParameterValue(3.0)); declare_parameter_if_not_declared( node, plugin_name_ + ".lookahead_dist", rclcpp::ParameterValue(0.6)); declare_parameter_if_not_declared( @@ -77,8 +88,6 @@ void RegulatedPurePursuitController::configure( declare_parameter_if_not_declared( node, plugin_name_ + ".use_velocity_scaled_lookahead_dist", rclcpp::ParameterValue(false)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".min_approach_linear_velocity", rclcpp::ParameterValue(0.05)); declare_parameter_if_not_declared( node, plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", rclcpp::ParameterValue(1.0)); @@ -112,8 +121,14 @@ void RegulatedPurePursuitController::configure( node, plugin_name_ + ".use_interpolation", rclcpp::ParameterValue(true)); + node->get_parameter(plugin_name_ + ".desired_linear_vel", desired_linear_vel_); base_desired_linear_vel_ = desired_linear_vel_; + node->get_parameter(plugin_name_ + ".max_linear_accel", max_linear_accel_); + node->get_parameter(plugin_name_ + ".max_linear_decel", max_linear_decel_); + node->get_parameter(plugin_name_ + ".max_linear_jerk", max_linear_jerk_); + node->get_parameter(plugin_name_ + ".max_angular_jerk", max_angular_jerk_); + node->get_parameter(plugin_name_ + ".kp_angle", kp_angle_); node->get_parameter(plugin_name_ + ".lookahead_dist", lookahead_dist_); node->get_parameter(plugin_name_ + ".min_lookahead_dist", min_lookahead_dist_); node->get_parameter(plugin_name_ + ".max_lookahead_dist", max_lookahead_dist_); @@ -125,9 +140,6 @@ void RegulatedPurePursuitController::configure( node->get_parameter( plugin_name_ + ".use_velocity_scaled_lookahead_dist", use_velocity_scaled_lookahead_dist_); - node->get_parameter( - plugin_name_ + ".min_approach_linear_velocity", - min_approach_linear_velocity_); node->get_parameter( plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", max_allowed_time_to_collision_up_to_carrot_); @@ -188,6 +200,11 @@ void RegulatedPurePursuitController::configure( collision_checker_ = std::make_unique>(costmap_); collision_checker_->setCostmap(costmap_); + + distance_profile_ = std::make_unique>(control_duration_); + angle_profile_ = std::make_unique>(control_duration_); + + system_time_ = clock_->now(); } void RegulatedPurePursuitController::cleanup() @@ -200,6 +217,9 @@ void RegulatedPurePursuitController::cleanup() global_path_pub_.reset(); carrot_pub_.reset(); carrot_arc_pub_.reset(); + + distance_profile_.reset(); + angle_profile_.reset(); } void RegulatedPurePursuitController::activate() @@ -218,6 +238,33 @@ void RegulatedPurePursuitController::activate() std::bind( &RegulatedPurePursuitController::dynamicParametersCallback, this, std::placeholders::_1)); + + // Distance Motion Profile + distance_profile_input_.max_velocity = {desired_linear_vel_}; + distance_profile_input_.max_acceleration = {max_linear_accel_}; + distance_profile_input_.min_acceleration = {-max_linear_decel_}; + distance_profile_input_.max_jerk = {max_linear_jerk_}; + distance_profile_input_.target_position = {0.0}; + distance_profile_input_.target_velocity = {0.0}; + // Reset current state + distance_profile_input_.control_interface = ruckig::ControlInterface::Position; + distance_profile_output_.new_position = {0.0}; + distance_profile_output_.new_velocity = {0.0}; + distance_profile_output_.new_acceleration = {0.0}; + distance_profile_output_.pass_to_input(distance_profile_input_); + + // Angle Motion Profile + angle_profile_input_.max_velocity = {rotate_to_heading_angular_vel_}; + angle_profile_input_.max_acceleration = {max_angular_accel_}; + angle_profile_input_.max_jerk = {max_angular_jerk_}; + angle_profile_input_.target_position = {0.0}; + angle_profile_input_.target_velocity = {0.0}; + // Reset current state + angle_profile_input_.control_interface = ruckig::ControlInterface::Velocity; + angle_profile_output_.new_position = {0.0}; + angle_profile_output_.new_velocity = {0.0}; + angle_profile_output_.new_acceleration = {0.0}; + angle_profile_output_.pass_to_input(angle_profile_input_); } void RegulatedPurePursuitController::deactivate() @@ -274,6 +321,36 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity goal_dist_tol_ = pose_tolerance.position.x; } + // Note(angstrem98): Controller needs to reset internal states in case goal has been calcelled. + // Current controller API does not have a callback if this happens, so we reset if controller has + // been inactive long enough. + rclcpp::Time t = clock_->now(); + // If controller was interrupted, reset internal states + if ((t - system_time_).seconds() >= 4 * control_duration_) { + distance_profile_output_.new_position = {0.0}; + distance_profile_output_.new_velocity = {0.0}; + distance_profile_output_.new_acceleration = {0.0}; + distance_profile_output_.pass_to_input(distance_profile_input_); + + distance_profile_input_.control_interface = ruckig::ControlInterface::Position; + + angle_profile_output_.new_position = {tf2::getYaw(pose.pose.orientation)}; + angle_profile_output_.new_velocity = {0.0}; + angle_profile_output_.new_acceleration = {0.0}; + angle_profile_output_.pass_to_input(angle_profile_input_); + + angle_profile_input_.control_interface = ruckig::ControlInterface::Velocity; + } + system_time_ = t; + + // Set acceleration and jerk limits + distance_profile_input_.max_acceleration = {max_linear_accel_}; + distance_profile_input_.min_acceleration = {-max_linear_decel_}; + distance_profile_input_.max_jerk = {max_linear_jerk_}; + + angle_profile_input_.max_acceleration = {max_angular_accel_}; + angle_profile_input_.max_jerk = {max_angular_jerk_}; + // Transform path to robot base frame auto transformed_plan = transformGlobalPlan(pose); @@ -291,7 +368,8 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity } } - auto carrot_pose = getLookAheadPoint(lookahead_dist, transformed_plan); + double remaining_path_length; + auto carrot_pose = getLookAheadPoint(lookahead_dist, transformed_plan, remaining_path_length); carrot_pub_->publish(createCarrotMsg(carrot_pose)); double linear_vel, angular_vel; @@ -308,6 +386,11 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity curvature = 2.0 * carrot_pose.pose.position.y / carrot_dist2; } + double carrot_dist = sqrt(carrot_dist2); + if (remaining_path_length < carrot_dist) { + remaining_path_length = carrot_dist; + } + // Setting the velocity direction double sign = 1.0; if (allow_reversing_) { @@ -316,34 +399,78 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity linear_vel = desired_linear_vel_; - // Make sure we're in compliance with basic constraints - double angle_to_heading; - if (shouldRotateToGoalHeading(carrot_pose)) { - double angle_to_goal = tf2::getYaw(transformed_plan.poses.back().pose.orientation); - rotateToHeading(linear_vel, angular_vel, angle_to_goal, speed); - } else if (shouldRotateToPath(carrot_pose, angle_to_heading)) { - rotateToHeading(linear_vel, angular_vel, angle_to_heading, speed); + distance_profile_input_.max_velocity = {desired_linear_vel_}; + + robot_angle_ = tf2::getYaw(pose.pose.orientation); + + if (angle_profile_input_.control_interface != ruckig::ControlInterface::Position) { + // Make sure we're in compliance with basic constraints + double angle_to_heading; + if (shouldRotateToGoalHeading(carrot_pose, lookahead_dist)) { + double angle_to_goal = tf2::getYaw(transformed_plan.poses.back().pose.orientation); + rotateToHeading(linear_vel, angular_vel, angle_to_goal, speed); + } else if (shouldRotateToPath(carrot_pose, angle_to_heading)) { + rotateToHeading(linear_vel, angular_vel, angle_to_heading, speed); + } else { + applyConstraints( + fabs(lookahead_dist - sqrt(carrot_dist2)), + lookahead_dist, curvature, speed, + costAtPose(pose.pose.position.x, pose.pose.position.y), linear_vel, sign); + + // Apply curvature to angular velocity after constraining linear velocity + angular_vel = linear_vel * curvature; + + distance_profile_input_.control_interface = ruckig::ControlInterface::Position; + distance_profile_input_.target_position = { + distance_profile_input_.current_position[0] + sign * remaining_path_length}; + distance_profile_input_.max_velocity = {fabs(linear_vel)}; + + angle_profile_input_.control_interface = ruckig::ControlInterface::Velocity; + angle_profile_input_.target_velocity = {angular_vel}; + } } else { - applyConstraints( - fabs(lookahead_dist - sqrt(carrot_dist2)), - lookahead_dist, curvature, speed, - costAtPose(pose.pose.position.x, pose.pose.position.y), linear_vel, sign); + // Angle profile is in Position mode - we are rotating in place + if (angle_profile_result_ == ruckig::Result::Finished) { + // If the rotation is finished we switch back to velocity mode + angle_profile_input_.control_interface = ruckig::ControlInterface::Velocity; + } + } - // Apply curvature to angular velocity after constraining linear velocity - angular_vel = linear_vel * curvature; + // Calculate next step for motion profiles + distance_profile_result_ = distance_profile_->update( + distance_profile_input_, distance_profile_output_); + angle_profile_result_ = angle_profile_->update(angle_profile_input_, angle_profile_output_); + + double linear_vel_command = 0.0; + double angular_vel_command = 0.0; + + if (distance_profile_result_ == ruckig::Result::Working) { + distance_profile_output_.pass_to_input(distance_profile_input_); + } + + if (angle_profile_result_ == ruckig::Result::Working) { + angle_profile_output_.pass_to_input(angle_profile_input_); + } + + linear_vel_command = distance_profile_output_.new_velocity[0]; + if (angle_profile_input_.control_interface == ruckig::ControlInterface::Velocity) { + angular_vel_command = angle_profile_output_.new_velocity[0]; + } else { + // proportional controller for robot angle when we are in position mode + angular_vel_command = kp_angle_ * angles::normalize_angle( + angle_profile_output_.new_position[0] - robot_angle_); } // Collision checking on this velocity heading - const double & carrot_dist = hypot(carrot_pose.pose.position.x, carrot_pose.pose.position.y); - if (isCollisionImminent(pose, linear_vel, angular_vel, carrot_dist)) { + if (isCollisionImminent(pose, linear_vel_command, angular_vel_command, carrot_dist)) { throw nav2_core::PlannerException("RegulatedPurePursuitController detected collision ahead!"); } // populate and return message geometry_msgs::msg::TwistStamped cmd_vel; cmd_vel.header = pose.header; - cmd_vel.twist.linear.x = linear_vel; - cmd_vel.twist.angular.z = angular_vel; + cmd_vel.twist.linear.x = linear_vel_command; + cmd_vel.twist.angular.z = angular_vel_command; return cmd_vel; } @@ -356,26 +483,29 @@ bool RegulatedPurePursuitController::shouldRotateToPath( } bool RegulatedPurePursuitController::shouldRotateToGoalHeading( - const geometry_msgs::msg::PoseStamped & carrot_pose) + const geometry_msgs::msg::PoseStamped & carrot_pose, const double & lookahead_dist) { // Whether we should rotate robot to goal heading - double dist_to_goal = std::hypot(carrot_pose.pose.position.x, carrot_pose.pose.position.y); - return use_rotate_to_heading_ && dist_to_goal < goal_dist_tol_; + double carrot_dist = std::hypot(carrot_pose.pose.position.x, carrot_pose.pose.position.y); + return use_rotate_to_heading_ && carrot_dist < goal_dist_tol_ && + fabs(lookahead_dist - carrot_dist) > 2.0 * costmap_->getResolution(); } void RegulatedPurePursuitController::rotateToHeading( - double & linear_vel, double & angular_vel, - const double & angle_to_path, const geometry_msgs::msg::Twist & curr_speed) + double & /*linear_vel*/, double & /*angular_vel*/, + const double & angle_to_path, const geometry_msgs::msg::Twist & /*curr_speed*/) { // Rotate in place using max angular velocity / acceleration possible - linear_vel = 0.0; - const double sign = angle_to_path > 0.0 ? 1.0 : -1.0; - angular_vel = sign * rotate_to_heading_angular_vel_; - - const double & dt = control_duration_; - const double min_feasible_angular_speed = curr_speed.angular.z - max_angular_accel_ * dt; - const double max_feasible_angular_speed = curr_speed.angular.z + max_angular_accel_ * dt; - angular_vel = std::clamp(angular_vel, min_feasible_angular_speed, max_feasible_angular_speed); + angle_profile_input_.control_interface = ruckig::ControlInterface::Position; + angle_profile_input_.max_velocity = {rotate_to_heading_angular_vel_}; + angle_profile_input_.current_position = {robot_angle_}; + angle_profile_input_.target_position = { + robot_angle_ + angle_to_path}; + angle_profile_input_.target_velocity = {0.0}; + + // Switch distance profile to velocity mode and keep it at zero - slowdown to a stop + distance_profile_input_.control_interface = ruckig::ControlInterface::Velocity; + distance_profile_input_.target_velocity = {0.0}; } geometry_msgs::msg::Point RegulatedPurePursuitController::circleSegmentIntersection( @@ -415,7 +545,7 @@ geometry_msgs::msg::Point RegulatedPurePursuitController::circleSegmentIntersect geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoint( const double & lookahead_dist, - const nav_msgs::msg::Path & transformed_plan) + const nav_msgs::msg::Path & transformed_plan, double & remaining_path_length) { // Find the first pose which is at a distance greater than the lookahead distance auto goal_pose_it = std::find_if( @@ -443,6 +573,11 @@ geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoin return pose; } + size_t lookahead_index = std::distance(transformed_plan.poses.begin(), goal_pose_it); + remaining_path_length = nav2_util::geometry_utils::calculate_path_length( + transformed_plan, + lookahead_index); + return *goal_pose_it; } @@ -572,13 +707,12 @@ double RegulatedPurePursuitController::costAtPose(const double & x, const double } void RegulatedPurePursuitController::applyConstraints( - const double & dist_error, const double & lookahead_dist, + const double & /*dist_error*/, const double & /*lookahead_dist*/, const double & curvature, const geometry_msgs::msg::Twist & /*curr_speed*/, const double & pose_cost, double & linear_vel, double & sign) { double curvature_vel = linear_vel; double cost_vel = linear_vel; - double approach_vel = linear_vel; // limit the linear velocity by curvature const double radius = fabs(1.0 / curvature); @@ -605,24 +739,6 @@ void RegulatedPurePursuitController::applyConstraints( linear_vel = std::min(cost_vel, curvature_vel); linear_vel = std::max(linear_vel, regulated_linear_scaling_min_speed_); - // if the actual lookahead distance is shorter than requested, that means we're at the - // end of the path. We'll scale linear velocity by error to slow to a smooth stop. - // This expression is eq. to (1) holding time to goal, t, constant using the theoretical - // lookahead distance and proposed velocity and (2) using t with the actual lookahead - // distance to scale the velocity (e.g. t = lookahead / velocity, v = carrot / t). - if (dist_error > 2.0 * costmap_->getResolution()) { - double velocity_scaling = 1.0 - (dist_error / lookahead_dist); - double unbounded_vel = approach_vel * velocity_scaling; - if (unbounded_vel < min_approach_linear_velocity_) { - approach_vel = min_approach_linear_velocity_; - } else { - approach_vel *= velocity_scaling; - } - - // Use the lowest velocity between approach and other constraints, if all overlapping - linear_vel = std::min(linear_vel, approach_vel); - } - // Limit linear velocities to be valid linear_vel = std::clamp(fabs(linear_vel), 0.0, desired_linear_vel_); linear_vel = sign * linear_vel; @@ -631,6 +747,8 @@ void RegulatedPurePursuitController::applyConstraints( void RegulatedPurePursuitController::setPlan(const nav_msgs::msg::Path & path) { global_plan_ = path; + + angle_profile_input_.control_interface = ruckig::ControlInterface::Velocity; } void RegulatedPurePursuitController::setSpeedLimit( @@ -798,6 +916,16 @@ RegulatedPurePursuitController::dynamicParametersCallback( } else if (name == plugin_name_ + ".desired_linear_vel") { desired_linear_vel_ = parameter.as_double(); base_desired_linear_vel_ = parameter.as_double(); + } else if (name == plugin_name_ + ".max_linear_accel") { + max_linear_accel_ = parameter.as_double(); + } else if (name == plugin_name_ + ".max_linear_decel") { + max_linear_decel_ = parameter.as_double(); + } else if (name == plugin_name_ + ".max_linear_jerk") { + max_linear_jerk_ = parameter.as_double(); + } else if (name == plugin_name_ + ".max_angular_jerk") { + max_angular_jerk_ = parameter.as_double(); + } else if (name == plugin_name_ + ".kp_angle") { + kp_angle_ = parameter.as_double(); } else if (name == plugin_name_ + ".lookahead_dist") { lookahead_dist_ = parameter.as_double(); } else if (name == plugin_name_ + ".max_lookahead_dist") { @@ -808,8 +936,6 @@ RegulatedPurePursuitController::dynamicParametersCallback( lookahead_time_ = parameter.as_double(); } else if (name == plugin_name_ + ".rotate_to_heading_angular_vel") { rotate_to_heading_angular_vel_ = parameter.as_double(); - } else if (name == plugin_name_ + ".min_approach_linear_velocity") { - min_approach_linear_velocity_ = parameter.as_double(); } else if (name == plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot") { max_allowed_time_to_collision_up_to_carrot_ = parameter.as_double(); } else if (name == plugin_name_ + ".cost_scaling_dist") { 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 8b9b45821b..eab81520e6 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -71,7 +71,8 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure geometry_msgs::msg::PoseStamped getLookAheadPointWrapper( const double & dist, const nav_msgs::msg::Path & path) { - return getLookAheadPoint(dist, path); + double remaining_path_length; + return getLookAheadPoint(dist, path, remaining_path_length); } bool shouldRotateToPathWrapper( @@ -80,9 +81,10 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure return shouldRotateToPath(carrot_pose, angle_to_path); } - bool shouldRotateToGoalHeadingWrapper(const geometry_msgs::msg::PoseStamped & carrot_pose) + bool shouldRotateToGoalHeadingWrapper( + const geometry_msgs::msg::PoseStamped & carrot_pose, const double & lookahead_dist) { - return shouldRotateToGoalHeading(carrot_pose); + return shouldRotateToGoalHeading(carrot_pose, lookahead_dist); } void rotateToHeadingWrapper( @@ -419,15 +421,16 @@ TEST(RegulatedPurePursuitTest, rotateTests) // shouldRotateToGoalHeading carrot.pose.position.x = 0.0; carrot.pose.position.y = 0.0; - EXPECT_EQ(ctrl->shouldRotateToGoalHeadingWrapper(carrot), true); + double lookahead = 0.6; + EXPECT_EQ(ctrl->shouldRotateToGoalHeadingWrapper(carrot, lookahead), true); carrot.pose.position.x = 0.0; carrot.pose.position.y = 0.24; - EXPECT_EQ(ctrl->shouldRotateToGoalHeadingWrapper(carrot), true); + EXPECT_EQ(ctrl->shouldRotateToGoalHeadingWrapper(carrot, lookahead), true); carrot.pose.position.x = 0.0; carrot.pose.position.y = 0.26; - EXPECT_EQ(ctrl->shouldRotateToGoalHeadingWrapper(carrot), false); + EXPECT_EQ(ctrl->shouldRotateToGoalHeadingWrapper(carrot, lookahead), false); // rotateToHeading double lin_v = 10.0;