15 #ifndef RCLCPP_ACTION__CLIENT_HPP_ 16 #define RCLCPP_ACTION__CLIENT_HPP_ 26 #include <rosidl_generator_c/action_type_support_struct.h> 27 #include <rosidl_typesupport_cpp/action_type_support.hpp> 69 template<
typename RepT =
int64_t,
typename RatioT = std::milli>
75 std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
128 rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
129 rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
130 rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
132 const rosidl_action_type_support_t * type_support,
254 template<
typename ActionT>
279 : goal_response_callback(nullptr),
280 feedback_callback(nullptr),
281 result_callback(nullptr)
316 rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
317 rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
318 rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
323 node_base, node_graph, node_logging, action_name,
324 rosidl_typesupport_cpp::get_action_type_support_handle<ActionT>(),
346 auto promise = std::make_shared<std::promise<typename GoalHandle::SharedPtr>>();
348 using GoalRequest =
typename ActionT::Impl::SendGoalService::Request;
349 auto goal_request = std::make_shared<GoalRequest>();
351 goal_request->goal = goal;
353 std::static_pointer_cast<void>(goal_request),
356 using GoalResponse =
typename ActionT::Impl::SendGoalService::Response;
358 if (!goal_response->accepted) {
359 promise->set_value(
nullptr);
360 if (options.goal_response_callback) {
361 options.goal_response_callback(future);
366 goal_info.goal_id.uuid = goal_request->goal_id.uuid;
367 goal_info.stamp = goal_response->stamp;
370 new GoalHandle(goal_info, options.feedback_callback, options.result_callback));
373 goal_handles_[goal_handle->get_goal_id()] = goal_handle;
375 promise->set_value(goal_handle);
376 if (options.goal_response_callback) {
377 options.goal_response_callback(future);
380 if (options.result_callback) {
382 this->make_result_aware(goal_handle);
402 typename GoalHandle::SharedPtr goal_handle,
406 if (goal_handles_.count(goal_handle->get_goal_id()) == 0) {
409 if (result_callback) {
411 goal_handle->set_result_callback(result_callback);
414 if (!goal_handle->is_result_aware()) {
415 this->make_result_aware(goal_handle);
417 return goal_handle->async_result();
435 typename GoalHandle::SharedPtr goal_handle,
439 if (goal_handles_.count(goal_handle->get_goal_id()) == 0) {
442 auto cancel_request = std::make_shared<CancelRequest>();
444 cancel_request->goal_info.goal_id.uuid = goal_handle->get_goal_id();
445 return async_cancel(cancel_request, cancel_callback);
461 auto cancel_request = std::make_shared<CancelRequest>();
464 cancel_request->goal_info.goal_id.uuid.begin(),
465 cancel_request->goal_info.goal_id.uuid.end(), 0u);
466 return async_cancel(cancel_request, cancel_callback);
485 auto cancel_request = std::make_shared<CancelRequest>();
488 cancel_request->goal_info.goal_id.uuid.begin(),
489 cancel_request->goal_info.goal_id.uuid.end(), 0u);
490 cancel_request->goal_info.stamp = stamp;
491 return async_cancel(cancel_request, cancel_callback);
498 auto it = goal_handles_.begin();
499 while (it != goal_handles_.end()) {
500 it->second->invalidate();
501 it = goal_handles_.erase(it);
510 using GoalResponse =
typename ActionT::Impl::SendGoalService::Response;
518 using GoalResultResponse =
typename ActionT::Impl::GetResultService::Response;
533 using FeedbackMessage =
typename ActionT::Impl::FeedbackMessage;
542 using FeedbackMessage =
typename ActionT::Impl::FeedbackMessage;
543 typename FeedbackMessage::SharedPtr feedback_message =
545 const GoalUUID & goal_id = feedback_message->goal_id.uuid;
546 if (goal_handles_.count(goal_id) == 0) {
549 "Received feedback for unknown goal. Ignoring...");
552 typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id];
553 auto feedback = std::make_shared<Feedback>();
554 *feedback = feedback_message->feedback;
555 goal_handle->call_feedback_callback(goal_handle, feedback);
562 using GoalStatusMessage =
typename ActionT::Impl::GoalStatusMessage;
571 using GoalStatusMessage =
typename ActionT::Impl::GoalStatusMessage;
573 for (
const GoalStatus & status : status_message->status_list) {
574 const GoalUUID & goal_id = status.goal_info.goal_id.uuid;
575 if (goal_handles_.count(goal_id) == 0) {
578 "Received status for unknown goal. Ignoring...");
581 typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id];
582 goal_handle->set_status(status.status);
583 const int8_t goal_status = goal_handle->get_status();
585 goal_status == GoalStatus::STATUS_SUCCEEDED ||
586 goal_status == GoalStatus::STATUS_CANCELED ||
587 goal_status == GoalStatus::STATUS_ABORTED)
589 goal_handles_.erase(goal_id);
596 make_result_aware(
typename GoalHandle::SharedPtr goal_handle)
598 using GoalResultRequest =
typename ActionT::Impl::GetResultService::Request;
599 auto goal_result_request = std::make_shared<GoalResultRequest>();
600 goal_result_request->goal_id.uuid = goal_handle->get_goal_id();
602 std::static_pointer_cast<void>(goal_result_request),
607 using GoalResultResponse =
typename ActionT::Impl::GetResultService::Response;
609 wrapped_result.result = std::make_shared<typename ActionT::Result>();
610 *wrapped_result.result = result_response->result;
611 wrapped_result.goal_id = goal_handle->get_goal_id();
612 wrapped_result.code =
static_cast<ResultCode>(result_response->status);
613 goal_handle->set_result(wrapped_result);
615 goal_handles_.erase(goal_handle->get_goal_id());
617 goal_handle->set_result_awareness(
true);
623 typename CancelRequest::SharedPtr cancel_request,
627 auto promise = std::make_shared<std::promise<typename CancelResponse::SharedPtr>>();
630 std::static_pointer_cast<void>(cancel_request),
634 promise->set_value(cancel_response);
635 if (cancel_callback) {
636 cancel_callback(cancel_response);
647 #endif // RCLCPP_ACTION__CLIENT_HPP_ virtual RCLCPP_ACTION_PUBLIC void send_result_request(std::shared_ptr< void > request, ResponseCallback callback)
SendGoalOptions()
Definition: client.hpp:278
Definition: client.hpp:57
Options for sending a goal.
Definition: client.hpp:276
typename ActionT::Goal Goal
Definition: client.hpp:260
std::shared_future< typename CancelResponse::SharedPtr > async_cancel_goal(typename GoalHandle::SharedPtr goal_handle, CancelCallback cancel_callback=nullptr)
Asynchronously request a goal be canceled.
Definition: client.hpp:434
virtual ~Client()
Definition: client.hpp:495
typename GoalHandle::ResultCallback ResultCallback
Definition: client.hpp:267
RCLCPP_ACTION_PUBLIC size_t get_number_of_ready_guard_conditions() override
Action Client.
Definition: client.hpp:255
virtual RCLCPP_ACTION_PUBLIC GoalUUID generate_goal_id()
ResultCallback result_callback
Function called when the result for the goal is received.
Definition: client.hpp:300
std::shared_future< WrappedResult > async_get_result(typename GoalHandle::SharedPtr goal_handle, ResultCallback result_callback=nullptr)
Asynchronously get the result for an active goal.
Definition: client.hpp:401
GoalResponse
A response returned by an action server callback when a goal is requested.
Definition: server.hpp:43
RCLCPP_ACTION_PUBLIC size_t get_number_of_ready_timers() override
typename ActionT::Feedback Feedback
Definition: client.hpp:261
ResultCode
The possible statuses that an action goal can finish with.
Definition: client_goal_handle.hpp:35
typename GoalHandle::WrappedResult WrappedResult
Definition: client.hpp:263
CancelResponse
A response returned by an action server callback when a goal has been asked to be canceled...
Definition: server.hpp:54
T current_exception(T... args)
typename ActionT::Impl::CancelGoalService::Response CancelResponse
Definition: client.hpp:269
RCLCPP_ACTION_PUBLIC ClientBase(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, const std::string &action_name, const rosidl_action_type_support_t *type_support, const rcl_action_client_options_t &options)
virtual std::shared_ptr< void > create_cancel_response() const =0
virtual RCLCPP_ACTION_PUBLIC void handle_result_response(const rmw_request_id_t &response_header, std::shared_ptr< void > result_response)
RCLCPP_ACTION_PUBLIC size_t get_number_of_ready_services() override
virtual std::shared_ptr< void > create_result_response() const =0
virtual RCLCPP_ACTION_PUBLIC ~ClientBase()
virtual std::shared_ptr< void > create_status_message() const =0
#define RCLCPP_DEBUG(logger,...)
Client(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, const std::string &action_name, const rcl_action_client_options_t client_options=rcl_action_client_get_default_options())
Construct an action client.
Definition: client.hpp:315
RCLCPP_ACTION_PUBLIC size_t get_number_of_ready_clients() override
std::shared_future< typename CancelResponse::SharedPtr > async_cancel_goals_before(const rclcpp::Time &stamp, CancelCallback cancel_callback=nullptr)
Asynchronously request all goals at or before a specified time be canceled.
Definition: client.hpp:481
virtual std::shared_ptr< void > create_goal_response() const =0
T static_pointer_cast(T... args)
std::shared_future< typename CancelResponse::SharedPtr > async_cancel_all_goals(CancelCallback cancel_callback=nullptr)
Asynchronously request for all goals to be canceled.
Definition: client.hpp:459
#define RCLCPP_ACTION_PUBLIC
Definition: visibility_control.hpp:50
typename GoalHandle::FeedbackCallback FeedbackCallback
Definition: client.hpp:266
#define RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(...)
virtual RCLCPP_ACTION_PUBLIC void handle_goal_response(const rmw_request_id_t &response_header, std::shared_ptr< void > goal_response)
action_msgs::msg::GoalStatus GoalStatus
Definition: types.hpp:33
GoalResponseCallback goal_response_callback
Function called when the goal is accepted or rejected.
Definition: client.hpp:294
typename ActionT::Impl::CancelGoalService::Request CancelRequest
Definition: client.hpp:268
action_msgs::msg::GoalInfo GoalInfo
Definition: types.hpp:34
RCL_ACTION_PUBLIC rcl_action_client_options_t rcl_action_client_get_default_options(void)
Definition: exceptions.hpp:24
RCLCPP_ACTION_PUBLIC bool wait_for_action_server_nanoseconds(std::chrono::nanoseconds timeout)
Wait for action_server_is_ready() to become true, or until the given timeout is reached.
Definition: client.hpp:44
virtual RCLCPP_ACTION_PUBLIC void send_goal_request(std::shared_ptr< void > request, ResponseCallback callback)
bool wait_for_action_server(std::chrono::duration< RepT, RatioT > timeout=std::chrono::duration< RepT, RatioT >(-1))
Wait for action_server_is_ready() to become true, or until the given timeout is reached.
Definition: client.hpp:71
virtual void handle_status_message(std::shared_ptr< void > message)=0
virtual RCLCPP_ACTION_PUBLIC void send_cancel_request(std::shared_ptr< void > request, ResponseCallback callback)
RCLCPP_ACTION_PUBLIC bool is_ready(rcl_wait_set_t *wait_set) override
FeedbackCallback feedback_callback
Function called whenever feedback is received for the goal.
Definition: client.hpp:297
virtual std::shared_ptr< void > create_feedback_message() const =0
Class for interacting with goals sent from action clients.
Definition: client_goal_handle.hpp:58
RCLCPP_ACTION_PUBLIC size_t get_number_of_ready_subscriptions() override
virtual RCLCPP_ACTION_PUBLIC void handle_cancel_response(const rmw_request_id_t &response_header, std::shared_ptr< void > cancel_response)
virtual void handle_feedback_message(std::shared_ptr< void > message)=0
RCLCPP_ACTION_PUBLIC rclcpp::Logger get_logger()
RCLCPP_ACTION_PUBLIC void execute() override
RCLCPP_ACTION_PUBLIC bool add_to_wait_set(rcl_wait_set_t *wait_set) override
std::shared_future< typename GoalHandle::SharedPtr > async_send_goal(const Goal &goal, const SendGoalOptions &options=SendGoalOptions())
Send an action goal and asynchronously get the result.
Definition: client.hpp:343
RCLCPP_ACTION_PUBLIC bool action_server_is_ready() const
Return true if there is an action server that is ready to take goal requests.