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

Add optional cancel callback to asynchronous cancel goal methods

Browse source 
Signed-off-by: Jacob Perron <jacob@openrobotics.org>
This commit is contained in:
Jacob Perron 2019-04-23 17:11:32 -07:00
parent 1a0f8e3f28
commit 02050c3901
2 changed files with 151 additions and 8 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -267,6 +267,8 @@ public:
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.
/**
@ -419,10 +421,15 @@ public:
* \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) {
@ -436,7 +443,7 @@ public:
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;
@ -446,12 +453,17 @@ public:
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
@ -459,19 +471,21 @@ public:
* 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
@ -479,7 +493,9 @@ public:
* 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);
@ -487,7 +503,7 @@ public:
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
@ -618,17 +634,22 @@ private:
/// \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;
}