Status is Never Set to Executing if ros2 action rclcpp_action

[tony-laza]

Operating System:

Linux tony-laptop 6.11.0-21-generic #21~24.04.1-Ubuntu SMP PREEMPT_DYNAMIC Mon Feb 24 16:52:15 UTC 2 x86_64 x86_64 x86_64 GNU/Linux

ROS version or commit hash:

rolling, jazzy

RMW implementation (if applicable):

No response

RMW Configuration (if applicable):

Can be reproduced in a colcon test gtest where all nodes are in the same process.

Client library (if applicable):

rclcpp

'ros2 doctor --report' output

Steps to reproduce issue

Run the tests in this PR to deterministically reproduce the issue.

Alternatively:

1. Create an action server and action client, ideally with a very short delay before calling execute() on the server goal handle. Threading (as in the examples) can lead to faster send of the status message, leading to a higher risk of status arriving before the send goal callback response.
2. Have the action client wait for Executing status (e.g. by looping in get_status `on the goal handle in a timer callback).
3. The action client will not receive the Executing status.

Note that debug message of the form [DEBUG] [1743106794.846124580] [rclcpp_action.test.client.fibonacci_action_test_client.rclcpp_action]: Received status for unknown goal. Ignoring... will be printed when the status is received before the callback.

Expected behavior

The tests in this PR should pass. The aforementioned debug log should not be printed, and the Executing status should have been received correctly.

Actual behavior

The tests fail, Executing status is never set. A debug message of the form [DEBUG] [1743106794.846124580] [rclcpp_action.test.client.fibonacci_action_test_client.rclcpp_action]: Received status for unknown goal. Ignoring... will be printed when the status is received before the callback.

Additional information

I believe that the following code can drop status messages in some cases, in rclcpp_action/include/rclcpp_action/client.hpp:

  async_send_goal(const Goal & goal, const SendGoalOptions & options = SendGoalOptions())
  {
    // Put promise in the heap to move it around.
    auto promise = std::make_shared<std::promise<typename GoalHandle::SharedPtr>>();
    std::shared_future<typename GoalHandle::SharedPtr> future(promise->get_future());
    using GoalRequest = typename ActionT::Impl::SendGoalService::Request;
    auto goal_request = std::make_shared<GoalRequest>();
    goal_request->goal_id.uuid = this->generate_goal_id();
    goal_request->goal = goal;
    this->send_goal_request(
      std::static_pointer_cast<void>(goal_request),
      [this, goal_request, options, promise](std::shared_ptr<void> response) mutable
      {
<snip some code for readability>
        std::shared_ptr<GoalHandle> goal_handle(
          new GoalHandle(goal_info, options.feedback_callback, options.result_callback));
        {
          std::lock_guard<std::recursive_mutex> guard(goal_handles_mutex_);
          goal_handles_[goal_handle->get_goal_id()] = goal_handle;
        }

This code adds the goal_handle to goal_handles_ only after success of the send_goal service. The code in the status callback ignores updates for goals not in goal_handles_:

  handle_status_message(std::shared_ptr<void> message) override
  {
    std::lock_guard<std::recursive_mutex> guard(goal_handles_mutex_);
    using GoalStatusMessage = typename ActionT::Impl::GoalStatusMessage;
    auto status_message = std::static_pointer_cast<GoalStatusMessage>(message);
    for (const GoalStatus & status : status_message->status_list) {
      const GoalUUID & goal_id = status.goal_info.goal_id.uuid;
      if (goal_handles_.count(goal_id) == 0) {
        RCLCPP_DEBUG(
          this->get_logger(),
          "Received status for unknown goal. Ignoring...");
        continue;
      }

In the case that the status update for a goal is published before the send_goal callback completes, the status update is dropped. I suspect that feedback messages arriving before callback completion are also dropped through a similar mechanism, but I have not tested it yet. I would be happy to help fix this issue and work on additional tests and test cases.