From 4ca6b484d75cf7ceab9e14cf3c0754e127318792 Mon Sep 17 00:00:00 2001 From: angstrem98 Date: Sun, 13 Feb 2022 21:00:12 +0100 Subject: [PATCH 01/20] Include ruckig Signed-off-by: angstrem98 --- nav2_regulated_pure_pursuit_controller/CMakeLists.txt | 4 +++- .../regulated_pure_pursuit_controller.hpp | 1 + nav2_regulated_pure_pursuit_controller/package.xml | 1 + 3 files changed, 5 insertions(+), 1 deletion(-) diff --git a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt index d64353d94a..1845e1d83c 100644 --- a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt +++ b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(pluginlib REQUIRED) find_package(tf2 REQUIRED) +find_package(ruckig REQUIRED) nav2_package() set(CMAKE_CXX_STANDARD 17) @@ -28,13 +29,14 @@ set(dependencies nav2_util nav2_core tf2 + ruckig ) 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/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 b69bd0e63c..9e828ded98 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 { diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index 0420ba04d2..1812f2bfb7 100644 --- a/nav2_regulated_pure_pursuit_controller/package.xml +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -19,6 +19,7 @@ nav2_msgs pluginlib tf2 + ruckig ament_cmake_gtest ament_lint_common From f72e6dfacdc34f7a332612e23a817b6e15a2bd46 Mon Sep 17 00:00:00 2001 From: angstrem98 Date: Sun, 13 Feb 2022 21:02:32 +0100 Subject: [PATCH 02/20] Add member variables Signed-off-by: angstrem98 --- .../regulated_pure_pursuit_controller.hpp | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) 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 9e828ded98..406e6c3173 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 @@ -251,6 +251,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_; @@ -273,6 +275,12 @@ class RegulatedPurePursuitController : public nav2_core::Controller double rotate_to_heading_min_angle_; double goal_dist_tol_; bool allow_reversing_; + double robot_angle_; + bool rotating_; + double kp_angle_; + double max_linear_jerk_; + double max_angular_jerk_; + rclcpp::Time system_time_; nav_msgs::msg::Path global_plan_; std::shared_ptr> global_path_pub_; @@ -282,6 +290,16 @@ class RegulatedPurePursuitController : public nav2_core::Controller std::unique_ptr> collision_checker_; + ruckig::Ruckig<1> * distance_profile_; + ruckig::InputParameter<1> distance_profile_input_; + ruckig::OutputParameter<1> distance_profile_output_; + ruckig::Result distance_profile_result_; + + ruckig::Ruckig<1> * 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_; From 1014e15f32977cc3d0ea321928fd543a311fd60c Mon Sep 17 00:00:00 2001 From: angstrem98 Date: Sun, 13 Feb 2022 21:07:34 +0100 Subject: [PATCH 03/20] Declare parameters Signed-off-by: angstrem98 --- .../src/regulated_pure_pursuit_controller.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) 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 2c3e7c63fb..298a7735cb 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,6 +62,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(0.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_angular_jerk", rclcpp::ParameterValue(120.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( @@ -105,9 +115,15 @@ 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)); + 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_); From cf77875b7bc1f651f3b5ee7402732c2422ccf425 Mon Sep 17 00:00:00 2001 From: angstrem98 Date: Sun, 13 Feb 2022 21:09:39 +0100 Subject: [PATCH 04/20] Motion profiles init Signed-off-by: angstrem98 --- .../src/regulated_pure_pursuit_controller.cpp | 37 +++++++++++++++++++ 1 file changed, 37 insertions(+) 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 298a7735cb..0dca75e4ad 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 @@ -192,6 +192,11 @@ void RegulatedPurePursuitController::configure( collision_checker_ = std::make_unique>(costmap_); collision_checker_->setCostmap(costmap_); + + distance_profile_ = new ruckig::Ruckig<1>{control_duration_}; + angle_profile_ = new ruckig::Ruckig<1>{control_duration_}; + + system_time_ = clock_->now(); } void RegulatedPurePursuitController::cleanup() @@ -204,6 +209,9 @@ void RegulatedPurePursuitController::cleanup() global_path_pub_.reset(); carrot_pub_.reset(); carrot_arc_pub_.reset(); + + delete distance_profile_; + delete angle_profile_; } void RegulatedPurePursuitController::activate() @@ -222,6 +230,35 @@ 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_); + + rotating_ = false; } void RegulatedPurePursuitController::deactivate() From 0e954d33529d916c706fd01a5295bbfa7297959c Mon Sep 17 00:00:00 2001 From: angstrem98 Date: Sun, 13 Feb 2022 21:13:17 +0100 Subject: [PATCH 05/20] Remaining path length calculation Signed-off-by: angstrem98 --- .../regulated_pure_pursuit_controller.hpp | 4 +++- .../src/regulated_pure_pursuit_controller.cpp | 7 ++++++- .../test/test_regulated_pp.cpp | 3 ++- 3 files changed, 11 insertions(+), 3 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 406e6c3173..10ed15a609 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 @@ -224,9 +224,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 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 0dca75e4ad..7a0f7dedd8 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 @@ -420,7 +420,7 @@ void RegulatedPurePursuitController::rotateToHeading( 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( @@ -433,6 +433,11 @@ geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoin goal_pose_it = std::prev(transformed_plan.poses.end()); } + 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; } 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 9139097890..681a2da3b9 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -61,7 +61,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( From c31209a8011f719c3ecbaeb30347fcb2739fdb7e Mon Sep 17 00:00:00 2001 From: angstrem98 Date: Sun, 13 Feb 2022 21:18:21 +0100 Subject: [PATCH 06/20] rotateToHeading using motion profile Signed-off-by: angstrem98 --- .../src/regulated_pure_pursuit_controller.cpp | 24 +++++++++++-------- 1 file changed, 14 insertions(+), 10 deletions(-) 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 7a0f7dedd8..e3fc5b996a 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 @@ -404,18 +404,22 @@ bool RegulatedPurePursuitController::shouldRotateToGoalHeading( } 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 + distance_profile_input_.control_interface = ruckig::ControlInterface::Velocity; + distance_profile_input_.target_velocity = {0.0}; + + rotating_ = true; } geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoint( From 2f4190f74414bc37ddda614c4a1b7c508eed0f4e Mon Sep 17 00:00:00 2001 From: angstrem98 Date: Sun, 13 Feb 2022 21:29:42 +0100 Subject: [PATCH 07/20] Add RPP code with motion profile Signed-off-by: angstrem98 --- .../regulated_pure_pursuit_controller.hpp | 7 ++ .../src/regulated_pure_pursuit_controller.cpp | 94 +++++++++++++++++-- 2 files changed, 93 insertions(+), 8 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 10ed15a609..45af0bf73d 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 @@ -237,6 +237,13 @@ class RegulatedPurePursuitController : public nav2_core::Controller */ double findDirectionChange(const geometry_msgs::msg::PoseStamped & pose); + /** + * @brief Normalizes angle in range [-pi, pi] + * @param angle Angle to normalize + * @return Normalized angle + */ + double angleNormalize(double angle); + /** * @brief Callback executed when a parameter change is detected * @param event ParameterEvent message 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 e3fc5b996a..a20ac1c46a 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 @@ -314,6 +314,25 @@ 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_); + + 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_); + } + system_time_ = t; + + // Transform path to robot base frame auto transformed_plan = transformGlobalPlan(pose); @@ -331,7 +350,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; @@ -348,6 +368,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_) { @@ -356,13 +381,23 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity linear_vel = desired_linear_vel_; + distance_profile_input_.max_velocity = {desired_linear_vel_}; + + robot_angle_ = tf2::getYaw(pose.pose.orientation); + // Make sure we're in compliance with basic constraints double angle_to_heading; - if (shouldRotateToGoalHeading(carrot_pose)) { + if (shouldRotateToGoalHeading(carrot_pose) && !rotating_) { 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 if (shouldRotateToPath(carrot_pose, angle_to_heading) && !rotating_) { + // multiply heading min angle by 0.1 so we rotate just enough + rotateToHeading( + linear_vel, angular_vel, angle_to_heading - rotate_to_heading_min_angle_ * 0.1, speed); + } else if (rotating_) { + if (angle_profile_result_ == ruckig::Result::Finished) { + rotating_ = false; + } } else { applyConstraints( fabs(lookahead_dist - sqrt(carrot_dist2)), @@ -371,19 +406,51 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity // 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}; + } + + // 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_ * angleNormalize( + 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; } @@ -762,6 +829,17 @@ bool RegulatedPurePursuitController::transformPose( return false; } +double RegulatedPurePursuitController::angleNormalize(double angle) +{ + while (angle > M_PI) { + angle -= 2.0 * M_PI; + } + while (angle < -M_PI) { + angle += 2.0 * M_PI; + } + + return angle; +} rcl_interfaces::msg::SetParametersResult RegulatedPurePursuitController::dynamicParametersCallback( From 39b3ef07032350ce960ae289fe452f756b9d3043 Mon Sep 17 00:00:00 2001 From: angstrem98 Date: Sun, 13 Feb 2022 21:31:30 +0100 Subject: [PATCH 08/20] Remove old velocity scaling near goal Signed-off-by: angstrem98 --- .../src/regulated_pure_pursuit_controller.cpp | 21 +------------------ 1 file changed, 1 insertion(+), 20 deletions(-) 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 a20ac1c46a..daae4c497f 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 @@ -638,13 +638,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); @@ -671,24 +670,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; From 645d9755814afe611d536d19631b4a23b03f4f51 Mon Sep 17 00:00:00 2001 From: angstrem98 Date: Sun, 13 Feb 2022 23:26:27 +0100 Subject: [PATCH 09/20] Improve shouldRotateToGoalHeading Signed-off-by: angstrem98 --- .../regulated_pure_pursuit_controller.hpp | 4 +++- .../src/regulated_pure_pursuit_controller.cpp | 9 +++++---- .../test/test_regulated_pp.cpp | 14 +++++++------- 3 files changed, 15 insertions(+), 12 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 45af0bf73d..553772ca3d 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 @@ -158,9 +158,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 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 daae4c497f..d007e96b1b 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 @@ -387,7 +387,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity // Make sure we're in compliance with basic constraints double angle_to_heading; - if (shouldRotateToGoalHeading(carrot_pose) && !rotating_) { + if (shouldRotateToGoalHeading(carrot_pose, lookahead_dist) && !rotating_) { 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) && !rotating_) { @@ -463,11 +463,12 @@ 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) > goal_dist_tol_; } void RegulatedPurePursuitController::rotateToHeading( 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 681a2da3b9..52f7607cf5 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -71,10 +71,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) - { - return shouldRotateToGoalHeading(carrot_pose); - } + // bool shouldRotateToGoalHeadingWrapper(const geometry_msgs::msg::PoseStamped & carrot_pose) + // { + // return shouldRotateToGoalHeading(carrot_pose); + // } void rotateToHeadingWrapper( double & linear_vel, double & angular_vel, @@ -264,15 +264,15 @@ TEST(RegulatedPurePursuitTest, rotateTests) // shouldRotateToGoalHeading carrot.pose.position.x = 0.0; carrot.pose.position.y = 0.0; - EXPECT_EQ(ctrl->shouldRotateToGoalHeadingWrapper(carrot), true); + // EXPECT_EQ(ctrl->shouldRotateToGoalHeadingWrapper(carrot), true); carrot.pose.position.x = 0.0; carrot.pose.position.y = 0.24; - EXPECT_EQ(ctrl->shouldRotateToGoalHeadingWrapper(carrot), true); + // EXPECT_EQ(ctrl->shouldRotateToGoalHeadingWrapper(carrot), true); carrot.pose.position.x = 0.0; carrot.pose.position.y = 0.26; - EXPECT_EQ(ctrl->shouldRotateToGoalHeadingWrapper(carrot), false); + // EXPECT_EQ(ctrl->shouldRotateToGoalHeadingWrapper(carrot), false); // rotateToHeading double lin_v = 10.0; From e3a875bac6e5d8d53c5eec4c8d1e0686f55efc69 Mon Sep 17 00:00:00 2001 From: angstrem98 Date: Mon, 14 Feb 2022 00:03:50 +0100 Subject: [PATCH 10/20] Fix linter, code style Signed-off-by: angstrem98 --- .../src/regulated_pure_pursuit_controller.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 d007e96b1b..03bdfcc0b3 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 @@ -115,7 +115,7 @@ 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)); - + node->get_parameter(plugin_name_ + ".desired_linear_vel", desired_linear_vel_); base_desired_linear_vel_ = desired_linear_vel_; @@ -230,7 +230,7 @@ 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_}; @@ -468,7 +468,7 @@ bool RegulatedPurePursuitController::shouldRotateToGoalHeading( // Whether we should rotate robot to goal heading 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) > goal_dist_tol_; + fabs(lookahead_dist - carrot_dist) > goal_dist_tol_; } void RegulatedPurePursuitController::rotateToHeading( From f46184ab7925676be33addaf9dbd8de8b12818bd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Du=C5=A1an=20Jovanovi=C4=87?= Date: Tue, 15 Feb 2022 14:58:51 +0100 Subject: [PATCH 11/20] Fix end of path detection --- .../src/regulated_pure_pursuit_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 03bdfcc0b3..67779b86c2 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 @@ -468,7 +468,7 @@ bool RegulatedPurePursuitController::shouldRotateToGoalHeading( // Whether we should rotate robot to goal heading 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) > goal_dist_tol_; + fabs(lookahead_dist - carrot_dist) > 2.0 * costmap_->getResolution(); } void RegulatedPurePursuitController::rotateToHeading( From 7f1e4e5584bed038cb660ac10166cc9a27b65bf7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Du=C5=A1an=20Jovanovi=C4=87?= Date: Wed, 9 Mar 2022 23:20:17 +0100 Subject: [PATCH 12/20] rotate to path remove unnecessary angle modification --- .../src/regulated_pure_pursuit_controller.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) 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 67779b86c2..8fc6592d83 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 @@ -391,9 +391,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity 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) && !rotating_) { - // multiply heading min angle by 0.1 so we rotate just enough - rotateToHeading( - linear_vel, angular_vel, angle_to_heading - rotate_to_heading_min_angle_ * 0.1, speed); + rotateToHeading(linear_vel, angular_vel, angle_to_heading, speed); } else if (rotating_) { if (angle_profile_result_ == ruckig::Result::Finished) { rotating_ = false; From 4eb8c3920d6e3f772e59f20b67556d4c22a0690d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Du=C5=A1an=20Jovanovi=C4=87?= Date: Sun, 13 Mar 2022 02:16:19 +0100 Subject: [PATCH 13/20] fix rotating_ not being reset to false --- .../src/regulated_pure_pursuit_controller.cpp | 5 +++++ 1 file changed, 5 insertions(+) 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 8fc6592d83..e9c45b3f91 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 @@ -329,6 +329,8 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity angle_profile_output_.new_velocity = {0.0}; angle_profile_output_.new_acceleration = {0.0}; angle_profile_output_.pass_to_input(angle_profile_input_); + + rotating_ = false; } system_time_ = t; @@ -677,6 +679,9 @@ void RegulatedPurePursuitController::applyConstraints( void RegulatedPurePursuitController::setPlan(const nav_msgs::msg::Path & path) { global_plan_ = path; + + angle_profile_input_.control_interface = ruckig::ControlInterface::Velocity; + rotating_ = false; } void RegulatedPurePursuitController::setSpeedLimit( From 7c7d265a1fb660d5e93d9beae66c0df813a2525c Mon Sep 17 00:00:00 2001 From: angstrem98 Date: Sun, 3 Apr 2022 03:00:43 +0200 Subject: [PATCH 14/20] normalize angle using ros/angles Signed-off-by: angstrem98 --- .../CMakeLists.txt | 2 ++ .../regulated_pure_pursuit_controller.hpp | 7 ------- nav2_regulated_pure_pursuit_controller/package.xml | 1 + .../src/regulated_pure_pursuit_controller.cpp | 14 ++------------ 4 files changed, 5 insertions(+), 19 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt index 1845e1d83c..7c2be316ee 100644 --- a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt +++ b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt @@ -12,6 +12,7 @@ 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) @@ -30,6 +31,7 @@ set(dependencies nav2_core tf2 ruckig + angles ) set(library_name nav2_regulated_pure_pursuit_controller) 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 e33ac15218..4103f6b865 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 @@ -262,13 +262,6 @@ class RegulatedPurePursuitController : public nav2_core::Controller */ double getCostmapMaxExtent() const; - /** - * @brief Normalizes angle in range [-pi, pi] - * @param angle Angle to normalize - * @return Normalized angle - */ - double angleNormalize(double angle); - /** * @brief Callback executed when a parameter change is detected * @param event ParameterEvent message diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index 1812f2bfb7..8cea88ea4a 100644 --- a/nav2_regulated_pure_pursuit_controller/package.xml +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -20,6 +20,7 @@ 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 761cf167ca..a6c8cc5bc5 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; @@ -450,7 +451,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity 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_ * angleNormalize( + angular_vel_command = kp_angle_ * angles::normalize_angle( angle_profile_output_.new_position[0] - robot_angle_); } @@ -881,17 +882,6 @@ bool RegulatedPurePursuitController::transformPose( return false; } -double RegulatedPurePursuitController::angleNormalize(double angle) -{ - while (angle > M_PI) { - angle -= 2.0 * M_PI; - } - while (angle < -M_PI) { - angle += 2.0 * M_PI; - } - - return angle; -} double RegulatedPurePursuitController::getCostmapMaxExtent() const { const double max_costmap_dim_meters = std::max( From c615a724988aa205baba226ce8c444843d8555e3 Mon Sep 17 00:00:00 2001 From: angstrem98 Date: Sun, 3 Apr 2022 03:07:55 +0200 Subject: [PATCH 15/20] switch raw memory management to unique_ptr Signed-off-by: angstrem98 --- .../regulated_pure_pursuit_controller.hpp | 4 ++-- .../src/regulated_pure_pursuit_controller.cpp | 8 ++++---- 2 files changed, 6 insertions(+), 6 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 4103f6b865..db33ddc0a6 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 @@ -319,12 +319,12 @@ class RegulatedPurePursuitController : public nav2_core::Controller std::unique_ptr> collision_checker_; - ruckig::Ruckig<1> * distance_profile_; + std::unique_ptr> distance_profile_; ruckig::InputParameter<1> distance_profile_input_; ruckig::OutputParameter<1> distance_profile_output_; ruckig::Result distance_profile_result_; - ruckig::Ruckig<1> * angle_profile_; + std::unique_ptr> angle_profile_; ruckig::InputParameter<1> angle_profile_input_; ruckig::OutputParameter<1> angle_profile_output_; ruckig::Result angle_profile_result_; 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 a6c8cc5bc5..fd276db54e 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 @@ -206,8 +206,8 @@ void RegulatedPurePursuitController::configure( FootprintCollisionChecker>(costmap_); collision_checker_->setCostmap(costmap_); - distance_profile_ = new ruckig::Ruckig<1>{control_duration_}; - angle_profile_ = new ruckig::Ruckig<1>{control_duration_}; + distance_profile_ = std::make_unique>(control_duration_); + angle_profile_ = std::make_unique>(control_duration_); system_time_ = clock_->now(); } @@ -223,8 +223,8 @@ void RegulatedPurePursuitController::cleanup() carrot_pub_.reset(); carrot_arc_pub_.reset(); - delete distance_profile_; - delete angle_profile_; + distance_profile_.reset(); + angle_profile_.reset(); } void RegulatedPurePursuitController::activate() From 2e698e2437ab5d717057bcfdd5dd8dea0747288c Mon Sep 17 00:00:00 2001 From: angstrem98 Date: Sun, 3 Apr 2022 03:33:57 +0200 Subject: [PATCH 16/20] remove rotating_ variable Signed-off-by: angstrem98 --- .../regulated_pure_pursuit_controller.hpp | 1 - .../src/regulated_pure_pursuit_controller.cpp | 63 ++++++++++--------- 2 files changed, 32 insertions(+), 32 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 db33ddc0a6..b28093aec3 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 @@ -303,7 +303,6 @@ class RegulatedPurePursuitController : public nav2_core::Controller double goal_dist_tol_; bool allow_reversing_; double robot_angle_; - bool rotating_; double kp_angle_; double max_linear_jerk_; double max_angular_jerk_; 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 fd276db54e..05d92580b8 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 @@ -270,8 +270,6 @@ void RegulatedPurePursuitController::activate() angle_profile_output_.new_velocity = {0.0}; angle_profile_output_.new_acceleration = {0.0}; angle_profile_output_.pass_to_input(angle_profile_input_); - - rotating_ = false; } void RegulatedPurePursuitController::deactivate() @@ -339,12 +337,14 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity 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_); - rotating_ = false; + angle_profile_input_.control_interface = ruckig::ControlInterface::Velocity; } system_time_ = t; @@ -401,33 +401,37 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity robot_angle_ = tf2::getYaw(pose.pose.orientation); - // Make sure we're in compliance with basic constraints - double angle_to_heading; - if (shouldRotateToGoalHeading(carrot_pose, lookahead_dist) && !rotating_) { - 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) && !rotating_) { - rotateToHeading(linear_vel, angular_vel, angle_to_heading, speed); - } else if (rotating_) { - if (angle_profile_result_ == ruckig::Result::Finished) { - rotating_ = false; - } - } else { - applyConstraints( - fabs(lookahead_dist - sqrt(carrot_dist2)), - lookahead_dist, curvature, speed, - costAtPose(pose.pose.position.x, pose.pose.position.y), linear_vel, sign); + 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; + // 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)}; + 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}; + angle_profile_input_.control_interface = ruckig::ControlInterface::Velocity; + angle_profile_input_.target_velocity = {angular_vel}; + } + } else { + // 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; + } } // Calculate next step for motion profiles @@ -497,11 +501,9 @@ void RegulatedPurePursuitController::rotateToHeading( robot_angle_ + angle_to_path}; angle_profile_input_.target_velocity = {0.0}; - // Switch distance profile to velocity mode and keep it at zero + // 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}; - - rotating_ = true; } geometry_msgs::msg::Point RegulatedPurePursuitController::circleSegmentIntersection( @@ -745,7 +747,6 @@ void RegulatedPurePursuitController::setPlan(const nav_msgs::msg::Path & path) global_plan_ = path; angle_profile_input_.control_interface = ruckig::ControlInterface::Velocity; - rotating_ = false; } void RegulatedPurePursuitController::setSpeedLimit( From e1ddd74d707bee94ca54b615ffcb93f29e628eb5 Mon Sep 17 00:00:00 2001 From: angstrem98 Date: Sun, 3 Apr 2022 04:13:05 +0200 Subject: [PATCH 17/20] remove min_approach_linear_velocity since it is not used anymore Signed-off-by: angstrem98 --- .../regulated_pure_pursuit_controller.hpp | 1 - .../src/regulated_pure_pursuit_controller.cpp | 7 ------- 2 files changed, 8 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 b28093aec3..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 @@ -287,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_; 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 05d92580b8..5584dc3c19 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 @@ -88,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)); @@ -142,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_); @@ -924,8 +919,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") { From 10d0d4fda2497205ada1db485e1df7000c3e6f37 Mon Sep 17 00:00:00 2001 From: angstrem98 Date: Sun, 3 Apr 2022 04:28:41 +0200 Subject: [PATCH 18/20] dynamic params Signed-off-by: angstrem98 --- .../src/regulated_pure_pursuit_controller.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) 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 5584dc3c19..c00a2657d3 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 @@ -343,6 +343,13 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity } 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); @@ -909,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") { From 2f461e07bdf30429ec4dce585213efbe10ad6b70 Mon Sep 17 00:00:00 2001 From: angstrem98 Date: Sun, 3 Apr 2022 04:41:11 +0200 Subject: [PATCH 19/20] readd tests Signed-off-by: angstrem98 --- .../test/test_regulated_pp.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) 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 869f623911..eab81520e6 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -81,10 +81,11 @@ 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) - // { - // return shouldRotateToGoalHeading(carrot_pose); - // } + bool shouldRotateToGoalHeadingWrapper( + const geometry_msgs::msg::PoseStamped & carrot_pose, const double & lookahead_dist) + { + return shouldRotateToGoalHeading(carrot_pose, lookahead_dist); + } void rotateToHeadingWrapper( double & linear_vel, double & angular_vel, @@ -420,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; From 03cf93ce88499222ed5e599f1a5dad8e8572c50d Mon Sep 17 00:00:00 2001 From: angstrem98 Date: Sun, 3 Apr 2022 05:01:22 +0200 Subject: [PATCH 20/20] update docs, force trapezoidal velocity profile on default Signed-off-by: angstrem98 --- .../README.md | 20 +++++++++++-------- .../src/regulated_pure_pursuit_controller.cpp | 4 ++-- 2 files changed, 14 insertions(+), 10 deletions(-) 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/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index c00a2657d3..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 @@ -68,9 +68,9 @@ void RegulatedPurePursuitController::configure( 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(0.5)); + node, plugin_name_ + ".max_linear_jerk", rclcpp::ParameterValue(10000.0)); declare_parameter_if_not_declared( - node, plugin_name_ + ".max_angular_jerk", rclcpp::ParameterValue(120.0)); + 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(