0
$\begingroup$

Rosanswers logo

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;
      },

Originally posted by AndyZe on ROS Answers with karma: 2331 on 2022-05-18

Post score: 2


Original comments

Comment by AndyZe on 2022-05-18:
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.

Comment by AndyZe on 2022-05-18:
I've tried a MultiThreadedExecutor now. Still no luck.

Comment by jschornak on 2022-05-18:
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.

Comment by AndyZe on 2022-05-18:
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!");

Comment by AndyZe on 2022-05-18:
@jschornak code for the action server which is not responding is here: https://github.com/ros-planning/moveit2/blob/5a39a7f6ce84ce1ec17567ff5593ca9d376b21b6/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/src/hybrid_planning_manager.cpp#L147

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

There's a solution in this PR:

https://github.com/ros-planning/moveit2/pull/1272

A quick-and-dirty fix is to move long-running callbacks into a detached thread, like in this rclcpp demo: https://github.com/AndyZe/examples/blob/14c743af8bbca6a40e83da7a470c5332dce66d9d/rclcpp/actions/minimal_action_server/member_functions.cpp#L110

And also to make sure each action server (if you have several) has its own callback group.


Originally posted by AndyZe with karma: 2331 on 2022-05-26

This answer was ACCEPTED on the original site

Post score: 1

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.