From c9bdb8abe4781bc77093a80b0c043cca39309385 Mon Sep 17 00:00:00 2001 From: "Tomoya.Fujita" Date: Mon, 1 Apr 2024 00:23:57 -0700 Subject: [PATCH] rclcpp_action: take and execute service entities in priority. Signed-off-by: Tomoya.Fujita --- rclcpp_action/src/client.cpp | 74 +++++++++++++++--------------- rclcpp_action/test/test_client.cpp | 13 +++--- 2 files changed, 45 insertions(+), 42 deletions(-) diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index febd2fd905..1fe091df5a 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -550,21 +550,8 @@ ClientBase::clear_on_ready_callback() std::shared_ptr ClientBase::take_data() { - if (pimpl_->is_feedback_ready) { - std::shared_ptr feedback_message = this->create_feedback_message(); - rcl_ret_t ret = rcl_action_take_feedback( - pimpl_->client_handle.get(), feedback_message.get()); - return std::static_pointer_cast( - std::make_shared>>( - ret, feedback_message)); - } else if (pimpl_->is_status_ready) { - std::shared_ptr status_message = this->create_status_message(); - rcl_ret_t ret = rcl_action_take_status( - pimpl_->client_handle.get(), status_message.get()); - return std::static_pointer_cast( - std::make_shared>>( - ret, status_message)); - } else if (pimpl_->is_goal_response_ready) { + // Take the service entities 1st to apply to the client + if (pimpl_->is_goal_response_ready) { rmw_request_id_t response_header; std::shared_ptr goal_response = this->create_goal_response(); rcl_ret_t ret = rcl_action_take_goal_response( @@ -588,6 +575,20 @@ ClientBase::take_data() return std::static_pointer_cast( std::make_shared>>( ret, response_header, cancel_response)); + } else if (pimpl_->is_feedback_ready) { + std::shared_ptr feedback_message = this->create_feedback_message(); + rcl_ret_t ret = rcl_action_take_feedback( + pimpl_->client_handle.get(), feedback_message.get()); + return std::static_pointer_cast( + std::make_shared>>( + ret, feedback_message)); + } else if (pimpl_->is_status_ready) { + std::shared_ptr status_message = this->create_status_message(); + rcl_ret_t ret = rcl_action_take_status( + pimpl_->client_handle.get(), status_message.get()); + return std::static_pointer_cast( + std::make_shared>>( + ret, status_message)); } else { throw std::runtime_error("Taking data from action client but nothing is ready"); } @@ -625,27 +626,8 @@ ClientBase::execute(std::shared_ptr & data) throw std::runtime_error("'data' is empty"); } - if (pimpl_->is_feedback_ready) { - auto shared_ptr = std::static_pointer_cast>>(data); - auto ret = std::get<0>(*shared_ptr); - pimpl_->is_feedback_ready = false; - if (RCL_RET_OK == ret) { - auto feedback_message = std::get<1>(*shared_ptr); - this->handle_feedback_message(feedback_message); - } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret, "error taking feedback"); - } - } else if (pimpl_->is_status_ready) { - auto shared_ptr = std::static_pointer_cast>>(data); - auto ret = std::get<0>(*shared_ptr); - pimpl_->is_status_ready = false; - if (RCL_RET_OK == ret) { - auto status_message = std::get<1>(*shared_ptr); - this->handle_status_message(status_message); - } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret, "error taking status"); - } - } else if (pimpl_->is_goal_response_ready) { + // Take the service entities 1st to apply to the client + if (pimpl_->is_goal_response_ready) { auto shared_ptr = std::static_pointer_cast< std::tuple>>(data); auto ret = std::get<0>(*shared_ptr); @@ -681,6 +663,26 @@ ClientBase::execute(std::shared_ptr & data) } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) { rclcpp::exceptions::throw_from_rcl_error(ret, "error taking cancel response"); } + } else if (pimpl_->is_feedback_ready) { + auto shared_ptr = std::static_pointer_cast>>(data); + auto ret = std::get<0>(*shared_ptr); + pimpl_->is_feedback_ready = false; + if (RCL_RET_OK == ret) { + auto feedback_message = std::get<1>(*shared_ptr); + this->handle_feedback_message(feedback_message); + } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "error taking feedback"); + } + } else if (pimpl_->is_status_ready) { + auto shared_ptr = std::static_pointer_cast>>(data); + auto ret = std::get<0>(*shared_ptr); + pimpl_->is_status_ready = false; + if (RCL_RET_OK == ret) { + auto status_message = std::get<1>(*shared_ptr); + this->handle_status_message(status_message); + } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "error taking status"); + } } else { throw std::runtime_error("Executing action client but nothing is ready"); } diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index d5be45840b..0033389445 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -122,28 +122,28 @@ class TestClient : public ::testing::Test goal_status.status = rclcpp_action::GoalStatus::STATUS_EXECUTING; status_message.status_list.push_back(goal_status); status_publisher->publish(status_message); - client_executor.spin_once(); + client_executor.spin_some(); ActionFeedbackMessage feedback_message; feedback_message.goal_id.uuid = goal_request->goal_id.uuid; feedback_message.feedback.sequence.push_back(0); feedback_publisher->publish(feedback_message); - client_executor.spin_once(); + client_executor.spin_some(); if (goal_request->goal.order > 0) { feedback_message.feedback.sequence.push_back(1); feedback_publisher->publish(feedback_message); - client_executor.spin_once(); + client_executor.spin_some(); for (size_t i = 1; i < static_cast(goal_request->goal.order); ++i) { feedback_message.feedback.sequence.push_back( feedback_message.feedback.sequence[i] + feedback_message.feedback.sequence[i - 1]); feedback_publisher->publish(feedback_message); - client_executor.spin_once(); + client_executor.spin_some(); } } goal_status.status = rclcpp_action::GoalStatus::STATUS_SUCCEEDED; status_message.status_list[0] = goal_status; status_publisher->publish(status_message); - client_executor.spin_once(); + client_executor.spin_some(); response->result.sequence = feedback_message.feedback.sequence; response->status = rclcpp_action::GoalStatus::STATUS_SUCCEEDED; goals.erase(request->goal_id.uuid); @@ -192,7 +192,7 @@ class TestClient : public ::testing::Test } } status_publisher->publish(status_message); - client_executor.spin_once(); + client_executor.spin_some(); }); ASSERT_TRUE(cancel_service != nullptr); allocator.deallocate(cancel_service_name, allocator.state); @@ -556,6 +556,7 @@ TEST_F(TestClientAgainstServer, async_send_goal_with_feedback_callback_wait_for_ dual_spin_until_future_complete(future_goal_handle); auto goal_handle = future_goal_handle.get(); EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); + EXPECT_EQ(0, feedback_count); EXPECT_TRUE(goal_handle->is_feedback_aware()); EXPECT_FALSE(goal_handle->is_result_aware()); auto future_result = action_client->async_get_result(goal_handle);