Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

rclcpp_action: take and execute service entities in priority. #2471

Closed
wants to merge 1 commit into from
Closed

rclcpp_action: take and execute service entities in priority. #2471

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
74 changes: 38 additions & 36 deletions rclcpp_action/src/client.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -550,21 +550,8 @@ ClientBase::clear_on_ready_callback()
std::shared_ptr<void>
ClientBase::take_data()
{
if (pimpl_->is_feedback_ready) {
std::shared_ptr<void> 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<void>(
std::make_shared<std::tuple<rcl_ret_t, std::shared_ptr<void>>>(
ret, feedback_message));
} else if (pimpl_->is_status_ready) {
std::shared_ptr<void> 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<void>(
std::make_shared<std::tuple<rcl_ret_t, std::shared_ptr<void>>>(
ret, status_message));
} else if (pimpl_->is_goal_response_ready) {
// Take the service entities 1st to apply to the client
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The change looks good, but I'm not sure I understand the meaning of this comment.
What's a "service entity"?

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

terms might be wrong. but it was supposed to mean it should take the action service executables 1st before action subscriptions.

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I would suggest a more verbose comment, for example:

/*
The order of these statements is important when using waitset-based executors because this method
will be called only once per executor iteration, regardless of how many events are ready.
We first check whether the state of the action goal has changed (i.e. whether we received a goal response,
a result response or a cancellation response).
This ensures that secondary events (e.g. feedback messages) won't prevent the goal state from advancing.
*/

if (pimpl_->is_goal_response_ready) {
rmw_request_id_t response_header;
std::shared_ptr<void> goal_response = this->create_goal_response();
rcl_ret_t ret = rcl_action_take_goal_response(
Expand All @@ -588,6 +575,20 @@ ClientBase::take_data()
return std::static_pointer_cast<void>(
std::make_shared<std::tuple<rcl_ret_t, rmw_request_id_t, std::shared_ptr<void>>>(
ret, response_header, cancel_response));
} else if (pimpl_->is_feedback_ready) {
std::shared_ptr<void> 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<void>(
std::make_shared<std::tuple<rcl_ret_t, std::shared_ptr<void>>>(
ret, feedback_message));
} else if (pimpl_->is_status_ready) {
std::shared_ptr<void> 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<void>(
std::make_shared<std::tuple<rcl_ret_t, std::shared_ptr<void>>>(
ret, status_message));
} else {
throw std::runtime_error("Taking data from action client but nothing is ready");
}
Expand Down Expand Up @@ -625,27 +626,8 @@ ClientBase::execute(std::shared_ptr<void> & data)
throw std::runtime_error("'data' is empty");
}

if (pimpl_->is_feedback_ready) {
auto shared_ptr = std::static_pointer_cast<std::tuple<rcl_ret_t, std::shared_ptr<void>>>(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<std::tuple<rcl_ret_t, std::shared_ptr<void>>>(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<rcl_ret_t, rmw_request_id_t, std::shared_ptr<void>>>(data);
auto ret = std::get<0>(*shared_ptr);
Expand Down Expand Up @@ -681,6 +663,26 @@ ClientBase::execute(std::shared_ptr<void> & 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<std::tuple<rcl_ret_t, std::shared_ptr<void>>>(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<std::tuple<rcl_ret_t, std::shared_ptr<void>>>(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");
}
Expand Down
13 changes: 7 additions & 6 deletions rclcpp_action/test/test_client.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<size_t>(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);
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand Down