Fix status message dropped when received before goal response callback (Fixes #2782)

[Aaravanand00]

Description

This PR fixes Issue #2782 (Status is Never Set to Executing if ros2 action rclcpp_action) and adds the corresponding unit test case (status_message_before_goal_response) for Issue #2781.

Cause of Bug (#2782):

When async_send_goal is called, the GoalHandle is only added to goal_handles_ after the SendGoal service response callback completes on the client side. If an action server starts executing immediately and publishes a GoalStatusMessage (STATUS_EXECUTING) on the action status topic before the client-side service response callback runs, handle_status_message checked goal_handles_.count(goal_id) == 0, logged "Received status for unknown goal. Ignoring...", and dropped the status update!

Fix:

1. rclcpp_action::Client now maintains a pending_statuses_ map to store early status messages received prior to GoalHandle creation.
2. When the SendGoal service response callback creates the GoalHandle and registers it in goal_handles_, it checks pending_statuses_ and immediately applies any pending status update (e.g. STATUS_EXECUTING).
3. Added unit test case TEST_F(TestClientAgainstServer, status_message_before_goal_response) in rclcpp_action/test/test_client.cpp to verify this behavior.

Fixes #2782
Closes #2781

Is this user-facing behavior change?

Yes, rclcpp_action client now correctly updates goal status to STATUS_EXECUTING when status messages arrive before the goal response callback completes.

Did you use Generative AI?

Yes Ai Agent was used to construct the verify code formatting.

[Aaravanand00]

Hi @fujitatomoya Fixed Issue #2782 by storing early status messages in a pending_statuses_ map so they aren't dropped when arriving before the goal response callback and added a unit test in test_client.cpp. PTAL...

[jmachowinski]

This is more a workaround than a proper fix. The underlying issue is that the rcl layer does not report the events in the correct order.

[Aaravanand00]

This is more a workaround than a proper fix. The underlying issue is that the rcl layer does not report the events in the correct order.

@jmachowinski makes sense.
should we open a pr in ros2/rcl (in rcl_action) to ensure events/status updates are properly ordered at the C layer? If so I'm ready to raise the PR there and update this one accordingly...

[jmachowinski]

This PR would then get reduced to only the fix, the workaround must not be needed if it is properly fixed in rcl.

rcl_action_client_wait_set_get_entities_ready is the function that needs to be fixed.
The function must be changed to only give out the correct next ready event that should be processed.
And for that you will need to add entries to the underlying message so that you can acutally reconstruct the sent order.

[Aaravanand00]

This PR would then get reduced to only the fix, the workaround must not be needed if it is properly fixed in rcl.

rcl_action_client_wait_set_get_entities_ready is the function that needs to be fixed. The function must be changed to only give out the correct next ready event that should be processed. And for that you will need to add entries to the underlying message so that you can acutally reconstruct the sent order.

ros2/rcl#1337 ptal... I have removed the workaround from rclcpp and implemented the proper fix in the rcl layer as you suggested. The wait_set function now strictly prioritizes goal responses over status updates, ensuring the correct event order. If this looks good, we can close this PR in favor of the new one....