Add API to gracefully cancel a controller

Signed-off-by: Ramon Wijnands <ramon.wijnands@nobleo.nl>

nav2_controller/src/controller_server.cpp

@@ -452,17 +452,25 @@ 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.");
         return;
       }
 
       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)

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)