diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 46e811aabd..24bc790d12 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP_ACTION__CLIENT_HPP_ #define RCLCPP_ACTION__CLIENT_HPP_ +#include #include #include #include @@ -29,12 +30,15 @@ #include "rcl/event_callback.h" #include "rclcpp/exceptions.hpp" +#include "rclcpp/clock.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/node_interfaces/node_logging_interface.hpp" #include "rclcpp/node_interfaces/node_graph_interface.hpp" #include "rclcpp/logger.hpp" +#include "rclcpp/qos.hpp" #include "rclcpp/time.hpp" +#include "rclcpp/waitable.hpp" #include "rosidl_runtime_c/action_type_support_struct.h" #include "rosidl_typesupport_cpp/action_type_support.hpp" @@ -194,6 +198,10 @@ class Client : public ClientBase using GoalResponse = typename ActionT::Impl::SendGoalService::Response; auto goal_response = std::static_pointer_cast(response); if (!goal_response->accepted) { + { + std::lock_guard guard(goal_handles_mutex_); + pending_statuses_.erase(goal_request->goal_id.uuid); + } promise->set_value(nullptr); if (options.goal_response_callback) { options.goal_response_callback(nullptr); @@ -222,6 +230,11 @@ class Client : public ClientBase new GoalHandle(goal_info, options.feedback_callback, options.result_callback)); { std::lock_guard guard(goal_handles_mutex_); + auto pending_it = pending_statuses_.find(goal_info.goal_id.uuid); + if (pending_it != pending_statuses_.end()) { + goal_handle->set_status(pending_it->second); + pending_statuses_.erase(pending_it); + } goal_handles_[goal_handle->get_goal_id()] = goal_handle; } promise->set_value(goal_handle); @@ -510,9 +523,11 @@ class Client : public ClientBase 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) { + pending_statuses_[goal_id] = status.status; RCLCPP_DEBUG( this->get_logger(), - "Received status for unknown goal. Ignoring..."); + "Received status for pending goal prior to response callback. Stored status %d.", + static_cast(status.status)); continue; } typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id].lock(); @@ -522,6 +537,7 @@ class Client : public ClientBase this->get_logger(), "Dropping weak reference to goal handle during status callback"); goal_handles_.erase(goal_id); + pending_statuses_.erase(goal_id); continue; } goal_handle->set_status(status.status); @@ -622,6 +638,7 @@ class Client : public ClientBase } std::map goal_handles_; + std::map pending_statuses_; std::recursive_mutex goal_handles_mutex_; }; } // namespace rclcpp_action diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index d11866251a..3adbb498f9 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -12,17 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include -#include #include #include #include #include -#include #include -#include #include +#include #include "gtest/gtest.h" @@ -35,17 +32,14 @@ #include "rcl_action/action_client.h" #include "rcl_action/wait.h" -#include "rclcpp/callback_group.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/executors.hpp" -#include "rclcpp/executors/single_threaded_executor.hpp" #include "rclcpp/node.hpp" #include "rclcpp/publisher.hpp" -#include "rclcpp/qos.hpp" +#include "rclcpp/rclcpp.hpp" #include "rclcpp/service.hpp" #include "rclcpp/time.hpp" -#include "rclcpp/utilities.hpp" #include "test_msgs/action/fibonacci.hpp" #include "test_msgs/msg/empty.hpp" @@ -707,6 +701,60 @@ TEST_F(TestClientAgainstServer, async_send_goal_with_goal_response_callback_wait } } +TEST_F(TestClientAgainstServer, status_message_before_goal_response) +{ + auto action_client = rclcpp_action::create_client(client_node, action_name); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + enable_pending_handling_goal(); + + bool goal_response_received = false; + auto send_goal_ops = rclcpp_action::Client::SendGoalOptions(); + send_goal_ops.goal_response_callback = + [&goal_response_received](typename ActionGoalHandle::SharedPtr goal_handle) + { + if (goal_handle) { + goal_response_received = true; + } + }; + + ActionGoal goal; + goal.order = 4; + auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops); + + // Spin server to process the goal request service call + server_executor.spin_some(); + + // Publish status message with STATUS_EXECUTING before completing the service response handling on the client side + ASSERT_EQ(1u, goals.size()); + auto goal_request = goals.begin()->second.first; + auto goal_response = goals.begin()->second.second; + + ActionStatusMessage status_message; + rclcpp_action::GoalStatus goal_status; + goal_status.goal_info.goal_id.uuid = goal_request->goal_id.uuid; + goal_status.goal_info.stamp = goal_response->stamp; + goal_status.status = rclcpp_action::GoalStatus::STATUS_EXECUTING; + status_message.status_list.push_back(goal_status); + status_publisher->publish(status_message); + + server_executor.spin_some(); + + // Allow server goal handling to proceed + disable_pending_handling_goal(); + + // Dual spin until goal handle future completes + dual_spin_until_future_complete(future_goal_handle); + + auto goal_handle = future_goal_handle.get(); + EXPECT_TRUE(goal_response_received); + ASSERT_NE(nullptr, goal_handle); + EXPECT_TRUE( + goal_handle->get_status() == rclcpp_action::GoalStatus::STATUS_ACCEPTED || + goal_handle->get_status() == rclcpp_action::GoalStatus::STATUS_EXECUTING); +} + + TEST_F(TestClientAgainstServer, async_send_goal_with_feedback_callback_wait_for_result) { auto action_client = rclcpp_action::create_client(client_node, action_name);