Skip to content

Commit

Permalink
Added RPP use_cancel_deceleration parameter (ros-navigation#4441)
Browse files Browse the repository at this point in the history
* Added RPP use_cancel_deceleration parameter

Signed-off-by: Jatin Patil <[email protected]>

* Fixed Linting code style

Signed-off-by: Jatin Patil <[email protected]>

---------

Signed-off-by: Jatin Patil <[email protected]>
  • Loading branch information
JatinPatil2003 authored and Manos-G committed Aug 1, 2024
1 parent 168600d commit f3248a9
Show file tree
Hide file tree
Showing 3 changed files with 10 additions and 0 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand Down Expand Up @@ -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(
Expand Down Expand Up @@ -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(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -290,6 +290,10 @@ 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_;
}
Expand Down

0 comments on commit f3248a9

Please sign in to comment.