Add API to gracefully cancel a controller

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

nav2_controller/src/controller_server.cpp

@@ -458,10 +458,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 {
+          RCLCPP_INFO_THROTTLE(
+            get_logger(),
+            *get_clock(), 1000,
+            "Goal was canceled. Requesting controller to stop robot gracefully.");
+        }
       }
 
       // 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)