From 6a4a4d5e5d210b50031fd95d4c20cd7ba6390a5e Mon Sep 17 00:00:00 2001 From: Jatin Patil Date: Sun, 16 Jun 2024 12:51:19 +0530 Subject: [PATCH] Added RPP use_cancel_deceleration parameter Signed-off-by: Jatin Patil --- .../parameter_handler.hpp | 1 + .../src/parameter_handler.cpp | 5 +++++ .../src/regulated_pure_pursuit_controller.cpp | 3 +++ 3 files changed, 9 insertions(+) diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp index d2258ceb80..b033289622 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp @@ -53,6 +53,7 @@ struct Parameters double curvature_lookahead_dist; bool use_rotate_to_heading; double max_angular_accel; + bool use_cancel_deceleration; double cancel_deceleration; double rotate_to_heading_min_angle; bool allow_reversing; diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp index a47af13ef0..42876c248e 100644 --- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp @@ -85,6 +85,8 @@ ParameterHandler::ParameterHandler( node, plugin_name_ + ".rotate_to_heading_min_angle", rclcpp::ParameterValue(0.785)); declare_parameter_if_not_declared( node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_cancel_deceleration", rclcpp::ParameterValue(false)); declare_parameter_if_not_declared( node, plugin_name_ + ".cancel_deceleration", rclcpp::ParameterValue(3.2)); declare_parameter_if_not_declared( @@ -153,6 +155,7 @@ ParameterHandler::ParameterHandler( node->get_parameter( plugin_name_ + ".rotate_to_heading_min_angle", params_.rotate_to_heading_min_angle); node->get_parameter(plugin_name_ + ".max_angular_accel", params_.max_angular_accel); + node->get_parameter(plugin_name_ + ".use_cancel_deceleration", params_.use_cancel_deceleration); node->get_parameter(plugin_name_ + ".cancel_deceleration", params_.cancel_deceleration); node->get_parameter(plugin_name_ + ".allow_reversing", params_.allow_reversing); node->get_parameter( @@ -258,6 +261,8 @@ ParameterHandler::dynamicParametersCallback( params_.use_collision_detection = parameter.as_bool(); } else if (name == plugin_name_ + ".use_rotate_to_heading") { params_.use_rotate_to_heading = parameter.as_bool(); + } else if (name == plugin_name_ + ".use_cancel_deceleration") { + params_.use_cancel_deceleration = parameter.as_bool(); } else if (name == plugin_name_ + ".allow_reversing") { if (params_.use_rotate_to_heading && parameter.as_bool()) { RCLCPP_WARN( 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 423d0d1a04..18817da9d3 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 @@ -290,6 +290,9 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity bool RegulatedPurePursuitController::cancel() { + // if false then publish zero velocity + if (!params_->use_cancel_deceleration) + return true; cancelling_ = true; return finished_cancelling_; }