From c22cbd6d9b087f9d186a81b7f7f490e043dd0a1c Mon Sep 17 00:00:00 2001 From: Ramon Wijnands Date: Fri, 29 Mar 2024 19:39:24 +0100 Subject: [PATCH] Add API to gracefully cancel a controller (#4136) * Add API to gracefully cancel a controller Signed-off-by: Ramon Wijnands * Add `cancel_deceleration` to RegulatedPurePursuitController Signed-off-by: Ramon Wijnands * Update nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp Co-authored-by: Steve Macenski Signed-off-by: Ramon Wijnands --------- Signed-off-by: Ramon Wijnands Signed-off-by: Ramon Wijnands Co-authored-by: Steve Macenski --- nav2_controller/src/controller_server.cpp | 13 ++++++--- nav2_core/include/nav2_core/controller.hpp | 10 +++++++ .../parameter_handler.hpp | 1 + .../regulated_pure_pursuit_controller.hpp | 7 +++-- .../src/parameter_handler.cpp | 5 ++++ .../src/regulated_pure_pursuit_controller.cpp | 29 +++++++++++++++++++ 6 files changed, 59 insertions(+), 6 deletions(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 791acaec94e..b62cbd9bd8c 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -465,10 +465,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) diff --git a/nav2_core/include/nav2_core/controller.hpp b/nav2_core/include/nav2_core/controller.hpp index dab79176e94..b01381a0414 100644 --- a/nav2_core/include/nav2_core/controller.hpp +++ b/nav2_core/include/nav2_core/controller.hpp @@ -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) 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 f24786299e1..6f2afd93f6c 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; + double cancel_deceleration; double rotate_to_heading_min_angle; bool allow_reversing; double max_robot_pose_search_dist; 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 c6c180a91ba..388b5da21c3 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 @@ -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 @@ -114,8 +116,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller /** * @brief Reset the state of the controller if necessary after task is exited */ - virtual void reset() override; - + void reset() override; protected: /** @@ -218,6 +219,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> global_path_pub_; std::shared_ptr> diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp index 82e3c5f097c..afc99df2ead 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_ + ".cancel_deceleration", rclcpp::ParameterValue(3.2)); declare_parameter_if_not_declared( node, plugin_name_ + ".allow_reversing", rclcpp::ParameterValue(false)); declare_parameter_if_not_declared( @@ -154,6 +156,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", @@ -243,6 +246,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(); } else if (name == plugin_name_ + ".desired_goal_distance") { 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 34122076a13..8e8963077ed 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 @@ -241,6 +241,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; } @@ -261,6 +278,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) @@ -453,6 +476,12 @@ void RegulatedPurePursuitController::setSpeedLimit( } } +void RegulatedPurePursuitController::reset() +{ + cancelling_ = false; + finished_cancelling_ = false; +} + double RegulatedPurePursuitController::findVelocitySignChange( const nav_msgs::msg::Path & transformed_plan) {