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

Add optional result callback to async_get_result

Browse source 
Signed-off-by: Jacob Perron <jacob@openrobotics.org>
This commit is contained in:
Jacob Perron 2019-04-23 15:38:33 -07:00
parent 0da966b981
commit 1a0f8e3f28
4 changed files with 56 additions and 3 deletions
Download patch file Download diff file Expand all files Collapse all files

13
rclcpp_action/include/rclcpp_action/client.hpp
View file

@ -263,8 +263,8 @@ public:
using WrappedResult = typename GoalHandle::WrappedResult;
using GoalResponseCallback =
std::function<void (std::shared_future<typename GoalHandle::SharedPtr>)>;
using FeedbackCallback = typename ClientGoalHandle<ActionT>::FeedbackCallback;
using ResultCallback = typename ClientGoalHandle<ActionT>::ResultCallback;
using FeedbackCallback = typename GoalHandle::FeedbackCallback;
using ResultCallback = typename GoalHandle::ResultCallback;
using CancelRequest = typename ActionT::Impl::CancelGoalService::Request;
using CancelResponse = typename ActionT::Impl::CancelGoalService::Response;
@ -391,15 +391,22 @@ public:
* \throws exceptions::UnknownGoalHandleError If the goal unknown or already reached a terminal
* state.
* \param[in] goal_handle The goal handle for which to get the result.
* \param[in] result_callback Optional callback that is called when the result is received.
* \return A future that is set to the goal result when the goal is finished.
*/
std::shared_future<WrappedResult>
async_get_result(typename GoalHandle::SharedPtr goal_handle)
async_get_result(
typename GoalHandle::SharedPtr goal_handle,
ResultCallback result_callback = nullptr)
{
std::lock_guard<std::mutex> lock(goal_handles_mutex_);
if (goal_handles_.count(goal_handle->get_goal_id()) == 0) {
throw exceptions::UnknownGoalHandleError();
}
if (result_callback) {
// This will override any previously registered callback
goal_handle->set_result_callback(result_callback);
}
// If the user chose to ignore the result before, then ask the server for the result now.
if (!goal_handle->is_result_aware()) {
this->make_result_aware(goal_handle);

3
rclcpp_action/include/rclcpp_action/client_goal_handle.hpp
View file

@ -126,6 +126,9 @@ private:
void
set_feedback_callback(FeedbackCallback callback);
void
set_result_callback(ResultCallback callback);
void
call_feedback_callback(
typename ClientGoalHandle<ActionT>::SharedPtr shared_this,

8
rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp
View file

@ -86,6 +86,14 @@ ClientGoalHandle<ActionT>::set_feedback_callback(FeedbackCallback callback)
feedback_callback_ = callback;
}
template<typename ActionT>
void
ClientGoalHandle<ActionT>::set_result_callback(ResultCallback callback)
{
std::lock_guard<std::mutex> guard(handle_mutex_);
result_callback_ = callback;
}
template<typename ActionT>
int8_t
ClientGoalHandle<ActionT>::get_status()

35
rclcpp_action/test/test_client.cpp
View file

@ -415,6 +415,41 @@ TEST_F(TestClient, async_send_goal_with_result_callback_wait_for_result)
EXPECT_EQ(3, wrapped_result.result->sequence.back());
}
TEST_F(TestClient, async_get_result_with_callback)
{
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;
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_NE(goal_handle, nullptr);
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());
bool result_callback_received = false;
auto future_result = action_client->async_get_result(
goal_handle,
[&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;
}
});
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);