Skip to content

Add optional callbacks to action client for goal response and result #701

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

Merged
jacobperron merged 3 commits into from
Apr 26, 2019
Merged

Add optional callbacks to action client for goal response and result #701

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
1b11c6b
Use options struct for passing callbacks to async_send_goal
jacobperron Apr 18, 2019
3764f7b
Add optional result callback to async_get_result
jacobperron Apr 23, 2019
a98a964
Add optional cancel callback to asynchronous cancel goal methods
jacobperron Apr 24, 2019
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
105 changes: 86 additions & 19 deletions rclcpp_action/include/rclcpp_action/client.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -261,9 +261,45 @@ class Client : public ClientBase
using Feedback = typename ActionT::Feedback;
using GoalHandle = ClientGoalHandle<ActionT>;
using WrappedResult = typename GoalHandle::WrappedResult;
using FeedbackCallback = typename ClientGoalHandle<ActionT>::FeedbackCallback;
using GoalResponseCallback =
std::function<void (std::shared_future<typename GoalHandle::SharedPtr>)>;
using FeedbackCallback = typename GoalHandle::FeedbackCallback;
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.
/**
* This struct is used to pass parameters to the function `async_send_goal`.
*/
struct SendGoalOptions
{
SendGoalOptions()
: goal_response_callback(nullptr),
feedback_callback(nullptr),
result_callback(nullptr)
{
}

/// Function called when the goal is accepted or rejected.
/**
* Takes a single argument that is a future to a goal handle shared pointer.
* If the goal is accepted, then the pointer points to a valid goal handle.
* If the goal is rejected, then pointer has the value `nullptr`.
* If an error occurs while waiting for the goal response an exception will be thrown
* when calling `future::get()`.
* Possible exceptions include `rclcpp::RCLError` and `rclcpp::RCLBadAlloc`.
*/
GoalResponseCallback goal_response_callback;

/// Function called whenever feedback is received for the goal.
FeedbackCallback feedback_callback;

/// Function called when the result for the goal is received.
ResultCallback result_callback;
};

/// Construct an action client.
/**
Expand Down Expand Up @@ -299,15 +335,13 @@ class Client : public ClientBase
* The goal handle is used to monitor the status of the goal and get the final result.
*
* \param[in] goal The goal request.
* \param[in] callback Optional user callback for feedback associated with the goal.
* \param[in] ignore_result If `true`, then the result for the goal will not be requested and
* therefore inaccessible from the goal handle.
* \param[in] options Options for sending the goal request. Contains references to callbacks for
* the goal response (accepted/rejected), feedback, and the final result.
* \return A future that completes when the goal has been accepted or rejected.
* If the goal is rejected, then the result will be a `nullptr`.
*/
std::shared_future<typename GoalHandle::SharedPtr>
async_send_goal(
const Goal & goal, FeedbackCallback callback = nullptr, bool ignore_result = false)
async_send_goal(const Goal & goal, const SendGoalOptions & options = SendGoalOptions())
{
// Put promise in the heap to move it around.
auto promise = std::make_shared<std::promise<typename GoalHandle::SharedPtr>>();
Expand All @@ -318,31 +352,38 @@ class Client : public ClientBase
goal_request->goal = goal;
this->send_goal_request(
std::static_pointer_cast<void>(goal_request),
[this, goal_request, callback, ignore_result, promise](
std::shared_ptr<void> response) mutable
[this, goal_request, options, promise, future](std::shared_ptr<void> response) mutable
{
using GoalResponse = typename ActionT::Impl::SendGoalService::Response;
auto goal_response = std::static_pointer_cast<GoalResponse>(response);
if (!goal_response->accepted) {
promise->set_value(nullptr);
if (options.goal_response_callback) {
options.goal_response_callback(future);
}
return;
}
GoalInfo goal_info;
goal_info.goal_id.uuid = goal_request->goal_id.uuid;
goal_info.stamp = goal_response->stamp;
// Do not use std::make_shared as friendship cannot be forwarded.
std::shared_ptr<GoalHandle> goal_handle(new GoalHandle(goal_info, callback));
if (!ignore_result) {
std::shared_ptr<GoalHandle> goal_handle(
new GoalHandle(goal_info, options.feedback_callback, options.result_callback));
if (options.result_callback) {
Copy link
Copy Markdown
Contributor

@hidmic hidmic Apr 24, 2019

Choose a reason for hiding this comment

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

@jacobperron just for the record, this now means that if you just want the result future, you need to ask for it explicitly later on, always.

Copy link
Copy Markdown
Member Author

@jacobperron jacobperron Apr 24, 2019

Choose a reason for hiding this comment

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

Yeah, that's true. Though, this is a subtle feature. I bet that most use cases would provide a callback anyways and the case for sending a request early only to get a reference to the result future later is rare. The user can still get the future later with the client method, async_get_result().

This brings me to another point. I think it's odd to have two public facing methods for the getting the result, Client.async_get_result() and ClientGoalHandle.async_result(). Originally, the client method was essentially bypassing the "ignore result" flag. I propose we make the goal handle method private and rely on the client method to return the future. Regarding the "ignore result" flag, in the case where the goal handle is not result aware, we have 2.5 options:

  1. Keep the current behavior and make the goal handle result aware. I.e. Does not fail if the user previously set the "ignore result" flag.
    1. Or we can not even provide the "ignore result" option and make result requests on demand (this PR).
  2. Throw an exception, because the user chose to ignore the result explicitly.

Let me know what you think.

To not drag this PR on any longer, I suggest we make any changes related to this discussion in a follow-up PR.

try {
this->make_result_aware(goal_handle);
} catch (...) {
promise->set_exception(std::current_exception());
options.goal_response_callback(future);
Copy link
Copy Markdown

@mathias-luedtke mathias-luedtke May 2, 2019

Choose a reason for hiding this comment

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

this does not check goal_response_callback first

Copy link
Copy Markdown
Member Author

@jacobperron jacobperron May 24, 2019

Choose a reason for hiding this comment

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

Good catch. I've swapped the two if-blocks as suggested in #738.

return;
}
}
std::lock_guard<std::mutex> guard(goal_handles_mutex_);
goal_handles_[goal_handle->get_goal_id()] = goal_handle;
promise->set_value(goal_handle);
if (options.goal_response_callback) {
options.goal_response_callback(future);
Copy link
Copy Markdown

@mathias-luedtke mathias-luedtke May 2, 2019

Choose a reason for hiding this comment

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

This might get called after result_callback (not 100% sure)

}
});
return future;
}
Expand All @@ -352,15 +393,22 @@ class Client : public ClientBase
* \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);
Expand All @@ -373,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 @@ -390,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 @@ -400,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 @@ -572,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
11 changes: 10 additions & 1 deletion rclcpp_action/include/rclcpp_action/client_goal_handle.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -72,10 +72,12 @@ class ClientGoalHandle
} WrappedResult;

using Feedback = typename ActionT::Feedback;
using Result = typename ActionT::Result;
using FeedbackCallback =
std::function<void (
typename ClientGoalHandle<ActionT>::SharedPtr,
const std::shared_ptr<const Feedback>)>;
using ResultCallback = std::function<void (const WrappedResult & result)>;

virtual ~ClientGoalHandle();

Expand Down Expand Up @@ -116,11 +118,17 @@ class ClientGoalHandle
// The templated Client creates goal handles
friend Client<ActionT>;

ClientGoalHandle(const GoalInfo & info, FeedbackCallback callback);
ClientGoalHandle(
const GoalInfo & info,
FeedbackCallback feedback_callback,
ResultCallback result_callback);

void
set_feedback_callback(FeedbackCallback callback);

void
set_result_callback(ResultCallback callback);

void
call_feedback_callback(
typename ClientGoalHandle<ActionT>::SharedPtr shared_this,
Expand All @@ -145,6 +153,7 @@ class ClientGoalHandle
std::shared_future<WrappedResult> result_future_;

FeedbackCallback feedback_callback_{nullptr};
ResultCallback result_callback_{nullptr};
int8_t status_{GoalStatus::STATUS_ACCEPTED};

std::mutex handle_mutex_;
Expand Down
19 changes: 16 additions & 3 deletions rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,8 +28,11 @@ namespace rclcpp_action

template<typename ActionT>
ClientGoalHandle<ActionT>::ClientGoalHandle(
const GoalInfo & info, FeedbackCallback callback)
: info_(info), result_future_(result_promise_.get_future()), feedback_callback_(callback)
const GoalInfo & info, FeedbackCallback feedback_callback, ResultCallback result_callback)
: info_(info),
result_future_(result_promise_.get_future()),
feedback_callback_(feedback_callback),
result_callback_(result_callback)
{
}

Expand All @@ -42,7 +45,6 @@ template<typename ActionT>
const GoalUUID &
ClientGoalHandle<ActionT>::get_goal_id() const
{
// return info_.goal_id;
return info_.goal_id.uuid;
}

Expand Down Expand Up @@ -71,6 +73,9 @@ ClientGoalHandle<ActionT>::set_result(const WrappedResult & wrapped_result)
std::lock_guard<std::mutex> guard(handle_mutex_);
status_ = static_cast<int8_t>(wrapped_result.code);
result_promise_.set_value(wrapped_result);
if (result_callback_) {
result_callback_(wrapped_result);
}
}

template<typename ActionT>
Expand All @@ -81,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()
Expand Down
Loading