Skip to content

Commit

Permalink
Add API to gracefully cancel a controller (ros-navigation#4136)
Browse files Browse the repository at this point in the history
* Add API to gracefully cancel a controller

Signed-off-by: Ramon Wijnands <[email protected]>

* Add `cancel_deceleration` to RegulatedPurePursuitController

Signed-off-by: Ramon Wijnands <[email protected]>

* Update nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp

Co-authored-by: Steve Macenski <[email protected]>
Signed-off-by: Ramon Wijnands <[email protected]>

---------

Signed-off-by: Ramon Wijnands <[email protected]>
Signed-off-by: Ramon Wijnands <[email protected]>
Co-authored-by: Steve Macenski <[email protected]>
  • Loading branch information
2 people authored and Marc-Morcos committed Jul 4, 2024
1 parent 70b042e commit 84c0680
Show file tree
Hide file tree
Showing 6 changed files with 60 additions and 4 deletions.
13 changes: 9 additions & 4 deletions nav2_controller/src/controller_server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -460,10 +460,15 @@ void ControllerServer::computeControl()
}

if (action_server_->is_cancel_requested()) {
RCLCPP_INFO(get_logger(), "Goal was canceled. Stopping the robot.");
action_server_->terminate_all();
publishZeroVelocity();
return;
if (controllers_[current_controller_]->cancel()) {
RCLCPP_INFO(get_logger(), "Cancellation was successful. Stopping the robot.");
action_server_->terminate_all();
publishZeroVelocity();
return;
} else {
RCLCPP_INFO_THROTTLE(
get_logger(), *get_clock(), 1000, "Waiting for the controller to finish cancellation");
}
}

// Don't compute a trajectory until costmap is valid (after clear costmap)
Expand Down
10 changes: 10 additions & 0 deletions nav2_core/include/nav2_core/controller.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -116,6 +116,16 @@ class Controller
const geometry_msgs::msg::Twist & velocity,
nav2_core::GoalChecker * goal_checker) = 0;

/**
* @brief Cancel the current control action
* @return True if the cancellation was successful. If false is returned, computeVelocityCommands
* will be called until cancel returns true.
*/
virtual bool cancel()
{
return true;
}

/**
* @brief Limits the maximum linear speed of the robot.
* @param speed_limit expressed in absolute value (in m/s)
Expand Down
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;
double cancel_deceleration;
double rotate_to_heading_min_angle;
bool allow_reversing;
double max_robot_pose_search_dist;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -96,6 +96,8 @@ class RegulatedPurePursuitController : public nav2_core::Controller
const geometry_msgs::msg::Twist & velocity,
nav2_core::GoalChecker * /*goal_checker*/) override;

bool cancel() override;

/**
* @brief nav2_core setPlan - Sets the global plan
* @param path The global plan
Expand All @@ -111,6 +113,8 @@ class RegulatedPurePursuitController : public nav2_core::Controller
*/
void setSpeedLimit(const double & speed_limit, const bool & percentage) override;

void reset() override;

protected:
/**
* @brief Get lookahead distance
Expand Down Expand Up @@ -212,6 +216,8 @@ class RegulatedPurePursuitController : public nav2_core::Controller
Parameters * params_;
double goal_dist_tol_;
double control_duration_;
bool cancelling_ = false;
bool finished_cancelling_ = false;

std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> global_path_pub_;
std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PointStamped>>
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_ + ".cancel_deceleration", rclcpp::ParameterValue(3.2));
declare_parameter_if_not_declared(
node, plugin_name_ + ".allow_reversing", rclcpp::ParameterValue(false));
declare_parameter_if_not_declared(
Expand Down Expand Up @@ -151,6 +153,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_ + ".cancel_deceleration", params_.cancel_deceleration);
node->get_parameter(plugin_name_ + ".allow_reversing", params_.allow_reversing);
node->get_parameter(
plugin_name_ + ".max_robot_pose_search_dist",
Expand Down Expand Up @@ -237,6 +240,8 @@ ParameterHandler::dynamicParametersCallback(
params_.regulated_linear_scaling_min_speed = parameter.as_double();
} else if (name == plugin_name_ + ".max_angular_accel") {
params_.max_angular_accel = parameter.as_double();
} else if (name == plugin_name_ + ".cancel_deceleration") {
params_.cancel_deceleration = parameter.as_double();
} else if (name == plugin_name_ + ".rotate_to_heading_min_angle") {
params_.rotate_to_heading_min_angle = parameter.as_double();
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -238,6 +238,23 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
collision_checker_->costAtPose(pose.pose.position.x, pose.pose.position.y), transformed_plan,
linear_vel, x_vel_sign);

if (cancelling_) {
const double & dt = control_duration_;
linear_vel = speed.linear.x - x_vel_sign * dt * params_->cancel_deceleration;

if (x_vel_sign > 0) {
if (linear_vel <= 0) {
linear_vel = 0;
finished_cancelling_ = true;
}
} else {
if (linear_vel >= 0) {
linear_vel = 0;
finished_cancelling_ = true;
}
}
}

// Apply curvature to angular velocity after constraining linear velocity
angular_vel = linear_vel * regulation_curvature;
}
Expand All @@ -258,6 +275,12 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
return cmd_vel;
}

bool RegulatedPurePursuitController::cancel()
{
cancelling_ = true;
return finished_cancelling_;
}

bool RegulatedPurePursuitController::shouldRotateToPath(
const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path,
double & x_vel_sign)
Expand Down Expand Up @@ -445,6 +468,12 @@ void RegulatedPurePursuitController::setSpeedLimit(
}
}

void RegulatedPurePursuitController::reset()
{
cancelling_ = false;
finished_cancelling_ = false;
}

double RegulatedPurePursuitController::findVelocitySignChange(
const nav_msgs::msg::Path & transformed_plan)
{
Expand Down

0 comments on commit 84c0680

Please sign in to comment.