Cannot cancel ROS2 action goal immediately after sending
I have some toy code to test ROS2 action cancelation. I'm trying to send an action goal, then cancel it immediately. With print statements, I can verify that the action goal is received, but the cancellation callback does not run. The program hangs when the cancellation is requested.
Send the action goal:
auto goal_handle = std::get<1>(client_tuple)->async_send_goal(stage_request.hybrid_request, send_goal_options);
^don't worry about the tuple here. It's unconventional but it works.
Now try to cancel it immediately afterwards:
// I've tried a sleep() here. No change
auto cancellation_future = std::get<1>(client_tuple)->async_cancel_all_goals();
I have also tried this method of cancellation:
auto cancellation_future = std::get<1>(client_tuple)->async_cancel_goal(goal_handle.get());
if (rclcpp::spin_until_future_complete(m_node, cancellation_future) !=
rclcpp::FutureReturnCode::SUCCESS)
{
// This does not print:
RCLCPP_INFO(LOGGER, "FAILED TO CANCEL!");
}
// This does not print, it is stuck above this line:
RCLCPP_INFO(LOGGER, "CANCELED");
My action server is set up like so:
hybrid_planning_request_server_ = rclcpp_action::create_server<moveit_msgs::action::HybridPlanner>(
this->get_node_base_interface(), this->get_node_clock_interface(), this->get_node_logging_interface(),
this->get_node_waitables_interface(), hybrid_planning_action_name,
// Goal callback
[](const rclcpp_action::GoalUUID& /*unused*/,
std::shared_ptr<const moveit_msgs::action::HybridPlanner::Goal> /*unused*/) {
// This does print
RCLCPP_INFO(LOGGER, "Received goal request");
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
},
// Cancel callback
[this](std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_msgs::action::HybridPlanner>> /*unused*/) {
// This does not print:
RCLCPP_INFO(LOGGER, "Received request to cancel global goal manager");
return rclcpp_action::CancelResponse::ACCEPT;
},
I wonder if I need to add another spinning thread? Or, somebody else tipped me off that the callbacks may be in the same group? I don't know what that means/how to fix it.
I've tried a MultiThreadedExecutor now. Still no luck.
Can you post the full source code for your test files? Like you mentioned, the behavior of the executor and the callback groups are important here.
I dropped some code in to verify the goal is accepted. This never prints, so it looks like the problem is the action server not responding:
if (rclcpp_action::GoalStatus::STATUS_ACCEPTED != future_goal_handle.get()->get_status()) { RCLCPP_ERROR_STREAM(LOGGER, "Goal was not accepted"); return; } RCLCPP_ERROR_STREAM(LOGGER, "Goal was accepted!");@jschornak code for the action server which is not responding is here: https://github.com/ros-planning/movei...