niklas/rclcpp
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Use options struct for passing callbacks to async_send_goal

Browse source 
Now supports callbacks for the goal response and result.
This also makes it easier to incorporate action clients in composable nodes since we don't have to rely on waiting on futures.

Signed-off-by: Jacob Perron <jacob@openrobotics.org>
This commit is contained in:
Jacob Perron 2019-04-17 22:23:55 -07:00
parent 6b10841477
commit 0da966b981
4 changed files with 151 additions and 30 deletions
Download patch file Download diff file Expand all files Collapse all files

105
rclcpp_action/test/test_client.cpp
View file

@ -268,20 +268,20 @@ TEST_F(TestClient, construction_and_destruction)
ASSERT_NO_THROW(rclcpp_action::create_client<ActionType>(client_node, action_name).reset());
}
TEST_F(TestClient, async_send_goal_but_ignore_feedback_and_result)
TEST_F(TestClient, async_send_goal_no_callbacks)
{
auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));
ActionGoal bad_goal;
bad_goal.order = -5;
auto future_goal_handle = action_client->async_send_goal(bad_goal, nullptr, true);
auto future_goal_handle = action_client->async_send_goal(bad_goal);
dual_spin_until_future_complete(future_goal_handle);
EXPECT_EQ(nullptr, future_goal_handle.get().get());
ActionGoal good_goal;
good_goal.order = 5;
future_goal_handle = action_client->async_send_goal(good_goal, nullptr, true);
future_goal_handle = action_client->async_send_goal(good_goal);
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());
@ -290,7 +290,7 @@ TEST_F(TestClient, async_send_goal_but_ignore_feedback_and_result)
EXPECT_THROW(goal_handle->async_result(), rclcpp_action::exceptions::UnawareGoalHandleError);
}
TEST_F(TestClient, async_send_goal_and_ignore_feedback_but_wait_for_result)
TEST_F(TestClient, async_send_goal_no_callbacks_wait_for_result)
{
auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));
@ -302,8 +302,9 @@ TEST_F(TestClient, async_send_goal_and_ignore_feedback_but_wait_for_result)
auto goal_handle = future_goal_handle.get();
EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status());
EXPECT_FALSE(goal_handle->is_feedback_aware());
EXPECT_FALSE(goal_handle->is_result_aware());
auto future_result = action_client->async_get_result(goal_handle);
EXPECT_TRUE(goal_handle->is_result_aware());
auto future_result = goal_handle->async_result();
dual_spin_until_future_complete(future_result);
auto wrapped_result = future_result.get();
ASSERT_EQ(6ul, wrapped_result.result->sequence.size());
@ -312,7 +313,41 @@ TEST_F(TestClient, async_send_goal_and_ignore_feedback_but_wait_for_result)
EXPECT_EQ(5, wrapped_result.result->sequence[5]);
}
TEST_F(TestClient, async_send_goal_with_feedback_and_result)
TEST_F(TestClient, async_send_goal_with_goal_response_callback_wait_for_result)
{
auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));
ActionGoal goal;
goal.order = 4;
bool goal_response_received = false;
auto send_goal_ops = rclcpp_action::Client<ActionType>::SendGoalOptions();
send_goal_ops.goal_response_callback =
[&goal_response_received]
(std::shared_future<typename ActionGoalHandle::SharedPtr> future) mutable
{
auto goal_handle = future.get();
if (goal_handle) {
goal_response_received = true;
}
};
auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops);
dual_spin_until_future_complete(future_goal_handle);
auto goal_handle = future_goal_handle.get();
EXPECT_TRUE(goal_response_received);
EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status());
EXPECT_FALSE(goal_handle->is_feedback_aware());
EXPECT_FALSE(goal_handle->is_result_aware());
auto future_result = action_client->async_get_result(goal_handle);
EXPECT_TRUE(goal_handle->is_result_aware());
dual_spin_until_future_complete(future_result);
auto wrapped_result = future_result.get();
ASSERT_EQ(5u, wrapped_result.result->sequence.size());
EXPECT_EQ(3, wrapped_result.result->sequence.back());
}
TEST_F(TestClient, async_send_goal_with_feedback_callback_wait_for_result)
{
auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));
@ -320,22 +355,24 @@ TEST_F(TestClient, async_send_goal_with_feedback_and_result)
ActionGoal goal;
goal.order = 4;
int feedback_count = 0;
auto future_goal_handle = action_client->async_send_goal(
goal,
auto send_goal_ops = rclcpp_action::Client<ActionType>::SendGoalOptions();
send_goal_ops.feedback_callback =
[&feedback_count](
typename ActionGoalHandle::SharedPtr goal_handle,
const std::shared_ptr<const ActionFeedback> feedback) mutable
typename ActionGoalHandle::SharedPtr goal_handle,
const std::shared_ptr<const ActionFeedback> feedback) mutable
{
(void)goal_handle;
(void)feedback;
feedback_count++;
});
};
auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops);
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_TRUE(goal_handle->is_feedback_aware());
EXPECT_FALSE(goal_handle->is_result_aware());
auto future_result = action_client->async_get_result(goal_handle);
EXPECT_TRUE(goal_handle->is_result_aware());
auto future_result = goal_handle->async_result();
dual_spin_until_future_complete(future_result);
auto wrapped_result = future_result.get();
@ -344,6 +381,40 @@ TEST_F(TestClient, async_send_goal_with_feedback_and_result)
EXPECT_EQ(5, feedback_count);
}
TEST_F(TestClient, async_send_goal_with_result_callback_wait_for_result)
{
auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));
ActionGoal goal;
goal.order = 4;
bool result_callback_received = false;
auto send_goal_ops = rclcpp_action::Client<ActionType>::SendGoalOptions();
send_goal_ops.result_callback =
[&result_callback_received](
const typename ActionGoalHandle::WrappedResult & result) mutable
{
if (rclcpp_action::ResultCode::SUCCEEDED == result.code &&
result.result->sequence.size() == 5u)
{
result_callback_received = true;
}
};
auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops);
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_FALSE(goal_handle->is_feedback_aware());
EXPECT_TRUE(goal_handle->is_result_aware());
auto future_result = action_client->async_get_result(goal_handle);
dual_spin_until_future_complete(future_result);
auto wrapped_result = future_result.get();
EXPECT_TRUE(result_callback_received);
ASSERT_EQ(5u, wrapped_result.result->sequence.size());
EXPECT_EQ(3, wrapped_result.result->sequence.back());
}
TEST_F(TestClient, async_cancel_one_goal)
{
auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
@ -351,7 +422,7 @@ TEST_F(TestClient, async_cancel_one_goal)
ActionGoal goal;
goal.order = 5;
auto future_goal_handle = action_client->async_send_goal(goal, nullptr, true);
auto future_goal_handle = action_client->async_send_goal(goal);
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());
@ -369,14 +440,14 @@ TEST_F(TestClient, async_cancel_all_goals)
ActionGoal goal;
goal.order = 6;
auto future_goal_handle0 = action_client->async_send_goal(goal, nullptr, true);
auto future_goal_handle0 = action_client->async_send_goal(goal);
dual_spin_until_future_complete(future_goal_handle0);
auto goal_handle0 = future_goal_handle0.get();
ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(2)));
goal.order = 8;
auto future_goal_handle1 = action_client->async_send_goal(goal, nullptr, true);
auto future_goal_handle1 = action_client->async_send_goal(goal);
dual_spin_until_future_complete(future_goal_handle1);
auto goal_handle1 = future_goal_handle1.get();
@ -404,14 +475,14 @@ TEST_F(TestClient, async_cancel_some_goals)
ActionGoal goal;
goal.order = 6;
auto future_goal_handle0 = action_client->async_send_goal(goal, nullptr, true);
auto future_goal_handle0 = action_client->async_send_goal(goal);
dual_spin_until_future_complete(future_goal_handle0);
auto goal_handle0 = future_goal_handle0.get();
ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(2)));
goal.order = 8;
auto future_goal_handle1 = action_client->async_send_goal(goal, nullptr, true);
auto future_goal_handle1 = action_client->async_send_goal(goal);
dual_spin_until_future_complete(future_goal_handle1);
auto goal_handle1 = future_goal_handle1.get();