Added RPP use_cancel_deceleration parameter

ros-navigation · Jun 16, 2024 · 2c173be · 2c173be
1 parent 635880d
commit 2c173be
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 0 deletions.
diff --git a/...e_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp b/...e_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
@@ -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
@@ -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_;
 }