Skip to content

Commit

Permalink
Add API to gracefully cancel a controller
Browse files Browse the repository at this point in the history
Signed-off-by: Ramon Wijnands <ramon.wijnands@nobleo.nl>
  • Loading branch information
Rayman committed Mar 25, 2024
1 parent 5bf14fc commit 6dad49c
Show file tree
Hide file tree
Showing 2 changed files with 21 additions and 4 deletions.
15 changes: 11 additions & 4 deletions nav2_controller/src/controller_server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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)
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

0 comments on commit 6dad49c

Please sign in to comment.