From 52327c890a4aaedc6bca99a1d7b271b8deaffeb9 Mon Sep 17 00:00:00 2001 From: Ramon Wijnands Date: Thu, 22 Feb 2024 15:11:17 +0100 Subject: [PATCH] Add API to gracefully cancel a controller Signed-off-by: Ramon Wijnands --- nav2_controller/src/controller_server.cpp | 16 ++++++++++++---- nav2_core/include/nav2_core/controller.hpp | 9 +++++++++ 2 files changed, 21 insertions(+), 4 deletions(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index fa25e0b1379..55bf587dffc 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -451,6 +451,7 @@ void ControllerServer::computeControl() last_valid_cmd_time_ = now(); rclcpp::WallRate loop_rate(controller_frequency_); + bool cancelling = false; while (rclcpp::ok()) { if (action_server_ == nullptr || !action_server_->is_server_active()) { RCLCPP_DEBUG(get_logger(), "Action server unavailable or inactive. Stopping."); @@ -458,10 +459,17 @@ 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 { + if (!cancelling) { + RCLCPP_INFO(get_logger(), "Goal was canceled. Trying to stop the robot."); + cancelling = true; + } + } } // 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..fdd0d25634e 100644 --- a/nav2_core/include/nav2_core/controller.hpp +++ b/nav2_core/include/nav2_core/controller.hpp @@ -116,6 +116,15 @@ 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)