15 #ifndef RCLCPP_ACTION__CLIENT_HPP_
16 #define RCLCPP_ACTION__CLIENT_HPP_
26 #include <rosidl_runtime_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>(),
348 auto promise = std::make_shared<std::promise<typename GoalHandle::SharedPtr>>();
350 using GoalRequest =
typename ActionT::Impl::SendGoalService::Request;
351 auto goal_request = std::make_shared<GoalRequest>();
353 goal_request->goal = goal;
355 std::static_pointer_cast<void>(goal_request),
358 using GoalResponse =
typename ActionT::Impl::SendGoalService::Response;
359 auto goal_response = std::static_pointer_cast<GoalResponse>(response);
360 if (!goal_response->accepted) {
361 promise->set_value(
nullptr);
362 if (options.goal_response_callback) {
363 options.goal_response_callback(future);
368 goal_info.goal_id.uuid = goal_request->goal_id.uuid;
369 goal_info.stamp = goal_response->stamp;
372 new GoalHandle(goal_info, options.feedback_callback, options.result_callback));
375 goal_handles_[goal_handle->get_goal_id()] = goal_handle;
377 promise->set_value(goal_handle);
378 if (options.goal_response_callback) {
379 options.goal_response_callback(future);
382 if (options.result_callback) {
384 this->make_result_aware(goal_handle);
398 auto goal_handle_it = goal_handles_.begin();
399 while (goal_handle_it != goal_handles_.end()) {
400 if (!goal_handle_it->second.lock()) {
403 "Dropping weak reference to goal handle during send_goal()");
404 goal_handle_it = goal_handles_.erase(goal_handle_it);
424 typename GoalHandle::SharedPtr goal_handle,
428 if (goal_handles_.count(goal_handle->get_goal_id()) == 0) {
431 if (result_callback) {
433 goal_handle->set_result_callback(result_callback);
435 this->make_result_aware(goal_handle);
436 return goal_handle->async_get_result();
454 typename GoalHandle::SharedPtr goal_handle,
458 if (goal_handles_.count(goal_handle->get_goal_id()) == 0) {
461 auto cancel_request = std::make_shared<CancelRequest>();
463 cancel_request->goal_info.goal_id.uuid = goal_handle->get_goal_id();
464 return async_cancel(cancel_request, cancel_callback);
480 auto cancel_request = std::make_shared<CancelRequest>();
483 cancel_request->goal_info.goal_id.uuid.begin(),
484 cancel_request->goal_info.goal_id.uuid.end(), 0u);
485 return async_cancel(cancel_request, cancel_callback);
504 auto cancel_request = std::make_shared<CancelRequest>();
507 cancel_request->goal_info.goal_id.uuid.begin(),
508 cancel_request->goal_info.goal_id.uuid.end(), 0u);
509 cancel_request->goal_info.stamp = stamp;
510 return async_cancel(cancel_request, cancel_callback);
517 auto it = goal_handles_.begin();
518 while (it != goal_handles_.end()) {
519 typename GoalHandle::SharedPtr goal_handle = it->second.lock();
521 goal_handle->invalidate();
523 it = goal_handles_.erase(it);
530 create_goal_response()
const override
532 using GoalResponse =
typename ActionT::Impl::SendGoalService::Response;
538 create_result_response()
const override
540 using GoalResultResponse =
typename ActionT::Impl::GetResultService::Response;
546 create_cancel_response()
const override
553 create_feedback_message()
const override
555 using FeedbackMessage =
typename ActionT::Impl::FeedbackMessage;
564 using FeedbackMessage =
typename ActionT::Impl::FeedbackMessage;
565 typename FeedbackMessage::SharedPtr feedback_message =
566 std::static_pointer_cast<FeedbackMessage>(message);
567 const GoalUUID & goal_id = feedback_message->goal_id.uuid;
568 if (goal_handles_.count(goal_id) == 0) {
571 "Received feedback for unknown goal. Ignoring...");
574 typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id].lock();
579 "Dropping weak reference to goal handle during feedback callback");
580 goal_handles_.erase(goal_id);
583 auto feedback = std::make_shared<Feedback>();
584 *feedback = feedback_message->feedback;
585 goal_handle->call_feedback_callback(goal_handle, feedback);
590 create_status_message()
const override
592 using GoalStatusMessage =
typename ActionT::Impl::GoalStatusMessage;
601 using GoalStatusMessage =
typename ActionT::Impl::GoalStatusMessage;
602 auto status_message = std::static_pointer_cast<GoalStatusMessage>(message);
603 for (
const GoalStatus & status : status_message->status_list) {
604 const GoalUUID & goal_id = status.goal_info.goal_id.uuid;
605 if (goal_handles_.count(goal_id) == 0) {
608 "Received status for unknown goal. Ignoring...");
611 typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id].lock();
616 "Dropping weak reference to goal handle during status callback");
617 goal_handles_.erase(goal_id);
620 goal_handle->set_status(status.status);
626 make_result_aware(
typename GoalHandle::SharedPtr goal_handle)
629 if (goal_handle->set_result_awareness(
true)) {
632 using GoalResultRequest =
typename ActionT::Impl::GetResultService::Request;
633 auto goal_result_request = std::make_shared<GoalResultRequest>();
634 goal_result_request->goal_id.uuid = goal_handle->get_goal_id();
636 std::static_pointer_cast<void>(goal_result_request),
641 using GoalResultResponse =
typename ActionT::Impl::GetResultService::Response;
642 auto result_response = std::static_pointer_cast<GoalResultResponse>(response);
643 wrapped_result.result = std::make_shared<typename ActionT::Result>();
644 *wrapped_result.result = result_response->result;
645 wrapped_result.goal_id = goal_handle->get_goal_id();
646 wrapped_result.code =
static_cast<ResultCode>(result_response->status);
647 goal_handle->set_result(wrapped_result);
649 goal_handles_.erase(goal_handle->get_goal_id());
656 typename CancelRequest::SharedPtr cancel_request,
660 auto promise = std::make_shared<std::promise<typename CancelResponse::SharedPtr>>();
663 std::static_pointer_cast<void>(cancel_request),
666 auto cancel_response = std::static_pointer_cast<CancelResponse>(response);
667 promise->set_value(cancel_response);
668 if (cancel_callback) {
669 cancel_callback(cancel_response);
680 #endif // RCLCPP_ACTION__CLIENT_HPP_