Skip to content

Commit

Permalink
Add optional cancel callback to asynchronous cancel goal methods
Browse files Browse the repository at this point in the history 
Signed-off-by: Jacob Perron <jacob@openrobotics.org>
  • Loading branch information
@jacobperron
jacobperron committed Apr 24, 2019
1 parent 9a26a24 commit 65160ed
Show file tree
Hide file tree
Showing 2 changed files with 151 additions and 8 deletions.
37 changes: 29 additions & 8 deletions rclcpp_action/include/rclcpp_action/client.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -267,6 +267,8 @@ class Client : public ClientBase
using ResultCallback = typename GoalHandle::ResultCallback;
using CancelRequest = typename ActionT::Impl::CancelGoalService::Request;
using CancelResponse = typename ActionT::Impl::CancelGoalService::Response;
using CancelOneCallback = std::function<void (typename GoalHandle::SharedPtr, bool)>;
using CancelMultipleCallback = std::function<void (typename CancelResponse::SharedPtr)>;

/// Options for sending a goal.
/**
Expand Down Expand Up @@ -419,10 +421,15 @@ class Client : public ClientBase
* \throws exceptions::UnknownGoalHandleError If the goal is unknown or already reached a
* terminal state.
* \param[in] goal_handle The goal handle requesting to be canceled.
* \param[in] cancel_callback Optional callback that is called when the response is received.
* The callback function takes two parameters: a shared pointer to the goal handle and a bool
* indicating if the action server accepted the cancel request or not.
* \return A future whose result indicates whether or not the cancel request was accepted.
*/
std::shared_future<bool>
async_cancel_goal(typename GoalHandle::SharedPtr goal_handle)
async_cancel_goal(
typename GoalHandle::SharedPtr goal_handle,
CancelOneCallback cancel_callback = nullptr)
{
std::lock_guard<std::mutex> lock(goal_handles_mutex_);
if (goal_handles_.count(goal_handle->get_goal_id()) == 0) {
Expand All @@ -436,7 +443,7 @@ class Client : public ClientBase
cancel_request->goal_info.goal_id.uuid = goal_handle->get_goal_id();
this->send_cancel_request(
std::static_pointer_cast<void>(cancel_request),
[goal_handle, promise](std::shared_ptr<void> response) mutable
[goal_handle, cancel_callback, promise](std::shared_ptr<void> response) mutable
{
auto cancel_response = std::static_pointer_cast<CancelResponse>(response);
bool goal_canceled = false;
Expand All @@ -446,48 +453,57 @@ class Client : public ClientBase
goal_canceled = (canceled_goal_info.goal_id.uuid == goal_handle->get_goal_id());
}
promise->set_value(goal_canceled);
if (cancel_callback) {
cancel_callback(goal_handle, goal_canceled);
}
});
return future;
}

/// Asynchronously request for all goals to be canceled.
/**
* \param[in] cancel_callback Optional callback that is called when the response is received.
* The callback takes one parameter: a shared pointer to the CancelResponse message.
* \return A future to a CancelResponse message that is set when the request has been
* acknowledged by an action server.
* See
* <a href="https://github.com/ros2/rcl_interfaces/blob/master/action_msgs/srv/CancelGoal.srv">
* action_msgs/CancelGoal.srv</a>.
*/
std::shared_future<typename CancelResponse::SharedPtr>
async_cancel_all_goals()
async_cancel_all_goals(CancelMultipleCallback cancel_callback = nullptr)
{
auto cancel_request = std::make_shared<CancelRequest>();
// std::fill(cancel_request->goal_info.goal_id.uuid, 0u);
std::fill(
cancel_request->goal_info.goal_id.uuid.begin(),
cancel_request->goal_info.goal_id.uuid.end(), 0u);
return async_cancel(cancel_request);
return async_cancel(cancel_request, cancel_callback);
}

/// Asynchronously request all goals at or before a specified time be canceled.
/**
* \param[in] stamp The timestamp for the cancel goal request.
* \param[in] cancel_callback Optional callback that is called when the response is received.
* The callback takes one parameter: a shared pointer to the CancelResponse message.
* \return A future to a CancelResponse message that is set when the request has been
* acknowledged by an action server.
* See
* <a href="https://github.com/ros2/rcl_interfaces/blob/master/action_msgs/srv/CancelGoal.srv">
* action_msgs/CancelGoal.srv</a>.
*/
std::shared_future<typename CancelResponse::SharedPtr>
async_cancel_goals_before(const rclcpp::Time & stamp)
async_cancel_goals_before(
const rclcpp::Time & stamp,
CancelMultipleCallback cancel_callback = nullptr)
{
auto cancel_request = std::make_shared<CancelRequest>();
// std::fill(cancel_request->goal_info.goal_id.uuid, 0u);
std::fill(
cancel_request->goal_info.goal_id.uuid.begin(),
cancel_request->goal_info.goal_id.uuid.end(), 0u);
cancel_request->goal_info.stamp = stamp;
return async_cancel(cancel_request);
return async_cancel(cancel_request, cancel_callback);
}

virtual
Expand Down Expand Up @@ -618,17 +634,22 @@ class Client : public ClientBase

/// \internal
std::shared_future<typename CancelResponse::SharedPtr>
async_cancel(typename CancelRequest::SharedPtr cancel_request)
async_cancel(
typename CancelRequest::SharedPtr cancel_request,
CancelMultipleCallback cancel_callback = nullptr)
{
// Put promise in the heap to move it around.
auto promise = std::make_shared<std::promise<typename CancelResponse::SharedPtr>>();
std::shared_future<typename CancelResponse::SharedPtr> future(promise->get_future());
this->send_cancel_request(
std::static_pointer_cast<void>(cancel_request),
[promise](std::shared_ptr<void> response) mutable
[cancel_callback, promise](std::shared_ptr<void> response) mutable
{
auto cancel_response = std::static_pointer_cast<CancelResponse>(response);
promise->set_value(cancel_response);
if (cancel_callback) {
cancel_callback(cancel_response);
}
});
return future;
}
Expand Down
122 changes: 122 additions & 0 deletions rclcpp_action/test/test_client.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -468,6 +468,37 @@ TEST_F(TestClient, async_cancel_one_goal)
EXPECT_TRUE(goal_canceled);
}

TEST_F(TestClient, async_cancel_one_goal_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 = 5;
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());

bool cancel_response_received = false;
auto future_cancel = action_client->async_cancel_goal(
goal_handle,
[&cancel_response_received, goal_handle](
typename ActionGoalHandle::SharedPtr goal_handle_canceled, bool cancel_accepted) mutable
{
if (
goal_handle_canceled->get_goal_id() == goal_handle->get_goal_id() &&
cancel_accepted)
{
cancel_response_received = true;
}
});
dual_spin_until_future_complete(future_cancel);
bool goal_canceled = future_cancel.get();
EXPECT_TRUE(goal_canceled);
EXPECT_TRUE(cancel_response_received);
}

TEST_F(TestClient, async_cancel_all_goals)
{
auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
Expand Down Expand Up @@ -503,6 +534,55 @@ TEST_F(TestClient, async_cancel_all_goals)
EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle1->get_status());
}

TEST_F(TestClient, async_cancel_all_goals_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 = 6;
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);
dual_spin_until_future_complete(future_goal_handle1);
auto goal_handle1 = future_goal_handle1.get();

if (goal_handle1->get_goal_id() < goal_handle0->get_goal_id()) {
goal_handle0.swap(goal_handle1);
}

ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(3)));

bool cancel_callback_received = false;
auto future_cancel_all = action_client->async_cancel_all_goals(
[&cancel_callback_received, goal_handle0, goal_handle1](
ActionCancelGoalResponse::SharedPtr response)
{
if (
response &&
2ul == response->goals_canceling.size() &&
goal_handle0->get_goal_id() == response->goals_canceling[0].goal_id.uuid &&
goal_handle1->get_goal_id() == response->goals_canceling[1].goal_id.uuid)
{
cancel_callback_received = true;
}
});
dual_spin_until_future_complete(future_cancel_all);
auto cancel_response = future_cancel_all.get();

EXPECT_TRUE(cancel_callback_received);
ASSERT_EQ(2ul, cancel_response->goals_canceling.size());
EXPECT_EQ(goal_handle0->get_goal_id(), cancel_response->goals_canceling[0].goal_id.uuid);
EXPECT_EQ(goal_handle1->get_goal_id(), cancel_response->goals_canceling[1].goal_id.uuid);
EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle0->get_status());
EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle1->get_status());
}

TEST_F(TestClient, async_cancel_some_goals)
{
auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
Expand Down Expand Up @@ -532,3 +612,45 @@ TEST_F(TestClient, async_cancel_some_goals)
EXPECT_EQ(goal_handle0->get_goal_id(), cancel_response->goals_canceling[0].goal_id.uuid);
EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle0->get_status());
}

TEST_F(TestClient, async_cancel_some_goals_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 = 6;
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);
dual_spin_until_future_complete(future_goal_handle1);
auto goal_handle1 = future_goal_handle1.get();

ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(3)));

bool cancel_callback_received = false;
auto future_cancel_some = action_client->async_cancel_goals_before(
goal_handle1->get_goal_stamp(),
[&cancel_callback_received, goal_handle0](ActionCancelGoalResponse::SharedPtr response)
{
if (
response &&
1ul == response->goals_canceling.size() &&
goal_handle0->get_goal_id() == response->goals_canceling[0].goal_id.uuid)
{
cancel_callback_received = true;
}
});
dual_spin_until_future_complete(future_cancel_some);
auto cancel_response = future_cancel_some.get();

EXPECT_TRUE(cancel_callback_received);
ASSERT_EQ(1ul, cancel_response->goals_canceling.size());
EXPECT_EQ(goal_handle0->get_goal_id(), cancel_response->goals_canceling[0].goal_id.uuid);
EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle0->get_status());
}

0 comments on commit 65160ed

Please sign in to comment.