From ecf8b5f9ae6d793ee1ae271d7c1035c3a415f169 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 20 Nov 2018 17:55:57 -0300 Subject: [PATCH 01/56] WIP --- .../include/rclcpp_action/client.hpp | 236 ++++++++++++++++-- .../rclcpp_action/client_goal_handle.hpp | 1 - .../include/rclcpp_action/create_client.hpp | 2 +- .../include/rclcpp_action/exceptions.hpp | 35 +++ rclcpp_action/include/rclcpp_action/types.hpp | 40 +++ rclcpp_action/src/client.cpp | 149 ++++++++++- 6 files changed, 437 insertions(+), 26 deletions(-) create mode 100644 rclcpp_action/include/rclcpp_action/exceptions.hpp create mode 100644 rclcpp_action/include/rclcpp_action/types.hpp diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 8a227f0b76..acc7ff1333 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -36,20 +36,50 @@ class ClientBaseImpl; /// Base Action Client implementation /// It is responsible for interfacing with the C action client API. -class ClientBase +class ClientBase : public Waitable { public: // TODO(sloretz) NodeLoggingInterface when it can be gotten off a node RCLCPP_ACTION_PUBLIC ClientBase( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, - const std::string & name, - const rosidl_action_type_support_t * type_support); + const std::string & action_name, + const rosidl_action_type_support_t * type_support, + const rcl_action_client_options_t & options); RCLCPP_ACTION_PUBLIC virtual ~ClientBase(); - // TODO(sloretz) add a link between this class and callbacks in the templated + RCLCPP_ACTION_PUBLIC + void handle_goal_response( + std::shared_ptr response_header, + std::shared_ptr response); + + RCLCPP_ACTION_PUBLIC + void handle_result_response( + std::shared_ptr response_header, + std::shared_ptr response); + + RCLCPP_ACTION_PUBLIC + void handle_cancel_response( + std::shared_ptr request_header, + std::shared_ptr response); + +protected: + RCLCPP_ACTION_PUBLIC + void send_goal_request( + std::shared_ptr request, + ResponseCallback callback); + + RCLCPP_ACTION_PUBLIC + void send_result_request( + std::shared_ptr request, + ResponseCallback callback); + + RCLCPP_ACTION_PUBLIC + void send_cancel_request( + std::shared_ptr request, + ResponseCallback callback); private: std::unique_ptr pimpl_; @@ -65,35 +95,207 @@ class Client : public ClientBase public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Client) + using Goal = typename ACTION::Goal; + using Feedback = typename ACTION::Feedback; + using Result = typename ACTION::Result; + using GoalHandle = ClientGoalHandle; using FeedbackCallback = std::function>, const typename ACTION::Feedback)>; - + GoalHandle::SharedPtr, const Feedback &)>; + using CancelRequest = typename ACTION::CancelGoalService::Request; + using CancelResponse = typename ACTION::CancelGoalService::Response; + Client( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, - const std::string & name + const std::string & action_name, const rcl_action_client_options_t & client_options ) : ClientBase( - node_base, - name, - rosidl_typesupport_cpp::get_action_type_support_handle()) + node_base, action_name, + rosidl_typesupport_cpp::get_action_type_support_handle(), + client_options) + { + } + + RCLCPP_ACTION_PUBLIC + void handle_feedback(std::shared_ptr message) { + using Feedback = typename ACTION::Feedback; + Feedback::SharedPtr feedback = std::static_pointer_cast(message); + std::unique_lock lock(goal_handles_mutex_); + if (goal_handles_.count(feedback->goal_id) == 0) { + RCUTILS_LOG_DEBUG_NAMED( + "rclcpp", "Received feedback for unknown goal. Ignoring..."); + return; + } + GoalHandle::SharedPtr goal_handle = goal_handles_[feedback->goal_id]; + if (!goal_handle->is_feedback_aware()) { + RCUTILS_LOG_DEBUG_NAMED( + "rclcpp", "Received feedback for goal %s, but it ignores feedback."); + return; + } + const FeedbackCallback & callback = goal_handle->get_feedback_callback(); + callback(goal_handle, *feedback); + } + + RCLCPP_ACTION_PUBLIC + void handle_status(std::shared_ptr message) { - // TODO(sloretz) what's the link that causes a feedback topic to be called ? + using GoalStatusArray = typename ACTION::GoalStatusArray; + GoalStatusArray::SharedPtr status_array = + std::static_pointer_cast(message); + std::lock_guard lock(goal_handles_mutex_); + for (const GoalStatus & status : status_array.status_list) + { + const GoalID & goal_id = status.goal_info.goal_id; + if (goal_handles_.count(goal_id) != 0) + { + goal_handles_[goal_id]->set_status(status); + if (status.status == STATUS_CANCELED || status.status == STATUS_ABORTED) + { + goal_handles_.erase(goal_id); + } + } + } } - ClientGoalHandle - async_send_goal(typename ACTION::Goal * goal, FeedbackCallback callback = nullptr); + RCLCPP_ACTION_PUBLIC + std::shared_future async_send_goal( + Goal::SharedPtr goal, FeedbackCallback callback = nullptr, bool ignore_result = false) + { + + if (pending_goal_handles_.count(goal.goal_id) != 0) { + + } + if (goal_handles_.count(goal.goal_id) != 0) { + + } + std::promise promise; + std::shared_future future(promise.get_future()); + this->send_goal_request( + std::static_pointer_cast(goal), + [this, promise{std::move(promise)}, callback] (std::shared_ptr response) + { + using GoalResponse = typename ACTION::GoalService::Response; + GoalResponse::SharedPtr goal_response = + std::static_pointer_cast(response); + if (!goal_response.accepted) { + promise->set_exception( + std::make_exception_ptr(RejectedGoalError(""))); + } + auto goal_handle = std::make_shared(goal_, callback); + if (!ignore_result) { + try { + this->async_get_result(goal_handle); + } catch (...) { + promise->set_exception(std::current_exception()); + return; + } + } + goal_handles_[goal_id] = goal_handles; + pending_goal_handles_.erase(goal_id); + promise->set_value(goal_handle); + }); + return future; + } RCLCPP_ACTION_PUBLIC - std::future - async_cancel_all_goals(); + std::shared_future + async_get_result_response(GoalHandle::SharedPtr goal_handle) { + std::lock_guard lock(goal_handles_mutex_); + const GoalID & goal_id = goal_handle->goal_id(); + if (goal_handles_.count(goal_id) == 0) { + throw; + } + if (!goal_handle->tracks_result()) { + using ResultRequest = typename ACTION::GoalResultService::Request; + auto result_request = std::make_shared(); + result_request.goal_id = goal_handle->goal_id(); + this->send_result_request( + std::static_pointer_cast(result_request), + [goal_handle] (std::shared_ptr response) { + Result::SharedPtr result = + std::static_pointer_cast(response); + goal_handle->set_result(result); + }); + goal_handle->set_result_tracking(); + } + return goal_handle->async_result(); + } RCLCPP_ACTION_PUBLIC - std::future - async_cancel_goals_before(rclcpp::Time); + std::shared_future + async_cancel_goal(GoalHandle::SharedPtr goal_handle) { + std::lock_guard lock(goal_handles_mutex_); + const GoalID & goal_id = goal_handle->goal_id(); + if (goal_handles_.count(goal_id) == 0) { + throw; + } + std::promise promise; + std::shared_future future(promise.get_future()); + auto cancel_request = std::make_shared(); + cancel_request->goal_info.goal_id = goal_id; + this->send_cancel_request( + std::static_pointer_cast(cancel_request), + [this, goal_handle, promise{std::move(promise)}] (std::shared_ptr response) { + CancelGoalResponse::SharedPtr cancel_response = + std::static_pointer_cast(response); + bool goal_canceled = false; + if (!cancel_response->goals_canceling.empty()) { + const GoalInfo & canceled_goal_info = cancel_response->goals_canceling[0]; + const GoalID & canceled_goal_id = canceled_goal_info.goal_id; + const GoalID & to_be_canceled_goal_id = goal_handle->goal_id(); + if (!uuidcmp(canceled_goal_id.uuid, to_be_canceled_goal_id.uuid)) { + goal_canceled = canceled_goal_info.accepted; + } + } + promise.set_value(goal_canceled); + }); + return future; + } + + RCLCPP_ACTION_PUBLIC + std::shared_future + async_cancel_all_goals() { + auto cancel_request = std::make_shared(); + cancel_request->goal_info.goal_id.uuid = zerouuid; + return async_cancel(cancel_request); + } + + RCLCPP_ACTION_PUBLIC + std::shared_future + async_cancel_goals_before(const rclcpp::Time & stamp) { + auto cancel_request = std::make_shared(); + cancel_request->goal_info.goal_id.uuid = zerouuid; + cancel_request->goal_info.stamp = stamp; + return async_cancel(cancel_request); + } virtual ~Client() { + std::unique_lock lock(goal_handles_mutex_); + auto it = goal_handles_.begin(); + while (it != it->goal_handles_.end()) { + it->second->set_invalid(); + it = goal_handles_.erase(it); + } } + + private: + std::shared_future + async_cancel(CancelGoalRequest::SharedPtr cancel_request) { + std::promise promise; + std::shared_future future(promise.get_future()); + this->send_cancel_request( + std::static_pointer_cast(cancel_request), + [promise{std::move(promise)}] (std::shared_ptr response) { + CancelGoalResponse::SharedPtr cancel_response = + std::static_pointer_cast(response); + promise.set_value(cancel_response); + }); + return future; + } + + std::map goal_handles_; + std::mutex goal_handles_mutex_; }; } // namespace rclcpp_action + #endif // RCLCPP_ACTION__CLIENT_HPP_ diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp index 87a663c596..3922b9cd2e 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp @@ -34,7 +34,6 @@ class ClientGoalHandle public: virtual ~ClientGoalHandle(); - /// TODO(sloretz) examples have this on client as `async_cancel_goal(goal_handle)` std::future async_cancel(); diff --git a/rclcpp_action/include/rclcpp_action/create_client.hpp b/rclcpp_action/include/rclcpp_action/create_client.hpp index 8f6381ccb9..a7d5762725 100644 --- a/rclcpp_action/include/rclcpp_action/create_client.hpp +++ b/rclcpp_action/include/rclcpp_action/create_client.hpp @@ -36,5 +36,5 @@ create_client( name); } } // namespace rclcpp_action + #endif // RCLCPP_ACTION__CREATE_CLIENT_HPP_ -// TODO(sloretz) rclcpp_action::create_client<>(node, action_name); diff --git a/rclcpp_action/include/rclcpp_action/exceptions.hpp b/rclcpp_action/include/rclcpp_action/exceptions.hpp new file mode 100644 index 0000000000..ffdf125e65 --- /dev/null +++ b/rclcpp_action/include/rclcpp_action/exceptions.hpp @@ -0,0 +1,35 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP_ACTION__EXCEPTIONS_HPP_ +#define RCLCPP_ACTION__EXCEPTIONS_HPP_ + +#include + +namespace rclcpp_action +{ +namespace exceptions +{ + +class InvalidGoalHandle : public std::invalid_argument +{ + InvalidGoalHandle() + : std::invalid_argument("") {} +}; + +} // namespace exceptions + +} // namespace rclcpp_action + +#endif // RCLCPP_ACTION__EXCEPTIONS_HPP_ diff --git a/rclcpp_action/include/rclcpp_action/types.hpp b/rclcpp_action/include/rclcpp_action/types.hpp new file mode 100644 index 0000000000..a7a6650ebd --- /dev/null +++ b/rclcpp_action/include/rclcpp_action/types.hpp @@ -0,0 +1,40 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP_ACTION__TYPES_HPP_ +#define RCLCPP_ACTION__TYPES_HPP_ + +#include + +namespace rclcpp_action { + +using GoalID = unique_identifier_msgs::msg::UUID; + +} // namespace + +namespace std { + +template<> +struct less { + bool operator()( + const rclcpp_action::GoalID & id0, + const rclcpp_action::GoalID & id1) { + return !uuidcmp(id0.uuid, id1.uuid); + } +}; + +} // namespace std + + +#endif // RCLCPP_ACTION__TYPES_HPP_ diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index a094e63faa..44e22693bd 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -16,26 +16,161 @@ #include +#include + + using rclcpp_action::ClientBase; namespace rclcpp_action { class ClientBaseImpl { +public: + ClientBaseImpl( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + const std::string & action_name, + const rosidl_action_type_support_t * type_support, + const rcl_action_client_options_t & client_options) + { + std::weak_ptr weak_node_handle(node_handle_); + client_handle_ = std::shared_ptr( + new rcl_client_t, [weak_node_handle](rcl_action_client_t * client) + { + auto handle = weak_node_handle.lock(); + if (handle) { + if (RCL_RET_OK != rcl_action_client_fini(client, handle.get())) { + RCLCPP_ERROR( + rclcpp::get_logger(rcl_node_get_logger_name(handle.get())).get_child("rclcpp"), + "Error in destruction of rcl client handle: %s", rcl_get_error_string().str); + rcl_reset_error(); + } + } else { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "Error in destruction of rcl client handle: " + "the Node Handle was destructed too early. You will leak memory"); + } + delete client; + }); + *client_handle_ = rcl_get_zero_initialized_client(); + rcl_ret_t ret = rcl_action_client_init( + client_handle_.get(), node_handle_.get(), type_support, + action_name.c_str(), &action_client_options); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "could not create action client"); + } + + } + + void + handle_goal_response( + std::shared_ptr response_header, + std::shared_ptr response) + { + std::unique_lock lock(goals_mutex_); + const int64_t sequence_number = response_header->sequence_number; + if (pending_goal_responses_.count(sequence_number) == 0) { + RCUTILS_LOG_ERROR_NAMED("rclcpp", "unknown goal response, ignoring..."); + return; + } + pending_goal_responses_[sequence_number](response); + pending_goal_responses_.erase(sequence_number); + } + + void + send_goal_request(std::shared_ptr request, ResponseCallback callback) + { + std::unique_lock lock(goals_mutex_); + int64_t sequence_number; + rcl_ret_t ret = rcl_action_send_goal_request( + client_handle_.get(), request.get(), &sequence_number); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send goal request"); + } + if (pending_goal_responses_.count(sequence_number) != 0) { + + } + pending_goal_responses_[sequence_number] = callback; + } + + void + send_result_request(std::shared_ptr request, ResponseCallback callback) + { + std::unique_lock lock(results_mutex_); + int64_t sequence_number; + rcl_ret_t ret = rcl_action_send_result_request( + client_handle_.get(), request.get(), &sequence_number); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send goal request"); + } + if (pending_result_responses_.count(sequence_number) != 0) { + + } + pending_result_responses_[sequence_number] = callback; + } + + void + handle_cancel_response( + std::shared_ptr response_header, + std::shared_ptr response) + { + std::unique_lock lock(cancellations_mutex_); + const int64_t sequence_number = response_header->sequence_number; + if (pending_cancel_responses_.count(sequence_number) == 0) { + RCUTILS_LOG_ERROR_NAMED("rclcpp", "unknown cancel response, ignoring..."); + return; + } + pending_cancel_responses_[sequence_number](response); + pending_cancel_responses_.erase(sequence_number); + } + + void + send_cancel_request(std::shared_ptr request, ResponseCallback callback) + { + std::unique_lock lock(cancellations_mutex_); + int64_t sequence_number; + rcl_ret_t ret = rcl_action_send_cancel_request( + client_handle_.get(), request.get(), &sequence_number); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send cancel request"); + } + if (pending_cancel_responses_.count(sequence_number) != 0) { + + } + pending_cancel_responses_[sequence_number] = callback; + } + +private: + std::map pending_goal_responses_; + std::map pending_result_responses_; + std::map pending_cancel_responses_; + std::shared_ptr client_handle_; + std::shared_ptr node_handle_; + std::mutex client_mutex_; }; -} +} ClientBase::ClientBase( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, - const std::string & name, - const rosidl_action_type_support_t * type_support) + const std::string & action_name, + const rosidl_action_type_support_t * type_support, + const rcl_action_client_options_t & client_options) + : pimpl_(new ClientBaseImpl( + node_base, action_name, + type_support, client_options)) +{ + +} + +bool +ClientBase::wait_for_action_nanoseconds(std::chrono::nanoseconds timeout) { - // TODO(sloretz) use rcl_action API when available - (void)node_base; - (void)name; - (void)type_support; + } + ClientBase::~ClientBase() { + } From ad227d4be720f81b8a7e3926f93f76cab155a963 Mon Sep 17 00:00:00 2001 From: Samuel Servulo Date: Mon, 26 Nov 2018 13:02:26 -0200 Subject: [PATCH 02/56] Added basic waitable interface to action client --- .../include/rclcpp_action/client.hpp | 9 +++ rclcpp_action/src/client.cpp | 56 ++++++++++++++++++- 2 files changed, 64 insertions(+), 1 deletion(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index acc7ff1333..a517fff84e 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -82,6 +82,15 @@ class ClientBase : public Waitable ResponseCallback callback); private: + void set_rcl_entities(); + + // rcl interface entities + site_t num_subscriptions_, + num_guard_conditions_, + num_timers_, + num_clients_, + num_services_; + std::unique_ptr pimpl_; }; diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index 44e22693bd..e6ca667034 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -160,7 +160,7 @@ ClientBase::ClientBase( node_base, action_name, type_support, client_options)) { - + set_rcl_entities(); } bool @@ -174,3 +174,57 @@ ClientBase::~ClientBase() { } + +void +ClientBase::set_rcl_entities() +{ + rcl_action_client_wait_set_get_num_entities( + num_subscriptions_. + num_guard_conditions_, + num_timers_, + num_clients_, + num_services_ + ); +} + +size_t +ClientBase::get_number_of_ready_subscriptions() override +{ + return num_subscriptions_; +} + +size_t +ClientBase::get_number_of_ready_guard_conditions() override +{ + return num_guard_conditions_; +} + +size_t +ClientBase::get_number_of_ready_timers() override +{ + return num_timers_; +} + +size_t +ClientBase::get_number_of_ready_clients() override +{ + return num_clients_; +} + +size_t +ClientBase::get_number_of_ready_services() override +{ + return num_services_; +} + +bool +ClientBase::add_to_wait_set(rcl_wait_set_t * wait_set) override +{} + +bool +ClientBase::is_ready(rcl_wait_set_t *) override +{} + +void +ClientBase::execute() override +{} From 584ab259925a7ecd2b32745329ee76a7eef4e646 Mon Sep 17 00:00:00 2001 From: Samuel Servulo Date: Mon, 26 Nov 2018 17:27:03 -0200 Subject: [PATCH 03/56] Updated waitable execute from action client --- rclcpp_action/src/client.cpp | 84 +++++++++++++++++++++++++++++++++--- 1 file changed, 78 insertions(+), 6 deletions(-) diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index e6ca667034..55f202172f 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -141,6 +141,12 @@ class ClientBaseImpl pending_cancel_responses_[sequence_number] = callback; } + std::shared_ptr + get_action_client() + { + return client_handle_; + } + private: std::map pending_goal_responses_; std::map pending_result_responses_; @@ -183,8 +189,7 @@ ClientBase::set_rcl_entities() num_guard_conditions_, num_timers_, num_clients_, - num_services_ - ); + num_services_); } size_t @@ -219,12 +224,79 @@ ClientBase::get_number_of_ready_services() override bool ClientBase::add_to_wait_set(rcl_wait_set_t * wait_set) override -{} +{ + rcl_action_wait_set_add_action_client( + wait_set, + pimpl_->get_action_client().get()); +} bool -ClientBase::is_ready(rcl_wait_set_t *) override -{} +ClientBase::is_ready(rcl_wait_set_t * wait_set) override +{ + + bool is_feedback_ready, + is_status_ready, + is_goal_response_ready, + is_cancel_response_ready, + is_result_response_ready; + + rcl_action_client_wait_set_get_entities_ready( + wait_set, + pimpl_->get_action_client().get(), + &is_feedback_ready, + &is_status_ready, + &is_goal_response_ready, + &is_cancel_response_ready, + &is_result_response_ready); + + return is_feedback_ready || + is_status_ready || + is_goal_response_ready || + is_cancel_response_ready || + is_result_response_ready; +} void ClientBase::execute() override -{} +{ + bool is_feedback_ready, + is_status_ready, + is_goal_response_ready, + is_cancel_response_ready, + is_result_response_ready; + + auto rcl_action_client = pimpl_->get_action_client().get(); + + rcl_action_client_wait_set_get_entities_ready( + wait_set, + rcl_action_client, + &is_feedback_ready, + &is_status_ready, + &is_goal_response_ready, + &is_cancel_response_ready, + &is_result_response_ready); + + void * message = nullptr; + if(is_feedback_ready) + { + rcl_action_take_feedback(rcl_action_client, message); + } + if(is_status_ready) + { + rcl_action_take_status(rcl_action_client, message); + } + if(is_goal_response_ready) + { + rcl_action_take_goal_response(rcl_action_client, message); + } + if(is_cancel_response_ready) + { + rcl_action_take_cancel_response(rcl_action_client, message); + } + if(is_result_response_ready) + { + rcl_action_take_result_response(rcl_action_client, message); + } +} + +//TODO: treat return values \ No newline at end of file From e221967f0ca6d263029453797a3975235063e928 Mon Sep 17 00:00:00 2001 From: Samuel Servulo Date: Mon, 26 Nov 2018 17:37:21 -0200 Subject: [PATCH 04/56] Added throw for rcl calls in action client --- rclcpp_action/src/client.cpp | 23 ++++++++++++++++++++--- 1 file changed, 20 insertions(+), 3 deletions(-) diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index 55f202172f..aafe68e01c 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -184,12 +184,16 @@ ClientBase::~ClientBase() void ClientBase::set_rcl_entities() { - rcl_action_client_wait_set_get_num_entities( + rcl_ret_t ret = rcl_action_client_wait_set_get_num_entities( num_subscriptions_. num_guard_conditions_, num_timers_, num_clients_, num_services_); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to set action client rcl entities"); + } } size_t @@ -225,9 +229,13 @@ ClientBase::get_number_of_ready_services() override bool ClientBase::add_to_wait_set(rcl_wait_set_t * wait_set) override { - rcl_action_wait_set_add_action_client( + rcl_ret_t ret = rcl_action_wait_set_add_action_client( wait_set, pimpl_->get_action_client().get()); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to add action client to wait set"); + } } bool @@ -240,7 +248,7 @@ ClientBase::is_ready(rcl_wait_set_t * wait_set) override is_cancel_response_ready, is_result_response_ready; - rcl_action_client_wait_set_get_entities_ready( + rcl_ret_t ret = rcl_action_client_wait_set_get_entities_ready( wait_set, pimpl_->get_action_client().get(), &is_feedback_ready, @@ -249,6 +257,10 @@ ClientBase::is_ready(rcl_wait_set_t * wait_set) override &is_cancel_response_ready, &is_result_response_ready); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed get ready entities in action client"); + } + return is_feedback_ready || is_status_ready || is_goal_response_ready || @@ -276,6 +288,11 @@ ClientBase::execute() override &is_cancel_response_ready, &is_result_response_ready); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, + "failed get ready entities to execute in action client"); + } + void * message = nullptr; if(is_feedback_ready) { From ce1b068f2354513d3b800216468ab0a582262b6e Mon Sep 17 00:00:00 2001 From: Samuel Servulo Date: Mon, 26 Nov 2018 17:49:59 -0200 Subject: [PATCH 05/56] Removed duplicated ready flags from action client --- .../include/rclcpp_action/client.hpp | 6 ++ rclcpp_action/src/client.cpp | 58 +++++-------------- 2 files changed, 21 insertions(+), 43 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index a517fff84e..e10c07e798 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -91,6 +91,12 @@ class ClientBase : public Waitable num_clients_, num_services_; + bool is_feedback_ready_, + is_status_ready_, + is_goal_response_ready_, + is_cancel_response_ready_, + is_result_response_ready_; + std::unique_ptr pimpl_; }; diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index aafe68e01c..24bb5cf933 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -242,75 +242,47 @@ bool ClientBase::is_ready(rcl_wait_set_t * wait_set) override { - bool is_feedback_ready, - is_status_ready, - is_goal_response_ready, - is_cancel_response_ready, - is_result_response_ready; - rcl_ret_t ret = rcl_action_client_wait_set_get_entities_ready( wait_set, pimpl_->get_action_client().get(), - &is_feedback_ready, - &is_status_ready, - &is_goal_response_ready, - &is_cancel_response_ready, - &is_result_response_ready); + &is_feedback_ready_, + &is_status_ready_, + &is_goal_response_ready_, + &is_cancel_response_ready_, + &is_result_response_ready_); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret, "failed get ready entities in action client"); } - return is_feedback_ready || - is_status_ready || - is_goal_response_ready || - is_cancel_response_ready || - is_result_response_ready; + return is_feedback_ready_ || + is_status_ready_ || + is_goal_response_ready_ || + is_cancel_response_ready_ || + is_result_response_ready_; } void ClientBase::execute() override { - bool is_feedback_ready, - is_status_ready, - is_goal_response_ready, - is_cancel_response_ready, - is_result_response_ready; - - auto rcl_action_client = pimpl_->get_action_client().get(); - - rcl_action_client_wait_set_get_entities_ready( - wait_set, - rcl_action_client, - &is_feedback_ready, - &is_status_ready, - &is_goal_response_ready, - &is_cancel_response_ready, - &is_result_response_ready); - - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret, - "failed get ready entities to execute in action client"); - } - void * message = nullptr; - if(is_feedback_ready) + if(is_feedback_ready_) { rcl_action_take_feedback(rcl_action_client, message); } - if(is_status_ready) + if(is_status_ready_) { rcl_action_take_status(rcl_action_client, message); } - if(is_goal_response_ready) + if(is_goal_response_ready_) { rcl_action_take_goal_response(rcl_action_client, message); } - if(is_cancel_response_ready) + if(is_cancel_response_ready_) { rcl_action_take_cancel_response(rcl_action_client, message); } - if(is_result_response_ready) + if(is_result_response_ready_) { rcl_action_take_result_response(rcl_action_client, message); } From ffcb8bb49d3480395c4e510bb7a57f0db9edc454 Mon Sep 17 00:00:00 2001 From: Samuel Servulo Date: Mon, 26 Nov 2018 18:58:13 -0200 Subject: [PATCH 06/56] Minor fix --- rclcpp_action/include/rclcpp_action/client.hpp | 2 +- rclcpp_action/src/client.cpp | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index e10c07e798..6dd9fd90db 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -230,7 +230,7 @@ class Client : public ClientBase std::static_pointer_cast(response); goal_handle->set_result(result); }); - goal_handle->set_result_tracking(); + goal_handle->set_result_awareness(true); } return goal_handle->async_result(); } diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index 24bb5cf933..0d76165c07 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -288,4 +288,3 @@ ClientBase::execute() override } } -//TODO: treat return values \ No newline at end of file From d888fd5a351c8ceeaa234cebe06c3c82875d0a04 Mon Sep 17 00:00:00 2001 From: Samuel Servulo Date: Mon, 26 Nov 2018 19:32:30 -0200 Subject: [PATCH 07/56] Added header to action ClientBaseImpl execute --- rclcpp_action/src/client.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index 0d76165c07..b424ae3666 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -266,6 +266,7 @@ void ClientBase::execute() override { void * message = nullptr; + rmw_request_id_t * header = nullptr; if(is_feedback_ready_) { rcl_action_take_feedback(rcl_action_client, message); @@ -276,15 +277,15 @@ ClientBase::execute() override } if(is_goal_response_ready_) { - rcl_action_take_goal_response(rcl_action_client, message); + rcl_action_take_goal_response(rcl_action_client, header, message); } if(is_cancel_response_ready_) { - rcl_action_take_cancel_response(rcl_action_client, message); + rcl_action_take_cancel_response(rcl_action_client, header, message); } if(is_result_response_ready_) { - rcl_action_take_result_response(rcl_action_client, message); + rcl_action_take_result_response(rcl_action_client, header, message); } } From 440ee032764833ba2e25427bef9dce27e224d16c Mon Sep 17 00:00:00 2001 From: Samuel Servulo Date: Tue, 27 Nov 2018 11:08:33 -0200 Subject: [PATCH 08/56] Mich's update to action client interface --- .../include/rclcpp_action/client.hpp | 88 +++++++----- rclcpp_action/src/client.cpp | 136 +++++++++++------- 2 files changed, 138 insertions(+), 86 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 6dd9fd90db..23919d61fe 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -50,25 +50,9 @@ class ClientBase : public Waitable RCLCPP_ACTION_PUBLIC virtual ~ClientBase(); - RCLCPP_ACTION_PUBLIC - void handle_goal_response( - std::shared_ptr response_header, - std::shared_ptr response); - - RCLCPP_ACTION_PUBLIC - void handle_result_response( - std::shared_ptr response_header, - std::shared_ptr response); - - RCLCPP_ACTION_PUBLIC - void handle_cancel_response( - std::shared_ptr request_header, - std::shared_ptr response); - protected: RCLCPP_ACTION_PUBLIC - void send_goal_request( - std::shared_ptr request, + void send_goal_request(std::shared_ptr request, ResponseCallback callback); RCLCPP_ACTION_PUBLIC @@ -82,20 +66,33 @@ class ClientBase : public Waitable ResponseCallback callback); private: - void set_rcl_entities(); + std::shared_ptr create_response_header(); + + virtual std::shared_ptr create_goal_response() = 0; + + void handle_goal_response( + std::shared_ptr response_header, + std::shared_ptr response); + + virtual std::shared_ptr create_result_response() = 0; - // rcl interface entities - site_t num_subscriptions_, - num_guard_conditions_, - num_timers_, - num_clients_, - num_services_; + void handle_result_response( + std::shared_ptr response_header, + std::shared_ptr response); - bool is_feedback_ready_, - is_status_ready_, - is_goal_response_ready_, - is_cancel_response_ready_, - is_result_response_ready_; + virtual std::shared_ptr create_cancel_response() = 0; + + void handle_cancel_response( + std::shared_ptr request_header, + std::shared_ptr response); + + virtual std::shared_ptr create_feedback_message() = 0; + + virtual void handle_feedback_message(std::shared_ptr message) = 0; + + virtual std::shared_ptr create_status_message() = 0; + + virtual void handle_status_message(std::shared_ptr message) = 0; std::unique_ptr pimpl_; }; @@ -111,11 +108,10 @@ class Client : public ClientBase RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Client) using Goal = typename ACTION::Goal; - using Feedback = typename ACTION::Feedback; using Result = typename ACTION::Result; using GoalHandle = ClientGoalHandle; using FeedbackCallback = std::function; + GoalHandle::SharedPtr, const typename ACTION::Feedback &)>; using CancelRequest = typename ACTION::CancelGoalService::Request; using CancelResponse = typename ACTION::CancelGoalService::Response; @@ -130,8 +126,27 @@ class Client : public ClientBase { } + std::shared_ptr create_goal_response() override + { + using GoalResponse = typename ACTION::GoalService::Response; + return std::shared_ptr(new GoalResponse()); + } + + std::shared_ptr create_result_response() override + { + using GoalResultResponse = typename ACTION::GoalResultService::Response; + return std::shared_ptr(new GoalResultResponse()); + } + + std::shared_ptr create_feedback_message() override + { + using Feedback = typename ACTION::Feedback; + return std::shared_ptr(new Feedback()); + } + RCLCPP_ACTION_PUBLIC - void handle_feedback(std::shared_ptr message) { + void handle_feedback_message(std::shared_ptr message) override + { using Feedback = typename ACTION::Feedback; Feedback::SharedPtr feedback = std::static_pointer_cast(message); std::unique_lock lock(goal_handles_mutex_); @@ -151,7 +166,14 @@ class Client : public ClientBase } RCLCPP_ACTION_PUBLIC - void handle_status(std::shared_ptr message) + std::shared_ptr create_status_message() override + { + using GoalStatusArray = typename ACTION::GoalStatusArray; + return std::shared_ptr(new GoalStatusArray()); + } + + RCLCPP_ACTION_PUBLIC + void handle_status_message(std::shared_ptr message) override { using GoalStatusArray = typename ACTION::GoalStatusArray; GoalStatusArray::SharedPtr status_array = diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index b424ae3666..bc34e0cebc 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -60,7 +60,17 @@ class ClientBaseImpl rclcpp::exceptions::throw_from_rcl_error( ret, "could not create action client"); } - + ret = rcl_action_client_wait_set_get_num_entities( + client_handle_.get() + &num_subscriptions, + &num_guard_conditions, + &num_timers, + &num_clients, + &num_services); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "could not retrieve action client details"); + } } void @@ -88,12 +98,10 @@ class ClientBaseImpl if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send goal request"); } - if (pending_goal_responses_.count(sequence_number) != 0) { - - } + assert(pending_goal_responses_.count(sequence_number) != 0); pending_goal_responses_[sequence_number] = callback; } - + void send_result_request(std::shared_ptr request, ResponseCallback callback) { @@ -104,9 +112,7 @@ class ClientBaseImpl if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send goal request"); } - if (pending_result_responses_.count(sequence_number) != 0) { - - } + assert(pending_result_responses_.count(sequence_number) != 0); pending_result_responses_[sequence_number] = callback; } @@ -147,6 +153,17 @@ class ClientBaseImpl return client_handle_; } + // rcl interface entities + site_t num_subscriptions_, + num_guard_conditions_, + num_timers_, + num_clients_, + num_services_; + bool is_feedback_ready_, + is_status_ready_, + is_goal_response_ready_, + is_cancel_response_ready_, + is_result_response_ready_; private: std::map pending_goal_responses_; std::map pending_result_responses_; @@ -166,7 +183,6 @@ ClientBase::ClientBase( node_base, action_name, type_support, client_options)) { - set_rcl_entities(); } bool @@ -181,80 +197,58 @@ ClientBase::~ClientBase() } -void -ClientBase::set_rcl_entities() -{ - rcl_ret_t ret = rcl_action_client_wait_set_get_num_entities( - num_subscriptions_. - num_guard_conditions_, - num_timers_, - num_clients_, - num_services_); - - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret, "failed to set action client rcl entities"); - } -} - size_t ClientBase::get_number_of_ready_subscriptions() override { - return num_subscriptions_; + return pimpl_->num_subscriptions; } size_t ClientBase::get_number_of_ready_guard_conditions() override { - return num_guard_conditions_; + return pimpl_->num_guard_conditions; } size_t ClientBase::get_number_of_ready_timers() override { - return num_timers_; + return pimpl_->num_timers; } size_t ClientBase::get_number_of_ready_clients() override { - return num_clients_; + return pimpl_->num_clients; } size_t ClientBase::get_number_of_ready_services() override { - return num_services_; + return pimpl_->num_services; } bool ClientBase::add_to_wait_set(rcl_wait_set_t * wait_set) override { rcl_ret_t ret = rcl_action_wait_set_add_action_client( - wait_set, - pimpl_->get_action_client().get()); - - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret, "failed to add action client to wait set"); - } + wait_set, pimpl_->get_action_client().get()); + return (RCL_RET_OK != ret); } bool ClientBase::is_ready(rcl_wait_set_t * wait_set) override { - rcl_ret_t ret = rcl_action_client_wait_set_get_entities_ready( - wait_set, - pimpl_->get_action_client().get(), + wait_set, pimpl_->get_action_client().get(), &is_feedback_ready_, &is_status_ready_, &is_goal_response_ready_, &is_cancel_response_ready_, &is_result_response_ready_); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret, "failed get ready entities in action client"); + rclcpp::exceptions::throw_from_rcl_error( + ret, "failed to check for ready entities"); } - return is_feedback_ready_ || is_status_ready_ || is_goal_response_ready_ || @@ -265,27 +259,63 @@ ClientBase::is_ready(rcl_wait_set_t * wait_set) override void ClientBase::execute() override { - void * message = nullptr; - rmw_request_id_t * header = nullptr; - if(is_feedback_ready_) + std::shared_ptr client_handle = pimpl_->get_client_handle(); + if (pimpl_->is_feedback_ready) { - rcl_action_take_feedback(rcl_action_client, message); + std::shared_ptr feedback_message = this->create_feedback_message(); + rcl_ret_t ret = rcl_action_take_feedback(client_handle.get(), feedback_message.get()); + if (RCL_RET_OK == ret) { + } else { + this->handle_feedback(feedback_message); + is_feedback_ready = false; + } } - if(is_status_ready_) + if (is_status_ready_) { - rcl_action_take_status(rcl_action_client, message); + std::shared_ptr status_message = this->create_status_message(); + rcl_ret_t ret = rcl_action_take_status(client_handle.get(), status_message.get()); + if (RCL_RET_OK == ret) { + is_status_ready = false; + this->handle_status(status_message); + } } - if(is_goal_response_ready_) + if (is_goal_response_ready_) { - rcl_action_take_goal_response(rcl_action_client, header, message); + std::shared_ptr goal_response = this->create_goal_response(); + std::shared_ptr response_header = this->create_response_header(); + rcl_ret_t ret = rcl_action_take_goal_response( + client_handle.get(), response_header.get(), goal_response.get()); + } - if(is_cancel_response_ready_) + if (is_cancel_response_ready_) { - rcl_action_take_cancel_response(rcl_action_client, header, message); + std::shared_ptr cancel_response = this->create_cancel_response(); + std::shared_ptr response_header = this->create_response_header(); + rcl_ret_t ret = rcl_action_take_cancel_response( + client_handle.get(), response_header.get(), cancel_response.get()); + if (RCL_RET_OK == ret) { + is_cancel_response_ready = false; + this->handle_cancel_response(response_header, cancel_response); + } else { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "take goal response failed for client of action '%s': %s", + pimpl_->get_action_name(), rcl_get_error_string().str); + rcl_reset_error(); + } } - if(is_result_response_ready_) + if (is_result_response_ready_) { - rcl_action_take_result_response(rcl_action_client, header, message); + std::shared_ptr result_response = this->create_result_response(); + std::shared_ptr response_header = this->create_response_header(); + rcl_ret_t ret = rcl_action_take_result_response( + client_handle.get(), response_header.get(), result_response.get()); + if (RCL_RET_OK == ret) { + is_result_response_ready = false; + this->handle_result_response(response_header, result_response); + } else { + + } } } From 0c7dfd8b755917278528a72fb81363b4a42e5245 Mon Sep 17 00:00:00 2001 From: Samuel Servulo Date: Thu, 22 Nov 2018 15:06:54 -0200 Subject: [PATCH 09/56] Removed async_cancel from action ClintGoalHandle API --- rclcpp_action/include/rclcpp_action/client_goal_handle.hpp | 3 --- .../include/rclcpp_action/client_goal_handle_impl.hpp | 7 ------- 2 files changed, 10 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp index 3922b9cd2e..c6f5a0e225 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp @@ -34,9 +34,6 @@ class ClientGoalHandle public: virtual ~ClientGoalHandle(); - std::future - async_cancel(); - std::future async_result(); diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp index cd2bf1a626..5879515b61 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -33,13 +33,6 @@ ClientGoalHandle::~ClientGoalHandle() { } -template -std::future -ClientGoalHandle::async_cancel() -{ - throw std::runtime_error("Failed to cancel goal"); -} - template std::future ClientGoalHandle::async_result() From a94ca35321f7a10d9fb54685006ab8685e6f766b Mon Sep 17 00:00:00 2001 From: Samuel Servulo Date: Thu, 22 Nov 2018 15:31:43 -0200 Subject: [PATCH 10/56] Added status handler to action client goal handler --- .../include/rclcpp_action/client_goal_handle.hpp | 6 ++++++ .../include/rclcpp_action/client_goal_handle_impl.hpp | 9 +++++++++ 2 files changed, 15 insertions(+) diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp index c6f5a0e225..e99eb21f85 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp @@ -19,6 +19,7 @@ #include #include +#include #include "rclcpp_action/visibility_control.hpp" @@ -43,9 +44,14 @@ class ClientGoalHandle ClientGoalHandle(rcl_action_client_t * rcl_client, const rcl_action_goal_info_t rcl_info); + void handle_status(rcl_action_goal_status_t status); + // TODO(sloretz) shared pointer to keep rcl_client_ alive while goal handles are alive rcl_action_client_t * rcl_client_; rcl_action_goal_info_t rcl_info_; + rcl_action_goal_status_t rcl_status_; + + std::mutex handler_mutex; }; } // namespace rclcpp_action diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp index 5879515b61..c08604bd11 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -17,6 +17,8 @@ #include +#include + namespace rclcpp_action { template @@ -39,6 +41,13 @@ ClientGoalHandle::async_result() { throw std::runtime_error("Failed to get result future"); } + +template +void ClientGoalHandle::handle_status(rcl_action_goal_status_t status) +{ + std::lock_guard guard(handler_mutex); + status_ = status; +} } // namespace rclcpp_action #endif // RCLCPP_ACTION__CLIENT_GOAL_HANDLE_IMPL_HPP_ From 7fbb2536e38e23385b723ef11c5dacedb14f621c Mon Sep 17 00:00:00 2001 From: Samuel Servulo Date: Thu, 22 Nov 2018 16:01:54 -0200 Subject: [PATCH 11/56] Added result handler to action client goal handler --- .../include/rclcpp_action/client_goal_handle.hpp | 7 +++++++ .../include/rclcpp_action/client_goal_handle_impl.hpp | 11 +++++++++-- 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp index e99eb21f85..f54075e695 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp @@ -46,11 +46,18 @@ class ClientGoalHandle void handle_status(rcl_action_goal_status_t status); + void handle_result(typename ACTION::Result result); + void handle_feedback(); + + void set_feedback_callback(); + // TODO(sloretz) shared pointer to keep rcl_client_ alive while goal handles are alive rcl_action_client_t * rcl_client_; rcl_action_goal_info_t rcl_info_; rcl_action_goal_status_t rcl_status_; + std::promise result_; + std::mutex handler_mutex; }; } // namespace rclcpp_action diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp index c08604bd11..4227043a0e 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -39,14 +39,21 @@ template std::future ClientGoalHandle::async_result() { - throw std::runtime_error("Failed to get result future"); + return result_.get_future(); } template void ClientGoalHandle::handle_status(rcl_action_goal_status_t status) { std::lock_guard guard(handler_mutex); - status_ = status; + rcl_status_ = status; +} + +template +void ClientGoalHandle::handle_result(typename ACTION::Result result) +{ + std::lock_guard guard(handler_mutex); + result_.set_value(result); } } // namespace rclcpp_action From 1a1f79a7e55105cfb3d555d261c4fb0e06ab87c5 Mon Sep 17 00:00:00 2001 From: Samuel Servulo Date: Thu, 22 Nov 2018 16:03:47 -0200 Subject: [PATCH 12/56] Identation fix --- .../include/rclcpp_action/client_goal_handle_impl.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp index 4227043a0e..3730f5a528 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -45,14 +45,14 @@ ClientGoalHandle::async_result() template void ClientGoalHandle::handle_status(rcl_action_goal_status_t status) { - std::lock_guard guard(handler_mutex); + std::lock_guard guard(handler_mutex); rcl_status_ = status; } template void ClientGoalHandle::handle_result(typename ACTION::Result result) { - std::lock_guard guard(handler_mutex); + std::lock_guard guard(handler_mutex); result_.set_value(result); } } // namespace rclcpp_action From 17a7a8d51df5506777912546d274e739b69efa00 Mon Sep 17 00:00:00 2001 From: Samuel Servulo Date: Thu, 22 Nov 2018 17:23:15 -0200 Subject: [PATCH 13/56] Added get/set for action client goal handler --- .../rclcpp_action/client_goal_handle.hpp | 22 +++++++++---- .../rclcpp_action/client_goal_handle_impl.hpp | 33 +++++++++++++++---- 2 files changed, 43 insertions(+), 12 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp index f54075e695..fa9f66d9bc 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp @@ -38,18 +38,26 @@ class ClientGoalHandle std::future async_result(); + std::function + get_feedback_callback(); + + void + set_feedback_callback(std::function callback); + + rcl_action_goal_status_t + get_status(); + private: // The templated Server creates goal handles friend Client; ClientGoalHandle(rcl_action_client_t * rcl_client, const rcl_action_goal_info_t rcl_info); - void handle_status(rcl_action_goal_status_t status); + void + handle_status(rcl_action_goal_status_t status); - void handle_result(typename ACTION::Result result); - void handle_feedback(); - - void set_feedback_callback(); + void + handle_result(typename ACTION::Result result); // TODO(sloretz) shared pointer to keep rcl_client_ alive while goal handles are alive rcl_action_client_t * rcl_client_; @@ -58,7 +66,9 @@ class ClientGoalHandle std::promise result_; - std::mutex handler_mutex; + std::function feedback_callback_; + + std::mutex handler_mutex_; }; } // namespace rclcpp_action diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp index 3730f5a528..8e9c1ca2c6 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -17,8 +17,6 @@ #include -#include - namespace rclcpp_action { template @@ -43,16 +41,39 @@ ClientGoalHandle::async_result() } template -void ClientGoalHandle::handle_status(rcl_action_goal_status_t status) +std::function +ClientGoalHandle::get_feedback_callback() +{ + return feedback_callback_; +} + +template +void +ClientGoalHandle::set_feedback_callback(std::function callback) +{ + feedback_callback_ = callback; +} + +template +rcl_action_goal_status_t +ClientGoalHandle::get_status() +{ + return rcl_status_; +} + +template +void +ClientGoalHandle::handle_status(rcl_action_goal_status_t status) { - std::lock_guard guard(handler_mutex); + std::lock_guard guard(handler_mutex_); rcl_status_ = status; } template -void ClientGoalHandle::handle_result(typename ACTION::Result result) +void +ClientGoalHandle::handle_result(typename ACTION::Result result) { - std::lock_guard guard(handler_mutex); + std::lock_guard guard(handler_mutex_); result_.set_value(result); } } // namespace rclcpp_action From bcf15537b816db69ebfc0b2c152bee52f5b1ff35 Mon Sep 17 00:00:00 2001 From: Samuel Servulo Date: Thu, 22 Nov 2018 18:27:00 -0200 Subject: [PATCH 14/56] Changed action client goal handler attrs from rcl to cpp versions --- .../rclcpp_action/client_goal_handle.hpp | 20 ++++++++++--------- .../rclcpp_action/client_goal_handle_impl.hpp | 15 +++++++------- 2 files changed, 19 insertions(+), 16 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp index fa9f66d9bc..ca07a714a3 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp @@ -21,6 +21,8 @@ #include #include +#include "action_msgs/msg/goal_status.hpp" +#include "action_msgs/msg/goal_info.hpp" #include "rclcpp_action/visibility_control.hpp" namespace rclcpp_action @@ -41,29 +43,29 @@ class ClientGoalHandle std::function get_feedback_callback(); - void - set_feedback_callback(std::function callback); - - rcl_action_goal_status_t + action_msgs::msg::GoalStatus get_status(); private: // The templated Server creates goal handles friend Client; - ClientGoalHandle(rcl_action_client_t * rcl_client, const rcl_action_goal_info_t rcl_info); + ClientGoalHandle(rcl_action_client_t * rcl_client, const action_msgs::msg::GoalInfo info); + + void + set_feedback_callback(std::function callback); void - handle_status(rcl_action_goal_status_t status); + set_status(action_msgs::msg::GoalStatus status); void - handle_result(typename ACTION::Result result); + set_result(typename ACTION::Result result); // TODO(sloretz) shared pointer to keep rcl_client_ alive while goal handles are alive rcl_action_client_t * rcl_client_; - rcl_action_goal_info_t rcl_info_; - rcl_action_goal_status_t rcl_status_; + action_msgs::msg::GoalInfo info_; + action_msgs::msg::GoalStatus status_; std::promise result_; std::function feedback_callback_; diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp index 8e9c1ca2c6..d41b92ffb2 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -22,9 +22,9 @@ namespace rclcpp_action template ClientGoalHandle::ClientGoalHandle( rcl_action_client_t * rcl_client, - const rcl_action_goal_info_t rcl_info + const action_msgs::msg::GoalInfo info ) -: rcl_client_(rcl_client), rcl_info_(rcl_info) +: rcl_client_(rcl_client), info_(info) { } @@ -51,27 +51,28 @@ template void ClientGoalHandle::set_feedback_callback(std::function callback) { + std::lock_guard guard(handler_mutex_); feedback_callback_ = callback; } template -rcl_action_goal_status_t +action_msgs::msg::GoalStatus ClientGoalHandle::get_status() { - return rcl_status_; + return status_; } template void -ClientGoalHandle::handle_status(rcl_action_goal_status_t status) +ClientGoalHandle::set_status(action_msgs::msg::GoalStatus status) { std::lock_guard guard(handler_mutex_); - rcl_status_ = status; + status_ = status; } template void -ClientGoalHandle::handle_result(typename ACTION::Result result) +ClientGoalHandle::set_result(typename ACTION::Result result) { std::lock_guard guard(handler_mutex_); result_.set_value(result); From 5b2bc4ce7b94be3cc78fbf7306790aeed925ccea Mon Sep 17 00:00:00 2001 From: Samuel Servulo Date: Thu, 22 Nov 2018 18:52:08 -0200 Subject: [PATCH 15/56] Added check methods to action client goal handler --- .../rclcpp_action/client_goal_handle.hpp | 21 +++++++- .../rclcpp_action/client_goal_handle_impl.hpp | 48 +++++++++++++++++-- 2 files changed, 65 insertions(+), 4 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp index ca07a714a3..d027ad5c87 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp @@ -46,6 +46,21 @@ class ClientGoalHandle action_msgs::msg::GoalStatus get_status(); + bool + is_feedback_aware(); + + bool + is_result_aware(); + + void + set_result_awareness(bool awareness); + + bool + is_valid(); + + void + invalidate(); + private: // The templated Server creates goal handles friend Client; @@ -66,10 +81,14 @@ class ClientGoalHandle action_msgs::msg::GoalInfo info_; action_msgs::msg::GoalStatus status_; - std::promise result_; std::function feedback_callback_; + bool is_result_aware_; + std::promise result_; + + bool is_handler_valid_; + std::mutex handler_mutex_; }; } // namespace rclcpp_action diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp index d41b92ffb2..cf006ddfdb 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -24,7 +24,11 @@ ClientGoalHandle::ClientGoalHandle( rcl_action_client_t * rcl_client, const action_msgs::msg::GoalInfo info ) -: rcl_client_(rcl_client), info_(info) +: rcl_client_(rcl_client), + info_(info), + feedback_callback_(nullptr), + is_result_aware_(false), + is_handler_valid_(true) { } @@ -40,6 +44,14 @@ ClientGoalHandle::async_result() return result_.get_future(); } +template +void +ClientGoalHandle::set_result(typename ACTION::Result result) +{ + std::lock_guard guard(handler_mutex_); + result_.set_value(result); +} + template std::function ClientGoalHandle::get_feedback_callback() @@ -70,13 +82,43 @@ ClientGoalHandle::set_status(action_msgs::msg::GoalStatus status) status_ = status; } +template +bool +ClientGoalHandle::is_feedback_aware() +{ + return feedback_callback_; +} + +template +bool +ClientGoalHandle::is_result_aware() +{ + return is_result_aware_; +} + template void -ClientGoalHandle::set_result(typename ACTION::Result result) +ClientGoalHandle::set_result_awareness(bool awareness) { std::lock_guard guard(handler_mutex_); - result_.set_value(result); + is_result_aware_ = awareness; +} + +template +bool +ClientGoalHandle::is_valid() +{ + return is_handler_valid_; } + +template +bool +ClientGoalHandle::invalidate() +{ + std::lock_guard guard(handler_mutex_); + is_handler_valid_ = false; +} + } // namespace rclcpp_action #endif // RCLCPP_ACTION__CLIENT_GOAL_HANDLE_IMPL_HPP_ From 6b3482f4b475380c12cbad118b3599d0c084e7eb Mon Sep 17 00:00:00 2001 From: Samuel Servulo Date: Thu, 22 Nov 2018 19:18:14 -0200 Subject: [PATCH 16/56] Removed rcl_client pointer from action client goal handler --- .../include/rclcpp_action/client_goal_handle.hpp | 5 +---- .../include/rclcpp_action/client_goal_handle_impl.hpp | 8 ++------ 2 files changed, 3 insertions(+), 10 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp index d027ad5c87..c6ea290103 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp @@ -65,7 +65,7 @@ class ClientGoalHandle // The templated Server creates goal handles friend Client; - ClientGoalHandle(rcl_action_client_t * rcl_client, const action_msgs::msg::GoalInfo info); + ClientGoalHandle(const action_msgs::msg::GoalInfo info); void set_feedback_callback(std::function callback); @@ -76,9 +76,6 @@ class ClientGoalHandle void set_result(typename ACTION::Result result); - // TODO(sloretz) shared pointer to keep rcl_client_ alive while goal handles are alive - rcl_action_client_t * rcl_client_; - action_msgs::msg::GoalInfo info_; action_msgs::msg::GoalStatus status_; diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp index cf006ddfdb..39e25136bb 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -20,12 +20,8 @@ namespace rclcpp_action { template -ClientGoalHandle::ClientGoalHandle( - rcl_action_client_t * rcl_client, - const action_msgs::msg::GoalInfo info -) -: rcl_client_(rcl_client), - info_(info), +ClientGoalHandle::ClientGoalHandle(const action_msgs::msg::GoalInfo info) +: info_(info), feedback_callback_(nullptr), is_result_aware_(false), is_handler_valid_(true) From 5967df8cdae4d3d940a3602d6961c203d357ddfc Mon Sep 17 00:00:00 2001 From: Samuel Servulo Date: Tue, 27 Nov 2018 12:55:37 -0200 Subject: [PATCH 17/56] Added trailing suffix to client pimpl attrs --- rclcpp_action/src/client.cpp | 22 ++++++++++++---------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index bc34e0cebc..fb4547e90e 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -153,12 +153,14 @@ class ClientBaseImpl return client_handle_; } - // rcl interface entities + // rcl interface entities site_t num_subscriptions_, - num_guard_conditions_, - num_timers_, - num_clients_, - num_services_; + num_guard_conditions_, + num_timers_, + num_clients_, + num_services_; + + // rcl interface flags bool is_feedback_ready_, is_status_ready_, is_goal_response_ready_, @@ -200,31 +202,31 @@ ClientBase::~ClientBase() size_t ClientBase::get_number_of_ready_subscriptions() override { - return pimpl_->num_subscriptions; + return pimpl_->num_subscriptions_; } size_t ClientBase::get_number_of_ready_guard_conditions() override { - return pimpl_->num_guard_conditions; + return pimpl_->num_guard_conditions_; } size_t ClientBase::get_number_of_ready_timers() override { - return pimpl_->num_timers; + return pimpl_->num_timers_; } size_t ClientBase::get_number_of_ready_clients() override { - return pimpl_->num_clients; + return pimpl_->num_clients_; } size_t ClientBase::get_number_of_ready_services() override { - return pimpl_->num_services; + return pimpl_->num_services_; } bool From 58215f39f6e72e287ea0a83c91581ac22c0d30f0 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 27 Nov 2018 16:08:32 -0800 Subject: [PATCH 18/56] Towards a consistent action client --- .../include/rclcpp_action/client.hpp | 362 ++++++++------- .../rclcpp_action/client_goal_handle.hpp | 51 ++- .../rclcpp_action/client_goal_handle_impl.hpp | 54 +-- .../include/rclcpp_action/exceptions.hpp | 8 +- rclcpp_action/include/rclcpp_action/types.hpp | 6 +- rclcpp_action/src/client.cpp | 429 ++++++++++-------- 6 files changed, 516 insertions(+), 394 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 23919d61fe..80bda55bc8 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -50,47 +50,84 @@ class ClientBase : public Waitable RCLCPP_ACTION_PUBLIC virtual ~ClientBase(); + RCLCPP_ACTION_PUBLIC + size_t + get_number_of_ready_subscriptions() override; + + RCLCPP_PUBLIC + size_t + get_number_of_ready_timers() override; + + RCLCPP_PUBLIC + size_t + get_number_of_ready_clients() override; + + RCLCPP_PUBLIC + size_t + get_number_of_ready_services() override; + + RCLCPP_PUBLIC + size_t + get_number_of_ready_guard_conditions() override; + + RCLCPP_ACTION_PUBLIC + bool + add_to_wait_set(rcl_wait_set_t * wait_set) override; + + RCLCPP_ACTION_PUBLIC + bool + is_ready(rcl_wait_set_t * wait_set) override; + + RCLCPP_ACTION_PUBLIC + void + execute() override; + protected: - RCLCPP_ACTION_PUBLIC - void send_goal_request(std::shared_ptr request, + RCLCPP_ACTION_PUBLIC + Logger get_logger(); + + RCLCPP_ACTION_PUBLIC + virtual GoalID generate_goal_id(); + + RCLCPP_ACTION_PUBLIC + virtual void send_goal_request( + std::shared_ptr request, ResponseCallback callback); RCLCPP_ACTION_PUBLIC - void send_result_request( + virtual void send_result_request( std::shared_ptr request, ResponseCallback callback); - RCLCPP_ACTION_PUBLIC - void send_cancel_request( + RCLCPP_ACTION_PUBLIC + virtual void send_cancel_request( std::shared_ptr request, ResponseCallback callback); private: - std::shared_ptr create_response_header(); - - virtual std::shared_ptr create_goal_response() = 0; + virtual std::shared_ptr create_goal_response() const = 0; - void handle_goal_response( - std::shared_ptr response_header, - std::shared_ptr response); + virtual void handle_goal_response( + const rmw_request_id_t & response_header, + std::shared_ptr goal_response); - virtual std::shared_ptr create_result_response() = 0; + virtual std::shared_ptr create_result_response() const = 0; - void handle_result_response( - std::shared_ptr response_header, - std::shared_ptr response); + virtual void handle_result_response( + const rmw_request_id_t & response_header, + std::shared_ptr result_response); - virtual std::shared_ptr create_cancel_response() = 0; + virtual std::shared_ptr create_cancel_response() const = 0; - void handle_cancel_response( - std::shared_ptr request_header, - std::shared_ptr response); + virtual void handle_cancel_response( + const rmw_request_id_t & response_header, + std::shared_ptr cancel_response); - virtual std::shared_ptr create_feedback_message() = 0; + virtual std::shared_ptr create_feedback_message() const = 0; virtual void handle_feedback_message(std::shared_ptr message) = 0; - virtual std::shared_ptr create_status_message() = 0; + virtual std::shared_ptr create_status_message() const = 0; virtual void handle_status_message(std::shared_ptr message) = 0; @@ -109,12 +146,13 @@ class Client : public ClientBase using Goal = typename ACTION::Goal; using Result = typename ACTION::Result; + using Feedback = typename ACTION::Feedback; using GoalHandle = ClientGoalHandle; using FeedbackCallback = std::function; + GoalHandle::SharedPtr, const Feedback &)>; using CancelRequest = typename ACTION::CancelGoalService::Request; using CancelResponse = typename ACTION::CancelGoalService::Response; - + Client( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, const std::string & action_name, const rcl_action_client_options_t & client_options @@ -126,133 +164,55 @@ class Client : public ClientBase { } - std::shared_ptr create_goal_response() override - { - using GoalResponse = typename ACTION::GoalService::Response; - return std::shared_ptr(new GoalResponse()); - } - - std::shared_ptr create_result_response() override - { - using GoalResultResponse = typename ACTION::GoalResultService::Response; - return std::shared_ptr(new GoalResultResponse()); - } - - std::shared_ptr create_feedback_message() override - { - using Feedback = typename ACTION::Feedback; - return std::shared_ptr(new Feedback()); - } - - RCLCPP_ACTION_PUBLIC - void handle_feedback_message(std::shared_ptr message) override - { - using Feedback = typename ACTION::Feedback; - Feedback::SharedPtr feedback = std::static_pointer_cast(message); - std::unique_lock lock(goal_handles_mutex_); - if (goal_handles_.count(feedback->goal_id) == 0) { - RCUTILS_LOG_DEBUG_NAMED( - "rclcpp", "Received feedback for unknown goal. Ignoring..."); - return; - } - GoalHandle::SharedPtr goal_handle = goal_handles_[feedback->goal_id]; - if (!goal_handle->is_feedback_aware()) { - RCUTILS_LOG_DEBUG_NAMED( - "rclcpp", "Received feedback for goal %s, but it ignores feedback."); - return; - } - const FeedbackCallback & callback = goal_handle->get_feedback_callback(); - callback(goal_handle, *feedback); - } - - RCLCPP_ACTION_PUBLIC - std::shared_ptr create_status_message() override - { - using GoalStatusArray = typename ACTION::GoalStatusArray; - return std::shared_ptr(new GoalStatusArray()); - } - - RCLCPP_ACTION_PUBLIC - void handle_status_message(std::shared_ptr message) override - { - using GoalStatusArray = typename ACTION::GoalStatusArray; - GoalStatusArray::SharedPtr status_array = - std::static_pointer_cast(message); - std::lock_guard lock(goal_handles_mutex_); - for (const GoalStatus & status : status_array.status_list) - { - const GoalID & goal_id = status.goal_info.goal_id; - if (goal_handles_.count(goal_id) != 0) - { - goal_handles_[goal_id]->set_status(status); - if (status.status == STATUS_CANCELED || status.status == STATUS_ABORTED) - { - goal_handles_.erase(goal_id); - } - } - } - } - RCLCPP_ACTION_PUBLIC std::shared_future async_send_goal( - Goal::SharedPtr goal, FeedbackCallback callback = nullptr, bool ignore_result = false) + const Goal & goal, FeedbackCallback callback = nullptr, bool ignore_result = false) { - - if (pending_goal_handles_.count(goal.goal_id) != 0) { - - } - if (goal_handles_.count(goal.goal_id) != 0) { - - } + using GoalRequest = typename ACTION::GoalRequestService::Request; + auto goal_request = std::make_shared(); + goal_request->goal_id = this->generate_goal_id(); + goal_request->goal = goal; std::promise promise; std::shared_future future(promise.get_future()); this->send_goal_request( - std::static_pointer_cast(goal), - [this, promise{std::move(promise)}, callback] (std::shared_ptr response) + std::static_pointer_cast(goal_request), + [this, goal_request, promise{std::move(promise)}, callback] (std::shared_ptr response) { using GoalResponse = typename ACTION::GoalService::Response; - GoalResponse::SharedPtr goal_response = - std::static_pointer_cast(response); - if (!goal_response.accepted) { - promise->set_exception( - std::make_exception_ptr(RejectedGoalError(""))); + GoalResponse::SharedPtr goal_response = std::static_pointer_cast(response); + if (!goal_response->accepted) { + promise->set_exception(std::make_exception_ptr( + exceptions::RejectedGoalError(goal_request->goal))); + return; } - auto goal_handle = std::make_shared(goal_, callback); + std::lock_guard lock(goal_handles_mutex_); + GoalInfo goal_info; + goal_info.goal_id = goal_request->goal_id; + goal_info.stamp = goal_response->stamp; + auto goal_handle = std::make_shared(goal_info, callback); if (!ignore_result) { try { - this->async_get_result(goal_handle); + this->make_result_aware(goal_handle); } catch (...) { promise->set_exception(std::current_exception()); return; } } - goal_handles_[goal_id] = goal_handles; - pending_goal_handles_.erase(goal_id); + this->goal_handles_[goal_handle->goal_id()] = goal_handle; promise->set_value(goal_handle); }); return future; } RCLCPP_ACTION_PUBLIC - std::shared_future - async_get_result_response(GoalHandle::SharedPtr goal_handle) { + std::shared_future + async_get_result(GoalHandle::SharedPtr goal_handle) { std::lock_guard lock(goal_handles_mutex_); - const GoalID & goal_id = goal_handle->goal_id(); - if (goal_handles_.count(goal_id) == 0) { - throw; + if (goal_handles_.count(goal_handle->goal_id()) == 0) { + throw exceptions::UnknownGoalHandleError(goal_handle); } - if (!goal_handle->tracks_result()) { - using ResultRequest = typename ACTION::GoalResultService::Request; - auto result_request = std::make_shared(); - result_request.goal_id = goal_handle->goal_id(); - this->send_result_request( - std::static_pointer_cast(result_request), - [goal_handle] (std::shared_ptr response) { - Result::SharedPtr result = - std::static_pointer_cast(response); - goal_handle->set_result(result); - }); - goal_handle->set_result_awareness(true); + if (!goal_handle->is_result_aware()) { + this->make_result_aware(goal_handle); } return goal_handle->async_result(); } @@ -261,25 +221,24 @@ class Client : public ClientBase std::shared_future async_cancel_goal(GoalHandle::SharedPtr goal_handle) { std::lock_guard lock(goal_handles_mutex_); - const GoalID & goal_id = goal_handle->goal_id(); - if (goal_handles_.count(goal_id) == 0) { - throw; + if (goal_handles_.count(goal_handle->goal_id()) == 0) { + throw exceptions::UnknownGoalHandleError(goal_handle); } std::promise promise; std::shared_future future(promise.get_future()); - auto cancel_request = std::make_shared(); - cancel_request->goal_info.goal_id = goal_id; + using CancelGoalRequest = typename ACTION::CancelGoalService::Request; + auto cancel_goal_request = std::make_shared(); + cancel_goal_request->goal_info.goal_id = goal_handle->goal_id(); this->send_cancel_request( - std::static_pointer_cast(cancel_request), - [this, goal_handle, promise{std::move(promise)}] (std::shared_ptr response) { - CancelGoalResponse::SharedPtr cancel_response = - std::static_pointer_cast(response); + std::static_pointer_cast(cancel_goal_request), + [goal_handle, promise{std::move(promise)}] (std::shared_ptr response) + { + CancelResponse::SharedPtr cancel_response = + std::static_pointer_cast(response); bool goal_canceled = false; if (!cancel_response->goals_canceling.empty()) { const GoalInfo & canceled_goal_info = cancel_response->goals_canceling[0]; - const GoalID & canceled_goal_id = canceled_goal_info.goal_id; - const GoalID & to_be_canceled_goal_id = goal_handle->goal_id(); - if (!uuidcmp(canceled_goal_id.uuid, to_be_canceled_goal_id.uuid)) { + if (canceled_goal_info.goal_id == goal_handle->goal_id()) { goal_canceled = canceled_goal_info.accepted; } } @@ -290,41 +249,134 @@ class Client : public ClientBase RCLCPP_ACTION_PUBLIC std::shared_future - async_cancel_all_goals() { - auto cancel_request = std::make_shared(); - cancel_request->goal_info.goal_id.uuid = zerouuid; - return async_cancel(cancel_request); + async_cancel_all_goals() + { + auto cancel_request = std::make_shared(); + std::fill(cancel_request->goal_info.goal_id.uuid, 0u); + return async_cancel(cancel_goal_request); } RCLCPP_ACTION_PUBLIC std::shared_future - async_cancel_goals_before(const rclcpp::Time & stamp) { - auto cancel_request = std::make_shared(); - cancel_request->goal_info.goal_id.uuid = zerouuid; + async_cancel_goals_before(const rclcpp::Time & stamp) + { + auto cancel_request = std::make_shared(); + std::fill(cancel_request->goal_info.goal_id.uuid, 0u); cancel_request->goal_info.stamp = stamp; return async_cancel(cancel_request); } virtual ~Client() { - std::unique_lock lock(goal_handles_mutex_); + std::lock_guard lock(goal_handles_mutex_); auto it = goal_handles_.begin(); while (it != it->goal_handles_.end()) { - it->second->set_invalid(); + it->second->invalidate(); it = goal_handles_.erase(it); } } - private: - std::shared_future - async_cancel(CancelGoalRequest::SharedPtr cancel_request) { - std::promise promise; - std::shared_future future(promise.get_future()); +private: + std::shared_ptr create_goal_response() override + { + using GoalResponse = typename ACTION::GoalRequestService::Response; + return std::shared_ptr(new GoalResponse()); + } + + std::shared_ptr create_result_response() override + { + using GoalResultResponse = typename ACTION::GoalResultService::Response; + return std::shared_ptr(new GoalResultResponse()); + } + + std::shared_ptr create_feedback_message() override + { + using FeedbackMessage = typename ACTION::FeedbackMessage; + return std::shared_ptr(new FeedbackMessage()); + } + + void handle_feedback_message(std::shared_ptr message) override + { + using FeedbackMessage = typename ACTION::FeedbackMessage; + FeedbackMessage::SharedPtr feedback_message = + std::static_pointer_cast(message); + std::unique_lock lock(goal_handles_mutex_); + const GoalID & goal_id = feedback_message->goal_id; + if (goal_handles_.count(goal_id) == 0) { + RCLCPP_DEBUG( + this->get_logger(), + "Received feedback for unknown goal. Ignoring..."); + return; + } + GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id]; + if (!goal_handle->is_feedback_aware()) { + RCLCPP_DEBUG( + this->get_logger(), + "Received feedback for goal but it ignores it."); + return; + } + const FeedbackCallback & callback = goal_handle->get_feedback_callback(); + callback(goal_handle, feedback_message->feedback); + } + + std::shared_ptr create_status_message() override + { + using GoalStatusMessage = typename ACTION::GoalStatusMessage; + return std::shared_ptr(new GoalStatusMessage()); + } + + void handle_status_message(std::shared_ptr message) override + { + std::lock_guard lock(goal_handles_mutex_); + using GoalStatusMessage = typename ACTION::GoalStatusMessage; + GoalStatusMessage::SharedPtr status_message = + std::static_pointer_cast(message); + for (const GoalStatus & status : status_message.status_list) { + const GoalID & goal_id = status.goal_info.goal_id; + if (goal_handles_.count(goal_id) == 0) { + RCLCPP_DEBUG( + this->get_logger(), + "Received status for unknown goal. Ignoring..."); + continue; + } + GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id]; + goal_handle->set_status(status); + if ( + goal_handle->get_status() == STATUS_CANCELED || + goal_handle->get_status() == STATUS_ABORTED + ) { + goal_handles_.erase(goal_id); + } + } + } + + void make_result_aware(GoalHandle::SharedPtr goal_handle) { + using GoalResultRequest = typename ACTION::GoalResultService::Request; + auto goal_result_request = std::make_shared(); + goal_result_request.goal_id = goal_handle->goal_id(); + this->send_result_request( + std::static_pointer_cast(goal_result_request), + [goal_handle] (std::shared_ptr response) + { + using GoalResultResponse = typename ACTION::GoalResultService::Response; + GoalResultRequest::SharedPtr goal_result_response = + std::static_pointer_cast(response); + goal_handle->set_result(goal_result_response); + }); + goal_handle->set_result_awareness(true); + } + + std::shared_future + async_cancel(CancelRequest::SharedPtr cancel_request) + { + std::promise promise; + std::shared_future future(promise.get_future()); this->send_cancel_request( std::static_pointer_cast(cancel_request), - [promise{std::move(promise)}] (std::shared_ptr response) { - CancelGoalResponse::SharedPtr cancel_response = - std::static_pointer_cast(response); + [promise{std::move(promise)}] (std::shared_ptr response) + { + CancelResponse::SharedPtr cancel_response = + std::static_pointer_cast(response); promise.set_value(cancel_response); }); return future; diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp index c6ea290103..8753667890 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp @@ -35,15 +35,22 @@ template class ClientGoalHandle { public: + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ClientGoalHandle) + + using Result = typename ACTION::Result; + using Feedback = typename ACTION::Feedback; + using FeedbackCallback = std::function; + virtual ~ClientGoalHandle(); - std::future + std::shared_future async_result(); - std::function + FeedbackCallback get_feedback_callback(); - action_msgs::msg::GoalStatus + int8_t get_status(); bool @@ -52,41 +59,35 @@ class ClientGoalHandle bool is_result_aware(); - void - set_result_awareness(bool awareness); - - bool - is_valid(); - - void - invalidate(); - private: // The templated Server creates goal handles friend Client; - ClientGoalHandle(const action_msgs::msg::GoalInfo info); + ClientGoalHandle(const GoalInfo & info, FeedbackCallback callback); void - set_feedback_callback(std::function callback); + set_feedback_callback(FeedbackCallback callback); void - set_status(action_msgs::msg::GoalStatus status); + set_result_awareness(bool awareness); void - set_result(typename ACTION::Result result); - - action_msgs::msg::GoalInfo info_; - action_msgs::msg::GoalStatus status_; + set_status(int8_t status); - std::function feedback_callback_; - - bool is_result_aware_; - std::promise result_; + void + set_result(const Result & result); + + void + invalidate(); - bool is_handler_valid_; + GoalInfo info_; + bool is_result_aware_{false}; + std::promise result_promise_; + std::shared_future result_future_; + FeedbackCallback feedback_callback_{nullptr}; + int8_t status_{GoalStatus::STATE_ACCEPTED}; - std::mutex handler_mutex_; + std::mutex handle_mutex_; }; } // namespace rclcpp_action diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp index 39e25136bb..fca8a84e54 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -20,11 +20,9 @@ namespace rclcpp_action { template -ClientGoalHandle::ClientGoalHandle(const action_msgs::msg::GoalInfo info) -: info_(info), - feedback_callback_(nullptr), - is_result_aware_(false), - is_handler_valid_(true) +ClientGoalHandle::ClientGoalHandle( + const GoalInfo & info, FeedbackCallback callback) +: info_(info), feedback_callback_(callback), { } @@ -34,47 +32,54 @@ ClientGoalHandle::~ClientGoalHandle() } template -std::future +std::shared_future::Result> ClientGoalHandle::async_result() { - return result_.get_future(); + std::lock_guard guard(handle_mutex_); + if (!is_result_aware_) { + throw exceptions::UnawareGoalHandleError(); + } + return result_future_; } template void -ClientGoalHandle::set_result(typename ACTION::Result result) +ClientGoalHandle::set_result(const Result & result) { - std::lock_guard guard(handler_mutex_); - result_.set_value(result); + std::lock_guard guard(handle_mutex_); + status_ = result.status; + promise_result_.set_value(result); } template -std::function +typename ClientGoalHandle::FeedbackCallback ClientGoalHandle::get_feedback_callback() { + std::lock_guard guard(handle_mutex_); return feedback_callback_; } template void -ClientGoalHandle::set_feedback_callback(std::function callback) +ClientGoalHandle::set_feedback_callback(FeedbackCallback callback) { - std::lock_guard guard(handler_mutex_); + std::lock_guard guard(handle_mutex_); feedback_callback_ = callback; } template -action_msgs::msg::GoalStatus +int8_t ClientGoalHandle::get_status() { + std::lock_guard guard(handle_mutex_); return status_; } template void -ClientGoalHandle::set_status(action_msgs::msg::GoalStatus status) +ClientGoalHandle::set_status(int8_t status) { - std::lock_guard guard(handler_mutex_); + std::lock_guard guard(handle_mutex_); status_ = status; } @@ -82,6 +87,7 @@ template bool ClientGoalHandle::is_feedback_aware() { + std::lock_guard guard(handle_mutex_); return feedback_callback_; } @@ -89,6 +95,7 @@ template bool ClientGoalHandle::is_result_aware() { + std::lock_guard guard(handle_mutex_); return is_result_aware_; } @@ -96,23 +103,18 @@ template void ClientGoalHandle::set_result_awareness(bool awareness) { - std::lock_guard guard(handler_mutex_); + std::lock_guard guard(handle_mutex_); is_result_aware_ = awareness; } -template -bool -ClientGoalHandle::is_valid() -{ - return is_handler_valid_; -} - template bool ClientGoalHandle::invalidate() { - std::lock_guard guard(handler_mutex_); - is_handler_valid_ = false; + std::lock_guard guard(handle_mutex_); + status_ = GoalStatus::STATE_UNKNOWN; + result_promise_.set_exception(std::make_exception_ptr( + exceptions::UnawareGoalHandleError())); } } // namespace rclcpp_action diff --git a/rclcpp_action/include/rclcpp_action/exceptions.hpp b/rclcpp_action/include/rclcpp_action/exceptions.hpp index ffdf125e65..5adc9ea7d5 100644 --- a/rclcpp_action/include/rclcpp_action/exceptions.hpp +++ b/rclcpp_action/include/rclcpp_action/exceptions.hpp @@ -24,8 +24,12 @@ namespace exceptions class InvalidGoalHandle : public std::invalid_argument { - InvalidGoalHandle() - : std::invalid_argument("") {} + InvalidGoalHandle() = default; +}; + +class UnawareGoalHandle : public std::runtime_error +{ + UnawareGoalHandle() = default; }; } // namespace exceptions diff --git a/rclcpp_action/include/rclcpp_action/types.hpp b/rclcpp_action/include/rclcpp_action/types.hpp index a7a6650ebd..4a014792f0 100644 --- a/rclcpp_action/include/rclcpp_action/types.hpp +++ b/rclcpp_action/include/rclcpp_action/types.hpp @@ -17,9 +17,12 @@ #include +#include + namespace rclcpp_action { using GoalID = unique_identifier_msgs::msg::UUID; +using GoalStatus = action_msgs::msg::GoalStatus; } // namespace @@ -30,11 +33,10 @@ struct less { bool operator()( const rclcpp_action::GoalID & id0, const rclcpp_action::GoalID & id1) { - return !uuidcmp(id0.uuid, id1.uuid); + return id0.uuid < id1.uuid; } }; } // namespace std - #endif // RCLCPP_ACTION__TYPES_HPP_ diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index fb4547e90e..ff37f9206a 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -18,11 +18,9 @@ #include - -using rclcpp_action::ClientBase; - namespace rclcpp_action { + class ClientBaseImpl { public: @@ -31,9 +29,12 @@ class ClientBaseImpl const std::string & action_name, const rosidl_action_type_support_t * type_support, const rcl_action_client_options_t & client_options) + : node_handle(node_base.get_shared_rcl_node_handle()), + logger(rclcpp::get_logger(rcl_node_get_logger_name( + node_handle.get())).get_child("rclcpp")) { - std::weak_ptr weak_node_handle(node_handle_); - client_handle_ = std::shared_ptr( + std::weak_ptr weak_node_handle(node_handle); + client_handle = std::shared_ptr( new rcl_client_t, [weak_node_handle](rcl_action_client_t * client) { auto handle = weak_node_handle.lock(); @@ -41,140 +42,67 @@ class ClientBaseImpl if (RCL_RET_OK != rcl_action_client_fini(client, handle.get())) { RCLCPP_ERROR( rclcpp::get_logger(rcl_node_get_logger_name(handle.get())).get_child("rclcpp"), - "Error in destruction of rcl client handle: %s", rcl_get_error_string().str); + "Error in destruction of rcl action client handle: %s", rcl_get_error_string().str); rcl_reset_error(); } } else { RCLCPP_ERROR( rclcpp::get_logger("rclcpp"), - "Error in destruction of rcl client handle: " + "Error in destruction of rcl action client handle: " "the Node Handle was destructed too early. You will leak memory"); } delete client; }); - *client_handle_ = rcl_get_zero_initialized_client(); + *client_handle = rcl_get_zero_initialized_client(); rcl_ret_t ret = rcl_action_client_init( - client_handle_.get(), node_handle_.get(), type_support, + client_handle.get(), node_handle.get(), type_support, action_name.c_str(), &action_client_options); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error( - ret, "could not create action client"); + ret, "could not initialize rcl action client"); } + ret = rcl_action_client_wait_set_get_num_entities( - client_handle_.get() - &num_subscriptions, - &num_guard_conditions, - &num_timers, - &num_clients, - &num_services); + client_handle.get() + &num_subscriptions, + &num_guard_conditions, + &num_timers, + &num_clients, + &num_services); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error( - ret, "could not retrieve action client details"); + ret, "could not retrieve rcl action client details"); } } - void - handle_goal_response( - std::shared_ptr response_header, - std::shared_ptr response) - { - std::unique_lock lock(goals_mutex_); - const int64_t sequence_number = response_header->sequence_number; - if (pending_goal_responses_.count(sequence_number) == 0) { - RCUTILS_LOG_ERROR_NAMED("rclcpp", "unknown goal response, ignoring..."); - return; - } - pending_goal_responses_[sequence_number](response); - pending_goal_responses_.erase(sequence_number); - } + size_t num_subscriptions{0u}; + size_t num_guard_conditions{0u}; + size_t num_timers{0u}; + size_t num_clients{0u}; + size_t num_services{0u}; - void - send_goal_request(std::shared_ptr request, ResponseCallback callback) - { - std::unique_lock lock(goals_mutex_); - int64_t sequence_number; - rcl_ret_t ret = rcl_action_send_goal_request( - client_handle_.get(), request.get(), &sequence_number); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send goal request"); - } - assert(pending_goal_responses_.count(sequence_number) != 0); - pending_goal_responses_[sequence_number] = callback; - } + bool is_feedback_ready{false}; + bool is_status_ready{false}; + bool is_goal_response_ready{false}; + bool is_cancel_response_ready{false}; + bool is_result_response_ready{false}; - void - send_result_request(std::shared_ptr request, ResponseCallback callback) - { - std::unique_lock lock(results_mutex_); - int64_t sequence_number; - rcl_ret_t ret = rcl_action_send_result_request( - client_handle_.get(), request.get(), &sequence_number); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send goal request"); - } - assert(pending_result_responses_.count(sequence_number) != 0); - pending_result_responses_[sequence_number] = callback; - } + std::shared_ptr client_handle{nullptr}; + std::shared_ptr node_handle{nullptr}; + Logger logger; - void - handle_cancel_response( - std::shared_ptr response_header, - std::shared_ptr response) - { - std::unique_lock lock(cancellations_mutex_); - const int64_t sequence_number = response_header->sequence_number; - if (pending_cancel_responses_.count(sequence_number) == 0) { - RCUTILS_LOG_ERROR_NAMED("rclcpp", "unknown cancel response, ignoring..."); - return; - } - pending_cancel_responses_[sequence_number](response); - pending_cancel_responses_.erase(sequence_number); - } + std::map pending_goal_responses; + std::mutex goal_requests_mutex; - void - send_cancel_request(std::shared_ptr request, ResponseCallback callback) - { - std::unique_lock lock(cancellations_mutex_); - int64_t sequence_number; - rcl_ret_t ret = rcl_action_send_cancel_request( - client_handle_.get(), request.get(), &sequence_number); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send cancel request"); - } - if (pending_cancel_responses_.count(sequence_number) != 0) { - - } - pending_cancel_responses_[sequence_number] = callback; - } + std::map pending_result_responses; + std::mutex result_requests_mutex; - std::shared_ptr - get_action_client() - { - return client_handle_; - } + std::map pending_cancel_responses; + std::mutex cancel_requests_mutex; - // rcl interface entities - site_t num_subscriptions_, - num_guard_conditions_, - num_timers_, - num_clients_, - num_services_; - - // rcl interface flags - bool is_feedback_ready_, - is_status_ready_, - is_goal_response_ready_, - is_cancel_response_ready_, - is_result_response_ready_; -private: - std::map pending_goal_responses_; - std::map pending_result_responses_; - std::map pending_cancel_responses_; - std::shared_ptr client_handle_; - std::shared_ptr node_handle_; - std::mutex client_mutex_; + std::independent_bits_engine< + std::default_random_engine, 8, uint8_t> random_bytes_generator; }; -} ClientBase::ClientBase( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, @@ -187,137 +115,270 @@ ClientBase::ClientBase( { } -bool -ClientBase::wait_for_action_nanoseconds(std::chrono::nanoseconds timeout) +ClientBase::~ClientBase() { } - -ClientBase::~ClientBase() +Logger +ClientBase::get_logger() { - + return pimpl_->logger; } size_t -ClientBase::get_number_of_ready_subscriptions() override +ClientBase::get_number_of_ready_subscriptions() { - return pimpl_->num_subscriptions_; + return pimpl_->num_subscriptions; } size_t -ClientBase::get_number_of_ready_guard_conditions() override +ClientBase::get_number_of_ready_guard_conditions() { - return pimpl_->num_guard_conditions_; + return pimpl_->num_guard_conditions; } size_t -ClientBase::get_number_of_ready_timers() override +ClientBase::get_number_of_ready_timers() { - return pimpl_->num_timers_; + return pimpl_->num_timers; } size_t -ClientBase::get_number_of_ready_clients() override +ClientBase::get_number_of_ready_clients() { - return pimpl_->num_clients_; + return pimpl_->num_clients; } size_t -ClientBase::get_number_of_ready_services() override +ClientBase::get_number_of_ready_services() { - return pimpl_->num_services_; + return pimpl_->num_services; } bool -ClientBase::add_to_wait_set(rcl_wait_set_t * wait_set) override +ClientBase::add_to_wait_set(rcl_wait_set_t * wait_set) { rcl_ret_t ret = rcl_action_wait_set_add_action_client( - wait_set, pimpl_->get_action_client().get()); - return (RCL_RET_OK != ret); + wait_set, pimpl_->client_handle.get()); + return (RCL_RET_OK == ret); } bool -ClientBase::is_ready(rcl_wait_set_t * wait_set) override +ClientBase::is_ready(rcl_wait_set_t * wait_set) { rcl_ret_t ret = rcl_action_client_wait_set_get_entities_ready( - wait_set, pimpl_->get_action_client().get(), - &is_feedback_ready_, - &is_status_ready_, - &is_goal_response_ready_, - &is_cancel_response_ready_, - &is_result_response_ready_); + wait_set, pimpl_->client_handle.get(), + &pimpl_->is_feedback_ready, + &pimpl_->is_status_ready, + &pimpl_->is_goal_response_ready, + &pimpl_->is_cancel_response_ready, + &pimpl_->is_result_response_ready); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error( - ret, "failed to check for ready entities"); + ret, "failed to check for any ready entities"); } - return is_feedback_ready_ || - is_status_ready_ || - is_goal_response_ready_ || - is_cancel_response_ready_ || - is_result_response_ready_; + return ( + pimpl_->is_feedback_ready || + pimpl_->is_status_ready || + pimpl_->is_goal_response_ready || + pimpl_->is_cancel_response_ready || + pimpl_->is_result_response_ready); } void -ClientBase::execute() override +ClientBase::handle_goal_response( + const rmw_request_id_t & response_header, + std::shared_ptr response) { - std::shared_ptr client_handle = pimpl_->get_client_handle(); - if (pimpl_->is_feedback_ready) - { + std::lock_guard lock(pimpl_->goal_requests_mutex); + const int64_t & sequence_number = response_header.sequence_number; + if (pimpl_->pending_goal_responses.count(sequence_number) == 0) { + RCLCPP_ERROR(pimpl_->logger, "unknown goal response, ignoring..."); + return; + } + pimpl_->pending_goal_responses[sequence_number](response); + pimpl_->pending_goal_responses.erase(sequence_number); +} + +void +ClientBase::send_goal_request(std::shared_ptr request, ResponseCallback callback) +{ + std::unique_lock lock(pimpl_->goal_requests_mutex); + int64_t sequence_number; + rcl_ret_t ret = rcl_action_send_goal_request( + pimpl_->client_handle.get(), request.get(), &sequence_number); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send goal request"); + } + assert(pimpl_->pending_goal_responses.count(sequence_number) != 0); + pimpl_->pending_goal_responses[sequence_number] = callback; +} + +void +ClientBase::handle_result_response( + const rmw_request_id_t & response_header, + std::shared_ptr response) +{ + std::lock_guard lock(pimpl_->result_requests_mutex); + const int64_t & sequence_number = response_header.sequence_number; + if (pimpl_->pending_result_responses.count(sequence_number) == 0) { + RCLCPP_ERROR(pimpl_->logger, "unknown result response, ignoring..."); + return; + } + pimpl_->pending_result_responses[sequence_number](response); + pimpl_->pending_result_responses.erase(sequence_number); +} + +void +ClientBase::send_result_request(std::shared_ptr request, ResponseCallback callback) +{ + std::lock_guard lock(pimpl_->result_requests_mutex); + int64_t sequence_number; + rcl_ret_t ret = rcl_action_send_result_request( + pimpl_->client_handle.get(), request.get(), &sequence_number); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send result request"); + } + assert(pimpl_->pending_result_responses.count(sequence_number) != 0); + pimpl_->pending_result_responses[sequence_number] = callback; +} + +void +ClientBase::handle_cancel_response( + const rmw_request_id_t & response_header, + std::shared_ptr response) +{ + std::lock_guard lock(pimpl_->goal_cancellations_mutex); + const int64_t & sequence_number = response_header.sequence_number; + if (pimpl_->pending_cancel_responses.count(sequence_number) == 0) { + RCLCPP_ERROR(pimpl_->logger, "unknown cancel response, ignoring..."); + return; + } + pimpl_->pending_cancel_responses[sequence_number](response); + pimpl_->pending_cancel_responses.erase(sequence_number); +} + +void +ClientBase::send_cancel_request(std::shared_ptr request, ResponseCallback callback) +{ + std::lock_guard lock(pimpl_->goal_cancellations_mutex); + int64_t sequence_number; + rcl_ret_t ret = rcl_action_send_cancel_request( + pimpl_->client_handle.get(), request.get(), &sequence_number); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send cancel request"); + } + assert(pimpl_->pending_cancel_responses.count(sequence_number) != 0); + pimpl_->pending_cancel_responses[sequence_number] = callback; +} + +GoalID +ClientBase::generate_goal_id() +{ + GoalID goal_id; + // TODO(hidmic): Do something better than this for UUID generation. + std::generate( + goal_id.uuid.begin(), goal_id.uuid.end(), + std::ref(pimpl_->random_bytes_generator)); + return goal_id; +} + +void +ClientBase::execute() +{ + if (pimpl_->is_feedback_ready) { std::shared_ptr feedback_message = this->create_feedback_message(); - rcl_ret_t ret = rcl_action_take_feedback(client_handle.get(), feedback_message.get()); - if (RCL_RET_OK == ret) { + rcl_ret_t ret = rcl_action_take_feedback( + pimpl_->client_handle.get(), feedback_message.get()); + pimpl_->is_feedback_ready = false; + if (RCL_RET_OK != ret) { + // TODO(hidmic): should throw instead? + RCLCPP_ERROR( + pimpl_->logger, + "Error taking feedback in '%s' action client: %s", + rcl_action_client_get_action_name(pimpl_->client_handle.get()), + rcl_get_error_string().str); + rcl_reset_error(); } else { - this->handle_feedback(feedback_message); - is_feedback_ready = false; + this->handle_feedback_message(feedback_message); } } - if (is_status_ready_) - { + + if (pimpl_->is_status_ready) { std::shared_ptr status_message = this->create_status_message(); - rcl_ret_t ret = rcl_action_take_status(client_handle.get(), status_message.get()); - if (RCL_RET_OK == ret) { - is_status_ready = false; - this->handle_status(status_message); + rcl_ret_t ret = rcl_action_take_status( + pimpl_->client_handle.get(), status_message.get()); + pimpl_->is_status_ready = false; + if (RCL_RET_OK != ret) { + // TODO(hidmic): should throw instead? + RCLCPP_ERROR( + pimpl_->logger, + "Error taking status in '%s' action client: %s", + rcl_action_client_get_action_name(pimpl_->client_handle.get()), + rcl_get_error_string().str); + rcl_reset_error(); + } else { + this->handle_status_message(status_message); } } - if (is_goal_response_ready_) - { + + if (pimpl_->is_goal_response_ready) { + rmw_request_id_t response_header; std::shared_ptr goal_response = this->create_goal_response(); - std::shared_ptr response_header = this->create_response_header(); rcl_ret_t ret = rcl_action_take_goal_response( - client_handle.get(), response_header.get(), goal_response.get()); - - } - if (is_cancel_response_ready_) - { - std::shared_ptr cancel_response = this->create_cancel_response(); - std::shared_ptr response_header = this->create_response_header(); - rcl_ret_t ret = rcl_action_take_cancel_response( - client_handle.get(), response_header.get(), cancel_response.get()); - if (RCL_RET_OK == ret) { - is_cancel_response_ready = false; - this->handle_cancel_response(response_header, cancel_response); - } else { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "take goal response failed for client of action '%s': %s", - pimpl_->get_action_name(), rcl_get_error_string().str); + pimpl_->client_handle.get(), &response_header, goal_response.get()); + pimpl_->is_goal_response_ready = false; + if (RCL_RET_OK != ret) { + // TODO(hidmic): should throw instead? + RCLCPP_ERROR( + pimpl_->logger, + "Error taking goal response in '%s' action client: %s", + rcl_action_client_get_action_name(pimpl_->client_handle.get()), + rcl_get_error_string().str); rcl_reset_error(); + } else { + this->handle_goal_response(response_header, goal_response); } } - if (is_result_response_ready_) - { + + if (pimpl_->is_result_response_ready) { + rmw_request_id_t response_header; std::shared_ptr result_response = this->create_result_response(); - std::shared_ptr response_header = this->create_response_header(); rcl_ret_t ret = rcl_action_take_result_response( - client_handle.get(), response_header.get(), result_response.get()); - if (RCL_RET_OK == ret) { - is_result_response_ready = false; - this->handle_result_response(response_header, result_response); + pimpl_->client_handle.get(), &response_header, result_response.get()); + pimpl_->is_result_response_ready = false; + if (RCL_RET_OK != ret) { + // TODO(hidmic): should throw instead? + RCLCPP_ERROR( + pimpl_->logger, + "Error taking result response in '%s' action client: %s", + rcl_action_client_get_action_name(pimpl_->client_handle.get()), + rcl_get_error_string().str); + rcl_reset_error(); } else { + this->handle_result_response(response_header, result_response); + } + } + if (pimpl_->is_cancel_response_ready) { + rmw_request_id_t response_header; + std::shared_ptr cancel_response = this->create_cancel_response(); + rcl_ret_t ret = rcl_action_take_cancel_response( + pimpl_->client_handle.get(), &response_header, cancel_response.get()); + pimpl_->is_cancel_response_ready = false; + if (RCL_RET_OK != ret) { + // TODO(hidmic): should throw instead? + RCLCPP_ERROR( + pimpl_->logger, + "Error taking cancel response in '%s' action client: %s", + rcl_action_client_get_action_name(pimpl_->client_handle.get()), + rcl_get_error_string().str); + rcl_reset_error(); + } else { + this->handle_cancel_response(response_header, cancel_response); } } } +} // namespace rclcpp_action From 1e9d56ea7a59f4daa375e8f89cd9b1aafd007739 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 27 Nov 2018 16:29:54 -0800 Subject: [PATCH 19/56] Misc fixes for the action client --- .../include/rclcpp_action/client.hpp | 36 +++++++++++-------- .../rclcpp_action/client_goal_handle.hpp | 12 +++++-- .../rclcpp_action/client_goal_handle_impl.hpp | 10 ++++++ rclcpp_action/include/rclcpp_action/types.hpp | 7 +++- rclcpp_action/src/client.cpp | 14 ++++---- 5 files changed, 55 insertions(+), 24 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 80bda55bc8..6eb58b87c3 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -15,17 +15,23 @@ #ifndef RCLCPP_ACTION__CLIENT_HPP_ #define RCLCPP_ACTION__CLIENT_HPP_ +#include +#include +#include +#include +#include +#include -#include +#include #include #include -#include +#include -#include -#include -#include +#include +#include #include "rclcpp_action/client_goal_handle.hpp" +#include "rclcpp_action/types.hpp" #include "rclcpp_action/visibility_control.hpp" @@ -198,7 +204,7 @@ class Client : public ClientBase return; } } - this->goal_handles_[goal_handle->goal_id()] = goal_handle; + this->goal_handles_[goal_handle->get_goal_id()] = goal_handle; promise->set_value(goal_handle); }); return future; @@ -208,7 +214,7 @@ class Client : public ClientBase std::shared_future async_get_result(GoalHandle::SharedPtr goal_handle) { std::lock_guard lock(goal_handles_mutex_); - if (goal_handles_.count(goal_handle->goal_id()) == 0) { + if (goal_handles_.count(goal_handle->get_goal_id()) == 0) { throw exceptions::UnknownGoalHandleError(goal_handle); } if (!goal_handle->is_result_aware()) { @@ -221,14 +227,14 @@ class Client : public ClientBase std::shared_future async_cancel_goal(GoalHandle::SharedPtr goal_handle) { std::lock_guard lock(goal_handles_mutex_); - if (goal_handles_.count(goal_handle->goal_id()) == 0) { + if (goal_handles_.count(goal_handle->get_goal_id()) == 0) { throw exceptions::UnknownGoalHandleError(goal_handle); } std::promise promise; std::shared_future future(promise.get_future()); using CancelGoalRequest = typename ACTION::CancelGoalService::Request; auto cancel_goal_request = std::make_shared(); - cancel_goal_request->goal_info.goal_id = goal_handle->goal_id(); + cancel_goal_request->goal_info.goal_id = goal_handle->get_goal_id(); this->send_cancel_request( std::static_pointer_cast(cancel_goal_request), [goal_handle, promise{std::move(promise)}] (std::shared_ptr response) @@ -238,7 +244,7 @@ class Client : public ClientBase bool goal_canceled = false; if (!cancel_response->goals_canceling.empty()) { const GoalInfo & canceled_goal_info = cancel_response->goals_canceling[0]; - if (canceled_goal_info.goal_id == goal_handle->goal_id()) { + if (canceled_goal_info.goal_id == goal_handle->get_goal_id()) { goal_canceled = canceled_goal_info.accepted; } } @@ -340,10 +346,12 @@ class Client : public ClientBase continue; } GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id]; - goal_handle->set_status(status); + goal_handle->set_status(status.status); + const int8_t goal_status = goal_handle->get_status(); if ( - goal_handle->get_status() == STATUS_CANCELED || - goal_handle->get_status() == STATUS_ABORTED + goal_status == GoalStatus::STATUS_SUCCEDED || + goal_status == GoalStatus::STATUS_CANCELED || + goal_status == GoalStatus::STATUS_ABORTED ) { goal_handles_.erase(goal_id); } @@ -353,7 +361,7 @@ class Client : public ClientBase void make_result_aware(GoalHandle::SharedPtr goal_handle) { using GoalResultRequest = typename ACTION::GoalResultService::Request; auto goal_result_request = std::make_shared(); - goal_result_request.goal_id = goal_handle->goal_id(); + goal_result_request.goal_id = goal_handle->get_goal_id(); this->send_result_request( std::static_pointer_cast(goal_result_request), [goal_handle] (std::shared_ptr response) diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp index 8753667890..52b17649ab 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp @@ -18,11 +18,13 @@ #include #include +#include #include #include -#include "action_msgs/msg/goal_status.hpp" -#include "action_msgs/msg/goal_info.hpp" +#include + +#include "rclcpp_action/types.hpp" #include "rclcpp_action/visibility_control.hpp" namespace rclcpp_action @@ -44,6 +46,8 @@ class ClientGoalHandle virtual ~ClientGoalHandle(); + const GoalID & get_goal_id() const; + std::shared_future async_result(); @@ -60,7 +64,7 @@ class ClientGoalHandle is_result_aware(); private: - // The templated Server creates goal handles + // The templated Client creates goal handles friend Client; ClientGoalHandle(const GoalInfo & info, FeedbackCallback callback); @@ -81,9 +85,11 @@ class ClientGoalHandle invalidate(); GoalInfo info_; + bool is_result_aware_{false}; std::promise result_promise_; std::shared_future result_future_; + FeedbackCallback feedback_callback_{nullptr}; int8_t status_{GoalStatus::STATE_ACCEPTED}; diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp index fca8a84e54..c78968ef65 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -17,8 +17,11 @@ #include +#include "rclcpp_action/exceptions.hpp" + namespace rclcpp_action { + template ClientGoalHandle::ClientGoalHandle( const GoalInfo & info, FeedbackCallback callback) @@ -31,6 +34,13 @@ ClientGoalHandle::~ClientGoalHandle() { } +template +const GoalID & +ClientGoalHandle::get_goal_id() const +{ + return info_.goal_id; +} + template std::shared_future::Result> ClientGoalHandle::async_result() diff --git a/rclcpp_action/include/rclcpp_action/types.hpp b/rclcpp_action/include/rclcpp_action/types.hpp index 4a014792f0..d505b15328 100644 --- a/rclcpp_action/include/rclcpp_action/types.hpp +++ b/rclcpp_action/include/rclcpp_action/types.hpp @@ -17,12 +17,17 @@ #include +#include +#include + #include + namespace rclcpp_action { using GoalID = unique_identifier_msgs::msg::UUID; using GoalStatus = action_msgs::msg::GoalStatus; +using GoalInfo = action_msgs::msg::GoalInfo; } // namespace @@ -33,7 +38,7 @@ struct less { bool operator()( const rclcpp_action::GoalID & id0, const rclcpp_action::GoalID & id1) { - return id0.uuid < id1.uuid; + return (id0.uuid < id1.uuid); } }; diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index ff37f9206a..606e811089 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -14,6 +14,8 @@ #include +#include +#include #include #include @@ -191,7 +193,7 @@ ClientBase::handle_goal_response( const rmw_request_id_t & response_header, std::shared_ptr response) { - std::lock_guard lock(pimpl_->goal_requests_mutex); + std::lock_guard guard(pimpl_->goal_requests_mutex); const int64_t & sequence_number = response_header.sequence_number; if (pimpl_->pending_goal_responses.count(sequence_number) == 0) { RCLCPP_ERROR(pimpl_->logger, "unknown goal response, ignoring..."); @@ -204,7 +206,7 @@ ClientBase::handle_goal_response( void ClientBase::send_goal_request(std::shared_ptr request, ResponseCallback callback) { - std::unique_lock lock(pimpl_->goal_requests_mutex); + std::unique_lock guard(pimpl_->goal_requests_mutex); int64_t sequence_number; rcl_ret_t ret = rcl_action_send_goal_request( pimpl_->client_handle.get(), request.get(), &sequence_number); @@ -220,7 +222,7 @@ ClientBase::handle_result_response( const rmw_request_id_t & response_header, std::shared_ptr response) { - std::lock_guard lock(pimpl_->result_requests_mutex); + std::lock_guard guard(pimpl_->result_requests_mutex); const int64_t & sequence_number = response_header.sequence_number; if (pimpl_->pending_result_responses.count(sequence_number) == 0) { RCLCPP_ERROR(pimpl_->logger, "unknown result response, ignoring..."); @@ -233,7 +235,7 @@ ClientBase::handle_result_response( void ClientBase::send_result_request(std::shared_ptr request, ResponseCallback callback) { - std::lock_guard lock(pimpl_->result_requests_mutex); + std::lock_guard guard(pimpl_->result_requests_mutex); int64_t sequence_number; rcl_ret_t ret = rcl_action_send_result_request( pimpl_->client_handle.get(), request.get(), &sequence_number); @@ -249,7 +251,7 @@ ClientBase::handle_cancel_response( const rmw_request_id_t & response_header, std::shared_ptr response) { - std::lock_guard lock(pimpl_->goal_cancellations_mutex); + std::lock_guard guard(pimpl_->goal_cancellations_mutex); const int64_t & sequence_number = response_header.sequence_number; if (pimpl_->pending_cancel_responses.count(sequence_number) == 0) { RCLCPP_ERROR(pimpl_->logger, "unknown cancel response, ignoring..."); @@ -262,7 +264,7 @@ ClientBase::handle_cancel_response( void ClientBase::send_cancel_request(std::shared_ptr request, ResponseCallback callback) { - std::lock_guard lock(pimpl_->goal_cancellations_mutex); + std::lock_guard guard(pimpl_->goal_cancellations_mutex); int64_t sequence_number; rcl_ret_t ret = rcl_action_send_cancel_request( pimpl_->client_handle.get(), request.get(), &sequence_number); From 5ec75621770ff2f63889629a3c82f8d77eb9a3e7 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 27 Nov 2018 17:44:24 -0800 Subject: [PATCH 20/56] Yet more misc fixes for the action client --- .../include/rclcpp_action/client.hpp | 158 ++++++++++-------- .../rclcpp_action/client_goal_handle.hpp | 4 +- .../rclcpp_action/client_goal_handle_impl.hpp | 11 +- .../include/rclcpp_action/exceptions.hpp | 25 ++- rclcpp_action/include/rclcpp_action/types.hpp | 6 +- rclcpp_action/src/client.cpp | 35 ++-- 6 files changed, 146 insertions(+), 93 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 6eb58b87c3..44a9dbd4f2 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -22,8 +22,9 @@ #include #include -#include +#include #include +#include #include #include @@ -42,7 +43,7 @@ class ClientBaseImpl; /// Base Action Client implementation /// It is responsible for interfacing with the C action client API. -class ClientBase : public Waitable +class ClientBase : public rclcpp::Waitable { public: // TODO(sloretz) NodeLoggingInterface when it can be gotten off a node @@ -60,19 +61,19 @@ class ClientBase : public Waitable size_t get_number_of_ready_subscriptions() override; - RCLCPP_PUBLIC + RCLCPP_ACTION_PUBLIC size_t get_number_of_ready_timers() override; - RCLCPP_PUBLIC + RCLCPP_ACTION_PUBLIC size_t get_number_of_ready_clients() override; - RCLCPP_PUBLIC + RCLCPP_ACTION_PUBLIC size_t get_number_of_ready_services() override; - RCLCPP_PUBLIC + RCLCPP_ACTION_PUBLIC size_t get_number_of_ready_guard_conditions() override; @@ -89,8 +90,11 @@ class ClientBase : public Waitable execute() override; protected: + using ResponseCallback = + std::function response)>; + RCLCPP_ACTION_PUBLIC - Logger get_logger(); + rclcpp::Logger get_logger(); RCLCPP_ACTION_PUBLIC virtual GoalID generate_goal_id(); @@ -123,7 +127,7 @@ class ClientBase : public Waitable const rmw_request_id_t & response_header, std::shared_ptr result_response); - virtual std::shared_ptr create_cancel_response() const = 0; + virtual std::shared_ptr create_cancel_response() const; virtual void handle_cancel_response( const rmw_request_id_t & response_header, @@ -133,7 +137,7 @@ class ClientBase : public Waitable virtual void handle_feedback_message(std::shared_ptr message) = 0; - virtual std::shared_ptr create_status_message() const = 0; + virtual std::shared_ptr create_status_message() const; virtual void handle_status_message(std::shared_ptr message) = 0; @@ -155,10 +159,18 @@ class Client : public ClientBase using Feedback = typename ACTION::Feedback; using GoalHandle = ClientGoalHandle; using FeedbackCallback = std::function; + typename GoalHandle::SharedPtr, const Feedback &)>; using CancelRequest = typename ACTION::CancelGoalService::Request; using CancelResponse = typename ACTION::CancelGoalService::Response; + Client( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + const std::string & action_name + ) + : Client(node_base, action_name, rcl_action_client_get_default_options()) + { + } + Client( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, const std::string & action_name, const rcl_action_client_options_t & client_options @@ -171,29 +183,34 @@ class Client : public ClientBase } RCLCPP_ACTION_PUBLIC - std::shared_future async_send_goal( + std::shared_future async_send_goal( const Goal & goal, FeedbackCallback callback = nullptr, bool ignore_result = false) { + std::promise promise; + std::shared_future future(promise.get_future()); using GoalRequest = typename ACTION::GoalRequestService::Request; - auto goal_request = std::make_shared(); - goal_request->goal_id = this->generate_goal_id(); - goal_request->goal = goal; - std::promise promise; - std::shared_future future(promise.get_future()); + // auto goal_request = std::make_shared(); + // goal_request->goal_id = this->generate_goal_id(); + // goal_request->goal = goal; + auto goal_request = std::make_shared(goal); + goal_request->uuid = this->generate_goal_id(); this->send_goal_request( std::static_pointer_cast(goal_request), - [this, goal_request, promise{std::move(promise)}, callback] (std::shared_ptr response) + [this, goal_request, + promise{std::move(promise)}, + callback, ignore_result] (std::shared_ptr response) mutable { using GoalResponse = typename ACTION::GoalService::Response; - GoalResponse::SharedPtr goal_response = std::static_pointer_cast(response); + typename GoalResponse::SharedPtr goal_response = + std::static_pointer_cast(response); if (!goal_response->accepted) { promise->set_exception(std::make_exception_ptr( exceptions::RejectedGoalError(goal_request->goal))); return; } - std::lock_guard lock(goal_handles_mutex_); GoalInfo goal_info; - goal_info.goal_id = goal_request->goal_id; + // goal_info.goal_id = goal_request->goal_id; + goal_info.uuid = goal_request->uuid; goal_info.stamp = goal_response->stamp; auto goal_handle = std::make_shared(goal_info, callback); if (!ignore_result) { @@ -204,7 +221,8 @@ class Client : public ClientBase return; } } - this->goal_handles_[goal_handle->get_goal_id()] = goal_handle; + std::lock_guard guard(goal_handles_mutex_); + goal_handles_[goal_handle->get_goal_id()] = goal_handle; promise->set_value(goal_handle); }); return future; @@ -212,7 +230,7 @@ class Client : public ClientBase RCLCPP_ACTION_PUBLIC std::shared_future - async_get_result(GoalHandle::SharedPtr goal_handle) { + async_get_result(typename GoalHandle::SharedPtr goal_handle) { std::lock_guard lock(goal_handles_mutex_); if (goal_handles_.count(goal_handle->get_goal_id()) == 0) { throw exceptions::UnknownGoalHandleError(goal_handle); @@ -225,28 +243,27 @@ class Client : public ClientBase RCLCPP_ACTION_PUBLIC std::shared_future - async_cancel_goal(GoalHandle::SharedPtr goal_handle) { + async_cancel_goal(typename GoalHandle::SharedPtr goal_handle) { std::lock_guard lock(goal_handles_mutex_); if (goal_handles_.count(goal_handle->get_goal_id()) == 0) { throw exceptions::UnknownGoalHandleError(goal_handle); } std::promise promise; std::shared_future future(promise.get_future()); - using CancelGoalRequest = typename ACTION::CancelGoalService::Request; - auto cancel_goal_request = std::make_shared(); - cancel_goal_request->goal_info.goal_id = goal_handle->get_goal_id(); + auto cancel_request = std::make_shared(); + // cancel_request->goal_info.goal_id = goal_handle->get_goal_id(); + cancel_request->goal_info.uuid = goal_handle->get_goal_id(); this->send_cancel_request( - std::static_pointer_cast(cancel_goal_request), - [goal_handle, promise{std::move(promise)}] (std::shared_ptr response) + std::static_pointer_cast(cancel_request), + [goal_handle, promise{std::move(promise)}] (std::shared_ptr response) mutable { - CancelResponse::SharedPtr cancel_response = + typename CancelResponse::SharedPtr cancel_response = std::static_pointer_cast(response); bool goal_canceled = false; if (!cancel_response->goals_canceling.empty()) { const GoalInfo & canceled_goal_info = cancel_response->goals_canceling[0]; - if (canceled_goal_info.goal_id == goal_handle->get_goal_id()) { - goal_canceled = canceled_goal_info.accepted; - } + // goal_canceled = (canceled_goal_info.goal_id == goal_handle->get_goal_id()); + goal_canceled = (canceled_goal_info.uuid == goal_handle->get_goal_id()); } promise.set_value(goal_canceled); }); @@ -254,48 +271,55 @@ class Client : public ClientBase } RCLCPP_ACTION_PUBLIC - std::shared_future + std::shared_future async_cancel_all_goals() { - auto cancel_request = std::make_shared(); - std::fill(cancel_request->goal_info.goal_id.uuid, 0u); - return async_cancel(cancel_goal_request); + auto cancel_request = std::make_shared(); + // std::fill(cancel_request->goal_info.goal_id.uuid, 0u); + std::fill(cancel_request->goal_info.uuid, 0u); + return async_cancel(cancel_request); } RCLCPP_ACTION_PUBLIC - std::shared_future + std::shared_future async_cancel_goals_before(const rclcpp::Time & stamp) { - auto cancel_request = std::make_shared(); - std::fill(cancel_request->goal_info.goal_id.uuid, 0u); + auto cancel_request = std::make_shared(); + // std::fill(cancel_request->goal_info.goal_id.uuid, 0u); + std::fill(cancel_request->goal_info.uuid, 0u); cancel_request->goal_info.stamp = stamp; return async_cancel(cancel_request); } virtual ~Client() { - std::lock_guard lock(goal_handles_mutex_); + std::lock_guard guard(goal_handles_mutex_); auto it = goal_handles_.begin(); - while (it != it->goal_handles_.end()) { + while (it != goal_handles_.end()) { it->second->invalidate(); it = goal_handles_.erase(it); } } private: - std::shared_ptr create_goal_response() override + std::shared_ptr create_goal_response() const override { using GoalResponse = typename ACTION::GoalRequestService::Response; return std::shared_ptr(new GoalResponse()); } - std::shared_ptr create_result_response() override + std::shared_ptr create_result_response() const override { using GoalResultResponse = typename ACTION::GoalResultService::Response; return std::shared_ptr(new GoalResultResponse()); } - std::shared_ptr create_feedback_message() override + std::shared_ptr create_cancel_response() const override + { + return std::shared_ptr(new CancelResponse()); + } + + std::shared_ptr create_feedback_message() const override { using FeedbackMessage = typename ACTION::FeedbackMessage; return std::shared_ptr(new FeedbackMessage()); @@ -303,10 +327,10 @@ class Client : public ClientBase void handle_feedback_message(std::shared_ptr message) override { + std::lock_guard guard(goal_handles_mutex_); using FeedbackMessage = typename ACTION::FeedbackMessage; - FeedbackMessage::SharedPtr feedback_message = + typename FeedbackMessage::SharedPtr feedback_message = std::static_pointer_cast(message); - std::unique_lock lock(goal_handles_mutex_); const GoalID & goal_id = feedback_message->goal_id; if (goal_handles_.count(goal_id) == 0) { RCLCPP_DEBUG( @@ -314,7 +338,7 @@ class Client : public ClientBase "Received feedback for unknown goal. Ignoring..."); return; } - GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id]; + typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id]; if (!goal_handle->is_feedback_aware()) { RCLCPP_DEBUG( this->get_logger(), @@ -325,7 +349,7 @@ class Client : public ClientBase callback(goal_handle, feedback_message->feedback); } - std::shared_ptr create_status_message() override + std::shared_ptr create_status_message() const override { using GoalStatusMessage = typename ACTION::GoalStatusMessage; return std::shared_ptr(new GoalStatusMessage()); @@ -333,23 +357,24 @@ class Client : public ClientBase void handle_status_message(std::shared_ptr message) override { - std::lock_guard lock(goal_handles_mutex_); + std::lock_guard guard(goal_handles_mutex_); using GoalStatusMessage = typename ACTION::GoalStatusMessage; - GoalStatusMessage::SharedPtr status_message = + typename GoalStatusMessage::SharedPtr status_message = std::static_pointer_cast(message); - for (const GoalStatus & status : status_message.status_list) { - const GoalID & goal_id = status.goal_info.goal_id; + for (const GoalStatus & status : status_message->status_list) { + // const GoalID & goal_id = status.goal_info.goal_id; + const GoalID & goal_id = status.goal_info.uuid; if (goal_handles_.count(goal_id) == 0) { RCLCPP_DEBUG( this->get_logger(), "Received status for unknown goal. Ignoring..."); continue; } - GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id]; + typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id]; goal_handle->set_status(status.status); const int8_t goal_status = goal_handle->get_status(); if ( - goal_status == GoalStatus::STATUS_SUCCEDED || + goal_status == GoalStatus::STATUS_SUCCEEDED || goal_status == GoalStatus::STATUS_CANCELED || goal_status == GoalStatus::STATUS_ABORTED ) { @@ -358,39 +383,40 @@ class Client : public ClientBase } } - void make_result_aware(GoalHandle::SharedPtr goal_handle) { + void make_result_aware(typename GoalHandle::SharedPtr goal_handle) { using GoalResultRequest = typename ACTION::GoalResultService::Request; auto goal_result_request = std::make_shared(); - goal_result_request.goal_id = goal_handle->get_goal_id(); + // goal_result_request.goal_id = goal_handle->get_goal_id(); + goal_result_request.uuid = goal_handle->get_goal_id(); this->send_result_request( std::static_pointer_cast(goal_result_request), [goal_handle] (std::shared_ptr response) { using GoalResultResponse = typename ACTION::GoalResultService::Response; - GoalResultRequest::SharedPtr goal_result_response = - std::static_pointer_cast(response); + typename GoalResultResponse::SharedPtr goal_result_response = + std::static_pointer_cast(response); goal_handle->set_result(goal_result_response); }); goal_handle->set_result_awareness(true); } - - std::shared_future - async_cancel(CancelRequest::SharedPtr cancel_request) + + std::shared_future + async_cancel(typename CancelRequest::SharedPtr cancel_request) { - std::promise promise; - std::shared_future future(promise.get_future()); + std::promise promise; + std::shared_future future(promise.get_future()); this->send_cancel_request( std::static_pointer_cast(cancel_request), - [promise{std::move(promise)}] (std::shared_ptr response) + [promise{std::move(promise)}] (std::shared_ptr response) mutable { - CancelResponse::SharedPtr cancel_response = + typename CancelResponse::SharedPtr cancel_response = std::static_pointer_cast(response); promise.set_value(cancel_response); }); return future; } - std::map goal_handles_; + std::map goal_handles_; std::mutex goal_handles_mutex_; }; } // namespace rclcpp_action diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp index 52b17649ab..ede3cfe96a 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include "rclcpp_action/types.hpp" #include "rclcpp_action/visibility_control.hpp" @@ -91,7 +91,7 @@ class ClientGoalHandle std::shared_future result_future_; FeedbackCallback feedback_callback_{nullptr}; - int8_t status_{GoalStatus::STATE_ACCEPTED}; + int8_t status_{GoalStatus::STATUS_ACCEPTED}; std::mutex handle_mutex_; }; diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp index c78968ef65..c6f3312ba5 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -25,7 +25,7 @@ namespace rclcpp_action template ClientGoalHandle::ClientGoalHandle( const GoalInfo & info, FeedbackCallback callback) -: info_(info), feedback_callback_(callback), +: info_(info), feedback_callback_(callback) { } @@ -38,7 +38,8 @@ template const GoalID & ClientGoalHandle::get_goal_id() const { - return info_.goal_id; + // return info_.goal_id; + return info_.uuid; } template @@ -58,7 +59,7 @@ ClientGoalHandle::set_result(const Result & result) { std::lock_guard guard(handle_mutex_); status_ = result.status; - promise_result_.set_value(result); + result_promise_.set_value(result); } template @@ -118,11 +119,11 @@ ClientGoalHandle::set_result_awareness(bool awareness) } template -bool +void ClientGoalHandle::invalidate() { std::lock_guard guard(handle_mutex_); - status_ = GoalStatus::STATE_UNKNOWN; + status_ = GoalStatus::STATUS_UNKNOWN; result_promise_.set_exception(std::make_exception_ptr( exceptions::UnawareGoalHandleError())); } diff --git a/rclcpp_action/include/rclcpp_action/exceptions.hpp b/rclcpp_action/include/rclcpp_action/exceptions.hpp index 5adc9ea7d5..777182554a 100644 --- a/rclcpp_action/include/rclcpp_action/exceptions.hpp +++ b/rclcpp_action/include/rclcpp_action/exceptions.hpp @@ -22,14 +22,31 @@ namespace rclcpp_action namespace exceptions { -class InvalidGoalHandle : public std::invalid_argument +class RejectedGoalError : public std::runtime_error { - InvalidGoalHandle() = default; +public: + RejectedGoalError() + : std::runtime_error("") + { + } }; -class UnawareGoalHandle : public std::runtime_error +class UnknownGoalHandleError : public std::invalid_argument { - UnawareGoalHandle() = default; +public: + UnknownGoalHandleError() + : std::invalid_argument("") + { + } +}; + +class UnawareGoalHandleError : public std::runtime_error +{ +public: + UnawareGoalHandleError() + : std::runtime_error("") + { + } }; } // namespace exceptions diff --git a/rclcpp_action/include/rclcpp_action/types.hpp b/rclcpp_action/include/rclcpp_action/types.hpp index d505b15328..a74447c844 100644 --- a/rclcpp_action/include/rclcpp_action/types.hpp +++ b/rclcpp_action/include/rclcpp_action/types.hpp @@ -25,7 +25,8 @@ namespace rclcpp_action { -using GoalID = unique_identifier_msgs::msg::UUID; +//using GoalID = unique_identifier_msgs::msg::UUID; +using GoalID = std::array; using GoalStatus = action_msgs::msg::GoalStatus; using GoalInfo = action_msgs::msg::GoalInfo; @@ -38,7 +39,8 @@ struct less { bool operator()( const rclcpp_action::GoalID & id0, const rclcpp_action::GoalID & id1) { - return (id0.uuid < id1.uuid); + // return (id0.uuid < id1.uuid); + return id0 < id1; } }; diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index 606e811089..1122d12193 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -12,13 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "rclcpp_action/client.hpp" -#include +#include #include #include -#include +#include +#include namespace rclcpp_action { @@ -31,13 +32,13 @@ class ClientBaseImpl const std::string & action_name, const rosidl_action_type_support_t * type_support, const rcl_action_client_options_t & client_options) - : node_handle(node_base.get_shared_rcl_node_handle()), + : node_handle(node_base->get_shared_rcl_node_handle()), logger(rclcpp::get_logger(rcl_node_get_logger_name( node_handle.get())).get_child("rclcpp")) { std::weak_ptr weak_node_handle(node_handle); client_handle = std::shared_ptr( - new rcl_client_t, [weak_node_handle](rcl_action_client_t * client) + new rcl_action_client_t, [weak_node_handle](rcl_action_client_t * client) { auto handle = weak_node_handle.lock(); if (handle) { @@ -55,17 +56,17 @@ class ClientBaseImpl } delete client; }); - *client_handle = rcl_get_zero_initialized_client(); + *client_handle = rcl_action_get_zero_initialized_client(); rcl_ret_t ret = rcl_action_client_init( client_handle.get(), node_handle.get(), type_support, - action_name.c_str(), &action_client_options); + action_name.c_str(), &client_options); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error( ret, "could not initialize rcl action client"); } ret = rcl_action_client_wait_set_get_num_entities( - client_handle.get() + client_handle.get(), &num_subscriptions, &num_guard_conditions, &num_timers, @@ -91,7 +92,10 @@ class ClientBaseImpl std::shared_ptr client_handle{nullptr}; std::shared_ptr node_handle{nullptr}; - Logger logger; + rclcpp::Logger logger; + + using ResponseCallback = + std::function response)>; std::map pending_goal_responses; std::mutex goal_requests_mutex; @@ -122,7 +126,7 @@ ClientBase::~ClientBase() } -Logger +rclcpp::Logger ClientBase::get_logger() { return pimpl_->logger; @@ -162,7 +166,7 @@ bool ClientBase::add_to_wait_set(rcl_wait_set_t * wait_set) { rcl_ret_t ret = rcl_action_wait_set_add_action_client( - wait_set, pimpl_->client_handle.get()); + wait_set, pimpl_->client_handle.get(), nullptr, nullptr); return (RCL_RET_OK == ret); } @@ -251,7 +255,7 @@ ClientBase::handle_cancel_response( const rmw_request_id_t & response_header, std::shared_ptr response) { - std::lock_guard guard(pimpl_->goal_cancellations_mutex); + std::lock_guard guard(pimpl_->cancel_requests_mutex); const int64_t & sequence_number = response_header.sequence_number; if (pimpl_->pending_cancel_responses.count(sequence_number) == 0) { RCLCPP_ERROR(pimpl_->logger, "unknown cancel response, ignoring..."); @@ -264,7 +268,7 @@ ClientBase::handle_cancel_response( void ClientBase::send_cancel_request(std::shared_ptr request, ResponseCallback callback) { - std::lock_guard guard(pimpl_->goal_cancellations_mutex); + std::lock_guard guard(pimpl_->cancel_requests_mutex); int64_t sequence_number; rcl_ret_t ret = rcl_action_send_cancel_request( pimpl_->client_handle.get(), request.get(), &sequence_number); @@ -280,8 +284,11 @@ ClientBase::generate_goal_id() { GoalID goal_id; // TODO(hidmic): Do something better than this for UUID generation. + // std::generate( + // goal_id.uuid.begin(), goal_id.uuid.end(), + // std::ref(pimpl_->random_bytes_generator)); std::generate( - goal_id.uuid.begin(), goal_id.uuid.end(), + goal_id.begin(), goal_id.end(), std::ref(pimpl_->random_bytes_generator)); return goal_id; } From 68a2e17397ff128b7ae50135916f83f959b5d7c9 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 27 Nov 2018 17:51:41 -0800 Subject: [PATCH 21/56] Few more fixes and shortcuts to deal with missing type support. --- .../include/rclcpp_action/client.hpp | 23 +++++++++++-------- .../rclcpp_action/client_goal_handle_impl.hpp | 2 +- rclcpp_action/include/rclcpp_action/types.hpp | 2 +- 3 files changed, 16 insertions(+), 11 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 44a9dbd4f2..2b5cb4d357 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -127,7 +127,7 @@ class ClientBase : public rclcpp::Waitable const rmw_request_id_t & response_header, std::shared_ptr result_response); - virtual std::shared_ptr create_cancel_response() const; + virtual std::shared_ptr create_cancel_response() const = 0; virtual void handle_cancel_response( const rmw_request_id_t & response_header, @@ -137,7 +137,7 @@ class ClientBase : public rclcpp::Waitable virtual void handle_feedback_message(std::shared_ptr message) = 0; - virtual std::shared_ptr create_status_message() const; + virtual std::shared_ptr create_status_message() const = 0; virtual void handle_status_message(std::shared_ptr message) = 0; @@ -321,17 +321,21 @@ class Client : public ClientBase std::shared_ptr create_feedback_message() const override { - using FeedbackMessage = typename ACTION::FeedbackMessage; - return std::shared_ptr(new FeedbackMessage()); + // using FeedbackMessage = typename ACTION::FeedbackMessage; + // return std::shared_ptr(new FeedbackMessage()); + return std::shared_ptr(new Feedback()); } void handle_feedback_message(std::shared_ptr message) override { std::lock_guard guard(goal_handles_mutex_); - using FeedbackMessage = typename ACTION::FeedbackMessage; - typename FeedbackMessage::SharedPtr feedback_message = - std::static_pointer_cast(message); - const GoalID & goal_id = feedback_message->goal_id; + // using FeedbackMessage = typename ACTION::FeedbackMessage; + // typename FeedbackMessage::SharedPtr feedback_message = + // std::static_pointer_cast(message); + typename Feedback::SharedPtr feedback_message = + std::static_pointer_cast(message); + // const GoalID & goal_id = feedback_message->goal_id; + const GoalID & goal_id = feedback_message->uuid; if (goal_handles_.count(goal_id) == 0) { RCLCPP_DEBUG( this->get_logger(), @@ -346,7 +350,8 @@ class Client : public ClientBase return; } const FeedbackCallback & callback = goal_handle->get_feedback_callback(); - callback(goal_handle, feedback_message->feedback); + // callback(goal_handle, feedback_message->feedback); + callback(goal_handle, *feedback_message); } std::shared_ptr create_status_message() const override diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp index c6f3312ba5..442894458f 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -99,7 +99,7 @@ bool ClientGoalHandle::is_feedback_aware() { std::lock_guard guard(handle_mutex_); - return feedback_callback_; + return (feedback_callback_ != nullptr); } template diff --git a/rclcpp_action/include/rclcpp_action/types.hpp b/rclcpp_action/include/rclcpp_action/types.hpp index a74447c844..99889ae91c 100644 --- a/rclcpp_action/include/rclcpp_action/types.hpp +++ b/rclcpp_action/include/rclcpp_action/types.hpp @@ -38,7 +38,7 @@ template<> struct less { bool operator()( const rclcpp_action::GoalID & id0, - const rclcpp_action::GoalID & id1) { + const rclcpp_action::GoalID & id1) const { // return (id0.uuid < id1.uuid); return id0 < id1; } From 4275ce71a6a8a0fd29a422b3a2114c2422ea9616 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Thu, 29 Nov 2018 13:57:55 -0800 Subject: [PATCH 22/56] Fixed lint errors in action headers and client --- rclcpp_action/include/rclcpp_action/client.hpp | 16 +++++++++------- .../include/rclcpp_action/client_goal_handle.hpp | 6 +++--- rclcpp_action/include/rclcpp_action/types.hpp | 8 ++++---- rclcpp_action/src/client.cpp | 9 +++++---- 4 files changed, 21 insertions(+), 18 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 2b5cb4d357..adc0e515b9 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -15,13 +15,6 @@ #ifndef RCLCPP_ACTION__CLIENT_HPP_ #define RCLCPP_ACTION__CLIENT_HPP_ -#include -#include -#include -#include -#include -#include - #include #include #include @@ -31,6 +24,15 @@ #include #include +#include +#include +#include +#include +#include +#include +#include +#include + #include "rclcpp_action/client_goal_handle.hpp" #include "rclcpp_action/types.hpp" #include "rclcpp_action/visibility_control.hpp" diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp index ede3cfe96a..8f84b58f1f 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp @@ -17,13 +17,13 @@ #include +#include + #include #include #include #include -#include - #include "rclcpp_action/types.hpp" #include "rclcpp_action/visibility_control.hpp" @@ -80,7 +80,7 @@ class ClientGoalHandle void set_result(const Result & result); - + void invalidate(); diff --git a/rclcpp_action/include/rclcpp_action/types.hpp b/rclcpp_action/include/rclcpp_action/types.hpp index 99889ae91c..5f7244ffea 100644 --- a/rclcpp_action/include/rclcpp_action/types.hpp +++ b/rclcpp_action/include/rclcpp_action/types.hpp @@ -15,22 +15,22 @@ #ifndef RCLCPP_ACTION__TYPES_HPP_ #define RCLCPP_ACTION__TYPES_HPP_ -#include +#include #include #include -#include +#include namespace rclcpp_action { -//using GoalID = unique_identifier_msgs::msg::UUID; +// using GoalID = unique_identifier_msgs::msg::UUID; using GoalID = std::array; using GoalStatus = action_msgs::msg::GoalStatus; using GoalInfo = action_msgs::msg::GoalInfo; -} // namespace +} // namespace rclcpp_action namespace std { diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index 1122d12193..0cf1f8f448 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -12,14 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "rclcpp_action/client.hpp" +#include +#include #include +#include +#include #include #include -#include -#include +#include "rclcpp_action/client.hpp" namespace rclcpp_action { @@ -123,7 +125,6 @@ ClientBase::ClientBase( ClientBase::~ClientBase() { - } rclcpp::Logger From 9545a0bdc6d88d8c5b2e7372568f746ce83af9d1 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Thu, 29 Nov 2018 14:21:49 -0800 Subject: [PATCH 23/56] Fixes to action client internal workflow. --- .../include/rclcpp_action/client.hpp | 38 ++++++++++--------- 1 file changed, 20 insertions(+), 18 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index adc0e515b9..d51523c1f3 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -188,8 +188,9 @@ class Client : public ClientBase std::shared_future async_send_goal( const Goal & goal, FeedbackCallback callback = nullptr, bool ignore_result = false) { - std::promise promise; - std::shared_future future(promise.get_future()); + // Put promise in the heap to move it around. + auto promise = std::make_shared>; + std::shared_future future(promise->get_future()); using GoalRequest = typename ACTION::GoalRequestService::Request; // auto goal_request = std::make_shared(); // goal_request->goal_id = this->generate_goal_id(); @@ -198,23 +199,22 @@ class Client : public ClientBase goal_request->uuid = this->generate_goal_id(); this->send_goal_request( std::static_pointer_cast(goal_request), - [this, goal_request, - promise{std::move(promise)}, - callback, ignore_result] (std::shared_ptr response) mutable + [this, goal_request, callback, ignore_result, + promise] (std::shared_ptr response) mutable { - using GoalResponse = typename ACTION::GoalService::Response; + using GoalResponse = typename ACTION::GoalRequestService::Response; typename GoalResponse::SharedPtr goal_response = std::static_pointer_cast(response); if (!goal_response->accepted) { - promise->set_exception(std::make_exception_ptr( - exceptions::RejectedGoalError(goal_request->goal))); + promise->set_exception(std::make_exception_ptr(exceptions::RejectedGoalError())); return; } GoalInfo goal_info; // goal_info.goal_id = goal_request->goal_id; goal_info.uuid = goal_request->uuid; goal_info.stamp = goal_response->stamp; - auto goal_handle = std::make_shared(goal_info, callback); + // Do not use std::make_shared as friendship cannot be forwarded. + std::shared_ptr goal_handle(new GoalHandle(goal_info, callback)); if (!ignore_result) { try { this->make_result_aware(goal_handle); @@ -250,14 +250,15 @@ class Client : public ClientBase if (goal_handles_.count(goal_handle->get_goal_id()) == 0) { throw exceptions::UnknownGoalHandleError(goal_handle); } - std::promise promise; - std::shared_future future(promise.get_future()); + // Put promise in the heap to move it around. + auto promise = std::make_shared>(); + std::shared_future future(promise->get_future()); auto cancel_request = std::make_shared(); // cancel_request->goal_info.goal_id = goal_handle->get_goal_id(); cancel_request->goal_info.uuid = goal_handle->get_goal_id(); this->send_cancel_request( std::static_pointer_cast(cancel_request), - [goal_handle, promise{std::move(promise)}] (std::shared_ptr response) mutable + [goal_handle, promise] (std::shared_ptr response) mutable { typename CancelResponse::SharedPtr cancel_response = std::static_pointer_cast(response); @@ -267,7 +268,7 @@ class Client : public ClientBase // goal_canceled = (canceled_goal_info.goal_id == goal_handle->get_goal_id()); goal_canceled = (canceled_goal_info.uuid == goal_handle->get_goal_id()); } - promise.set_value(goal_canceled); + promise->set_value(goal_canceled); }); return future; } @@ -397,7 +398,7 @@ class Client : public ClientBase goal_result_request.uuid = goal_handle->get_goal_id(); this->send_result_request( std::static_pointer_cast(goal_result_request), - [goal_handle] (std::shared_ptr response) + [goal_handle] (std::shared_ptr response) mutable { using GoalResultResponse = typename ACTION::GoalResultService::Response; typename GoalResultResponse::SharedPtr goal_result_response = @@ -410,15 +411,16 @@ class Client : public ClientBase std::shared_future async_cancel(typename CancelRequest::SharedPtr cancel_request) { - std::promise promise; - std::shared_future future(promise.get_future()); + // Put promise in the heap to move it around. + auto promise = std::make_shared>(); + std::shared_future future(promise->get_future()); this->send_cancel_request( std::static_pointer_cast(cancel_request), - [promise{std::move(promise)}] (std::shared_ptr response) mutable + [promise] (std::shared_ptr response) mutable { typename CancelResponse::SharedPtr cancel_response = std::static_pointer_cast(response); - promise.set_value(cancel_response); + promise->set_value(cancel_response); }); return future; } From ab5df90ded4c55a4899b244be9968f7d45171038 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 29 Nov 2018 18:18:29 -0800 Subject: [PATCH 24/56] Misc fixes to get client example to build --- rclcpp_action/include/rclcpp_action/client.hpp | 4 ++-- .../include/rclcpp_action/client_goal_handle.hpp | 6 ++++++ rclcpp_action/include/rclcpp_action/create_client.hpp | 9 +++++++++ 3 files changed, 17 insertions(+), 2 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index d51523c1f3..606ececdc7 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -189,7 +189,7 @@ class Client : public ClientBase const Goal & goal, FeedbackCallback callback = nullptr, bool ignore_result = false) { // Put promise in the heap to move it around. - auto promise = std::make_shared>; + auto promise = std::make_shared>(); std::shared_future future(promise->get_future()); using GoalRequest = typename ACTION::GoalRequestService::Request; // auto goal_request = std::make_shared(); @@ -395,7 +395,7 @@ class Client : public ClientBase using GoalResultRequest = typename ACTION::GoalResultService::Request; auto goal_result_request = std::make_shared(); // goal_result_request.goal_id = goal_handle->get_goal_id(); - goal_result_request.uuid = goal_handle->get_goal_id(); + goal_result_request->uuid = goal_handle->get_goal_id(); this->send_result_request( std::static_pointer_cast(goal_result_request), [goal_handle] (std::shared_ptr response) mutable diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp index 8f84b58f1f..17aef0a6ac 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp @@ -81,6 +81,12 @@ class ClientGoalHandle void set_result(const Result & result); + void + set_result(const typename Result::SharedPtr & result) + { + return set_result(*(result.get())); + } + void invalidate(); diff --git a/rclcpp_action/include/rclcpp_action/create_client.hpp b/rclcpp_action/include/rclcpp_action/create_client.hpp index a7d5762725..b0692875a6 100644 --- a/rclcpp_action/include/rclcpp_action/create_client.hpp +++ b/rclcpp_action/include/rclcpp_action/create_client.hpp @@ -35,6 +35,15 @@ create_client( node->get_node_base_interface(), name); } + +template +typename Client::SharedPtr +create_client( + rclcpp::Node::SharedPtr node, + const std::string & name) +{ + return create_client(node.get(), name); +} } // namespace rclcpp_action #endif // RCLCPP_ACTION__CREATE_CLIENT_HPP_ From 4825f957962d2029c7611b1db49fb42d0e76751e Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 29 Nov 2018 19:39:19 -0800 Subject: [PATCH 25/56] More misck client fixes --- rclcpp_action/include/rclcpp_action/client.hpp | 4 +++- .../include/rclcpp_action/client_goal_handle_impl.hpp | 2 +- rclcpp_action/include/rclcpp_action/create_client.hpp | 4 +++- rclcpp_action/src/client.cpp | 8 +++++--- 4 files changed, 12 insertions(+), 6 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 606ececdc7..928ea31913 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -398,12 +398,14 @@ class Client : public ClientBase goal_result_request->uuid = goal_handle->get_goal_id(); this->send_result_request( std::static_pointer_cast(goal_result_request), - [goal_handle] (std::shared_ptr response) mutable + [goal_handle, this] (std::shared_ptr response) mutable { using GoalResultResponse = typename ACTION::GoalResultService::Response; typename GoalResultResponse::SharedPtr goal_result_response = std::static_pointer_cast(response); goal_handle->set_result(goal_result_response); + std::lock_guard lock(goal_handles_mutex_); + goal_handles_.erase(goal_handle->get_goal_id()); }); goal_handle->set_result_awareness(true); } diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp index 442894458f..64e5cbb9ba 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -25,7 +25,7 @@ namespace rclcpp_action template ClientGoalHandle::ClientGoalHandle( const GoalInfo & info, FeedbackCallback callback) -: info_(info), feedback_callback_(callback) +: info_(info), result_future_(result_promise_.get_future()), feedback_callback_(callback) { } diff --git a/rclcpp_action/include/rclcpp_action/create_client.hpp b/rclcpp_action/include/rclcpp_action/create_client.hpp index b0692875a6..2a8d4de046 100644 --- a/rclcpp_action/include/rclcpp_action/create_client.hpp +++ b/rclcpp_action/include/rclcpp_action/create_client.hpp @@ -31,9 +31,11 @@ create_client( rclcpp::Node * node, const std::string & name) { - return Client::make_shared( + auto action_client = Client::make_shared( node->get_node_base_interface(), name); + node->get_node_waitables_interface()->add_waitable(action_client, nullptr); + return action_client; } template diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index 0cf1f8f448..e09785e87b 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -36,7 +36,8 @@ class ClientBaseImpl const rcl_action_client_options_t & client_options) : node_handle(node_base->get_shared_rcl_node_handle()), logger(rclcpp::get_logger(rcl_node_get_logger_name( - node_handle.get())).get_child("rclcpp")) + node_handle.get())).get_child("rclcpp")), + random_bytes_generator(std::random_device{}()) { std::weak_ptr weak_node_handle(node_handle); client_handle = std::shared_ptr( @@ -198,6 +199,7 @@ ClientBase::handle_goal_response( const rmw_request_id_t & response_header, std::shared_ptr response) { + std::cerr << "handle goal response received xxx\n"; std::lock_guard guard(pimpl_->goal_requests_mutex); const int64_t & sequence_number = response_header.sequence_number; if (pimpl_->pending_goal_responses.count(sequence_number) == 0) { @@ -218,7 +220,7 @@ ClientBase::send_goal_request(std::shared_ptr request, ResponseCallback ca if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send goal request"); } - assert(pimpl_->pending_goal_responses.count(sequence_number) != 0); + assert(pimpl_->pending_goal_responses.count(sequence_number) == 0); pimpl_->pending_goal_responses[sequence_number] = callback; } @@ -247,7 +249,7 @@ ClientBase::send_result_request(std::shared_ptr request, ResponseCallback if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send result request"); } - assert(pimpl_->pending_result_responses.count(sequence_number) != 0); + assert(pimpl_->pending_result_responses.count(sequence_number) == 0); pimpl_->pending_result_responses[sequence_number] = callback; } From f04a50eb769914013d4012dd764e16c91c6845f7 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 29 Nov 2018 19:44:06 -0800 Subject: [PATCH 26/56] Remove debug print --- rclcpp_action/src/client.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index e09785e87b..20e30926f4 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -199,7 +199,6 @@ ClientBase::handle_goal_response( const rmw_request_id_t & response_header, std::shared_ptr response) { - std::cerr << "handle goal response received xxx\n"; std::lock_guard guard(pimpl_->goal_requests_mutex); const int64_t & sequence_number = response_header.sequence_number; if (pimpl_->pending_goal_responses.count(sequence_number) == 0) { From 750e19b320c2371c229e87cea2b0d43f28970f1e Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Sat, 1 Dec 2018 09:02:28 -0800 Subject: [PATCH 27/56] replace logging with throw_from_rcl_error --- rclcpp_action/src/client.cpp | 40 +++++------------------------------- 1 file changed, 5 insertions(+), 35 deletions(-) diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index 20e30926f4..aa832dd028 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -304,13 +304,7 @@ ClientBase::execute() pimpl_->client_handle.get(), feedback_message.get()); pimpl_->is_feedback_ready = false; if (RCL_RET_OK != ret) { - // TODO(hidmic): should throw instead? - RCLCPP_ERROR( - pimpl_->logger, - "Error taking feedback in '%s' action client: %s", - rcl_action_client_get_action_name(pimpl_->client_handle.get()), - rcl_get_error_string().str); - rcl_reset_error(); + rclcpp::exceptions::throw_from_rcl_error(ret, "error taking feedback"); } else { this->handle_feedback_message(feedback_message); } @@ -322,13 +316,7 @@ ClientBase::execute() pimpl_->client_handle.get(), status_message.get()); pimpl_->is_status_ready = false; if (RCL_RET_OK != ret) { - // TODO(hidmic): should throw instead? - RCLCPP_ERROR( - pimpl_->logger, - "Error taking status in '%s' action client: %s", - rcl_action_client_get_action_name(pimpl_->client_handle.get()), - rcl_get_error_string().str); - rcl_reset_error(); + rclcpp::exceptions::throw_from_rcl_error(ret, "error taking status"); } else { this->handle_status_message(status_message); } @@ -341,13 +329,7 @@ ClientBase::execute() pimpl_->client_handle.get(), &response_header, goal_response.get()); pimpl_->is_goal_response_ready = false; if (RCL_RET_OK != ret) { - // TODO(hidmic): should throw instead? - RCLCPP_ERROR( - pimpl_->logger, - "Error taking goal response in '%s' action client: %s", - rcl_action_client_get_action_name(pimpl_->client_handle.get()), - rcl_get_error_string().str); - rcl_reset_error(); + rclcpp::exceptions::throw_from_rcl_error(ret, "error taking goal response"); } else { this->handle_goal_response(response_header, goal_response); } @@ -360,13 +342,7 @@ ClientBase::execute() pimpl_->client_handle.get(), &response_header, result_response.get()); pimpl_->is_result_response_ready = false; if (RCL_RET_OK != ret) { - // TODO(hidmic): should throw instead? - RCLCPP_ERROR( - pimpl_->logger, - "Error taking result response in '%s' action client: %s", - rcl_action_client_get_action_name(pimpl_->client_handle.get()), - rcl_get_error_string().str); - rcl_reset_error(); + rclcpp::exceptions::throw_from_rcl_error(ret, "error taking result response"); } else { this->handle_result_response(response_header, result_response); } @@ -379,13 +355,7 @@ ClientBase::execute() pimpl_->client_handle.get(), &response_header, cancel_response.get()); pimpl_->is_cancel_response_ready = false; if (RCL_RET_OK != ret) { - // TODO(hidmic): should throw instead? - RCLCPP_ERROR( - pimpl_->logger, - "Error taking cancel response in '%s' action client: %s", - rcl_action_client_get_action_name(pimpl_->client_handle.get()), - rcl_get_error_string().str); - rcl_reset_error(); + rclcpp::exceptions::throw_from_rcl_error(ret, "error taking cancel response"); } else { this->handle_cancel_response(response_header, cancel_response); } From af831b3cf67cbc2dfaa2880da3a3cb39322d68bd Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Sat, 1 Dec 2018 10:04:53 -0800 Subject: [PATCH 28/56] Wrap result object given by client to user --- .../include/rclcpp_action/client.hpp | 11 ++++--- .../rclcpp_action/client_goal_handle.hpp | 29 ++++++++++++++----- .../rclcpp_action/client_goal_handle_impl.hpp | 2 +- 3 files changed, 30 insertions(+), 12 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 928ea31913..20172cfe97 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -157,9 +157,9 @@ class Client : public ClientBase RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Client) using Goal = typename ACTION::Goal; - using Result = typename ACTION::Result; using Feedback = typename ACTION::Feedback; using GoalHandle = ClientGoalHandle; + using Result = typename GoalHandle::Result; using FeedbackCallback = std::function; using CancelRequest = typename ACTION::CancelGoalService::Request; @@ -400,10 +400,13 @@ class Client : public ClientBase std::static_pointer_cast(goal_result_request), [goal_handle, this] (std::shared_ptr response) mutable { + // Wrap the response in a struct with the fields a user cares about + Result result; using GoalResultResponse = typename ACTION::GoalResultService::Response; - typename GoalResultResponse::SharedPtr goal_result_response = - std::static_pointer_cast(response); - goal_handle->set_result(goal_result_response); + result.response = std::static_pointer_cast(response); + result.goal_id = goal_handle->get_goal_id(); + result.code = static_cast(result.response->status); + goal_handle->set_result(result); std::lock_guard lock(goal_handles_mutex_); goal_handles_.erase(goal_handle->get_goal_id()); }); diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp index 17aef0a6ac..0127f5e31e 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp @@ -17,6 +17,7 @@ #include +#include #include #include @@ -29,6 +30,16 @@ namespace rclcpp_action { +/// The possible statuses that an action goal can finish with. +enum class ResultCode : int8_t +{ + UNKNOWN = action_msgs::msg::GoalStatus::STATUS_UNKNOWN, + SUCCEEDED = action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, + CANCELED = action_msgs::msg::GoalStatus::STATUS_CANCELED, + ABORTED = action_msgs::msg::GoalStatus::STATUS_ABORTED +}; + + // Forward declarations template class Client; @@ -39,7 +50,17 @@ class ClientGoalHandle public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ClientGoalHandle) - using Result = typename ACTION::Result; + // A wrapper that defines the result of an action + typedef struct Result + { + /// The unique identifier of the goal + GoalID goal_id; + /// A status to indicate if the goal was canceled, aborted, or suceeded + ResultCode code; + /// User defined fields sent back with an action + typename ACTION::Result::SharedPtr response; + } Result; + using Feedback = typename ACTION::Feedback; using FeedbackCallback = std::function; @@ -81,12 +102,6 @@ class ClientGoalHandle void set_result(const Result & result); - void - set_result(const typename Result::SharedPtr & result) - { - return set_result(*(result.get())); - } - void invalidate(); diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp index 64e5cbb9ba..47f88cad0b 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -58,7 +58,7 @@ void ClientGoalHandle::set_result(const Result & result) { std::lock_guard guard(handle_mutex_); - status_ = result.status; + status_ = static_cast(result.code); result_promise_.set_value(result); } From da9d530be80befc1fc567cdb547db068a0804153 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Sat, 1 Dec 2018 10:19:16 -0800 Subject: [PATCH 29/56] Fix a couple bugs trying to cancel goals --- rclcpp_action/include/rclcpp_action/client.hpp | 2 +- rclcpp_action/src/client.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 20172cfe97..8cc17c7a05 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -248,7 +248,7 @@ class Client : public ClientBase async_cancel_goal(typename GoalHandle::SharedPtr goal_handle) { std::lock_guard lock(goal_handles_mutex_); if (goal_handles_.count(goal_handle->get_goal_id()) == 0) { - throw exceptions::UnknownGoalHandleError(goal_handle); + throw exceptions::UnknownGoalHandleError(); } // Put promise in the heap to move it around. auto promise = std::make_shared>(); diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index aa832dd028..722845b16a 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -277,7 +277,7 @@ ClientBase::send_cancel_request(std::shared_ptr request, ResponseCallback if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send cancel request"); } - assert(pimpl_->pending_cancel_responses.count(sequence_number) != 0); + assert(pimpl_->pending_cancel_responses.count(sequence_number) == 0); pimpl_->pending_cancel_responses[sequence_number] = callback; } From 65fa0411ec6b115488f69bf4933c7faee658126a Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 4 Dec 2018 12:13:09 -0800 Subject: [PATCH 30/56] Use unique_indentifier_msgs --- rclcpp_action/include/rclcpp_action/client.hpp | 8 ++++---- .../include/rclcpp_action/client_goal_handle_impl.hpp | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 8cc17c7a05..c65ef7cea6 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -211,7 +211,7 @@ class Client : public ClientBase } GoalInfo goal_info; // goal_info.goal_id = goal_request->goal_id; - goal_info.uuid = goal_request->uuid; + goal_info.goal_id.uuid = goal_request->uuid; goal_info.stamp = goal_response->stamp; // Do not use std::make_shared as friendship cannot be forwarded. std::shared_ptr goal_handle(new GoalHandle(goal_info, callback)); @@ -255,7 +255,7 @@ class Client : public ClientBase std::shared_future future(promise->get_future()); auto cancel_request = std::make_shared(); // cancel_request->goal_info.goal_id = goal_handle->get_goal_id(); - cancel_request->goal_info.uuid = goal_handle->get_goal_id(); + cancel_request->goal_info.goal_id.uuid = goal_handle->get_goal_id(); this->send_cancel_request( std::static_pointer_cast(cancel_request), [goal_handle, promise] (std::shared_ptr response) mutable @@ -266,7 +266,7 @@ class Client : public ClientBase if (!cancel_response->goals_canceling.empty()) { const GoalInfo & canceled_goal_info = cancel_response->goals_canceling[0]; // goal_canceled = (canceled_goal_info.goal_id == goal_handle->get_goal_id()); - goal_canceled = (canceled_goal_info.uuid == goal_handle->get_goal_id()); + goal_canceled = (canceled_goal_info.goal_id.uuid == goal_handle->get_goal_id()); } promise->set_value(goal_canceled); }); @@ -371,7 +371,7 @@ class Client : public ClientBase std::static_pointer_cast(message); for (const GoalStatus & status : status_message->status_list) { // const GoalID & goal_id = status.goal_info.goal_id; - const GoalID & goal_id = status.goal_info.uuid; + const GoalID & goal_id = status.goal_info.goal_id.uuid; if (goal_handles_.count(goal_id) == 0) { RCLCPP_DEBUG( this->get_logger(), diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp index 47f88cad0b..f71b3c80f4 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -39,7 +39,7 @@ const GoalID & ClientGoalHandle::get_goal_id() const { // return info_.goal_id; - return info_.uuid; + return info_.goal_id.uuid; } template From 2faa4b73ea88d95715fe9ebe764d14b00b273733 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 4 Dec 2018 15:30:25 -0800 Subject: [PATCH 31/56] create_client accepts group and removes waitable --- .../include/rclcpp_action/create_client.hpp | 52 +++++++++++++------ rclcpp_action/test/test_client.cpp | 2 +- 2 files changed, 38 insertions(+), 16 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/create_client.hpp b/rclcpp_action/include/rclcpp_action/create_client.hpp index 2a8d4de046..babb975028 100644 --- a/rclcpp_action/include/rclcpp_action/create_client.hpp +++ b/rclcpp_action/include/rclcpp_action/create_client.hpp @@ -25,26 +25,48 @@ namespace rclcpp_action { -template -typename Client::SharedPtr -create_client( - rclcpp::Node * node, - const std::string & name) -{ - auto action_client = Client::make_shared( - node->get_node_base_interface(), - name); - node->get_node_waitables_interface()->add_waitable(action_client, nullptr); - return action_client; -} - template typename Client::SharedPtr create_client( rclcpp::Node::SharedPtr node, - const std::string & name) + const std::string & name, + rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr) { - return create_client(node.get(), name); + std::weak_ptr weak_node = + node->get_node_waitables_interface(); + std::weak_ptr weak_group = group; + bool group_is_null = (nullptr == group.get()); + + auto deleter = [weak_node, weak_group, group_is_null](Client * ptr) + { + if (nullptr == ptr) { + return; + } + auto shared_node = weak_node.lock(); + if (!shared_node) { + return; + } + // API expects a shared pointer, give it one with a deleter that does nothing. + std::shared_ptr> fake_shared_ptr(ptr, [] (Client *) {}); + + if (group_is_null) { + // Was added to default group + shared_node->remove_waitable(fake_shared_ptr, nullptr); + } else { + // Was added to a specfic group + auto shared_group = weak_group.lock(); + if (shared_group) { + shared_node->remove_waitable(fake_shared_ptr, shared_group); + } + } + delete ptr; + }; + + std::shared_ptr> action_client( + new Client(node->get_node_base_interface(), name), deleter); + + node->get_node_waitables_interface()->add_waitable(action_client, group); + return action_client; } } // namespace rclcpp_action diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index 72cb792a47..984b81fceb 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -37,6 +37,6 @@ TEST_F(TestClient, construction_and_destruction) { auto node = std::make_shared("my_node", "/rclcpp_action/test/client"); - auto ac = rclcpp_action::create_client(node.get(), "fibonacci"); + auto ac = rclcpp_action::create_client(node, "fibonacci"); (void)ac; } From 7fa9a14296ff6188032b7998830c28959baa6dbb Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 4 Dec 2018 15:44:30 -0800 Subject: [PATCH 32/56] Uncrustify fixes --- .../include/rclcpp_action/client.hpp | 41 +++++++++---------- .../rclcpp_action/client_goal_handle.hpp | 3 +- .../rclcpp_action/client_goal_handle_impl.hpp | 2 +- .../include/rclcpp_action/create_client.hpp | 2 +- rclcpp_action/include/rclcpp_action/types.hpp | 16 ++++---- rclcpp_action/src/client.cpp | 20 ++++----- 6 files changed, 38 insertions(+), 46 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index c65ef7cea6..ae3c57c72c 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -92,8 +92,7 @@ class ClientBase : public rclcpp::Waitable execute() override; protected: - using ResponseCallback = - std::function response)>; + using ResponseCallback = std::function response)>; RCLCPP_ACTION_PUBLIC rclcpp::Logger get_logger(); @@ -160,8 +159,7 @@ class Client : public ClientBase using Feedback = typename ACTION::Feedback; using GoalHandle = ClientGoalHandle; using Result = typename GoalHandle::Result; - using FeedbackCallback = std::function; + using FeedbackCallback = std::function; using CancelRequest = typename ACTION::CancelGoalService::Request; using CancelResponse = typename ACTION::CancelGoalService::Response; @@ -199,12 +197,11 @@ class Client : public ClientBase goal_request->uuid = this->generate_goal_id(); this->send_goal_request( std::static_pointer_cast(goal_request), - [this, goal_request, callback, ignore_result, - promise] (std::shared_ptr response) mutable + [this, goal_request, callback, ignore_result, promise]( + std::shared_ptr response) mutable { using GoalResponse = typename ACTION::GoalRequestService::Response; - typename GoalResponse::SharedPtr goal_response = - std::static_pointer_cast(response); + auto goal_response = std::static_pointer_cast(response); if (!goal_response->accepted) { promise->set_exception(std::make_exception_ptr(exceptions::RejectedGoalError())); return; @@ -232,7 +229,8 @@ class Client : public ClientBase RCLCPP_ACTION_PUBLIC std::shared_future - async_get_result(typename GoalHandle::SharedPtr goal_handle) { + async_get_result(typename GoalHandle::SharedPtr goal_handle) + { std::lock_guard lock(goal_handles_mutex_); if (goal_handles_.count(goal_handle->get_goal_id()) == 0) { throw exceptions::UnknownGoalHandleError(goal_handle); @@ -245,7 +243,8 @@ class Client : public ClientBase RCLCPP_ACTION_PUBLIC std::shared_future - async_cancel_goal(typename GoalHandle::SharedPtr goal_handle) { + async_cancel_goal(typename GoalHandle::SharedPtr goal_handle) + { std::lock_guard lock(goal_handles_mutex_); if (goal_handles_.count(goal_handle->get_goal_id()) == 0) { throw exceptions::UnknownGoalHandleError(); @@ -258,10 +257,9 @@ class Client : public ClientBase cancel_request->goal_info.goal_id.uuid = goal_handle->get_goal_id(); this->send_cancel_request( std::static_pointer_cast(cancel_request), - [goal_handle, promise] (std::shared_ptr response) mutable + [goal_handle, promise](std::shared_ptr response) mutable { - typename CancelResponse::SharedPtr cancel_response = - std::static_pointer_cast(response); + auto cancel_response = std::static_pointer_cast(response); bool goal_canceled = false; if (!cancel_response->goals_canceling.empty()) { const GoalInfo & canceled_goal_info = cancel_response->goals_canceling[0]; @@ -367,8 +365,7 @@ class Client : public ClientBase { std::lock_guard guard(goal_handles_mutex_); using GoalStatusMessage = typename ACTION::GoalStatusMessage; - typename GoalStatusMessage::SharedPtr status_message = - std::static_pointer_cast(message); + auto status_message = std::static_pointer_cast(message); for (const GoalStatus & status : status_message->status_list) { // const GoalID & goal_id = status.goal_info.goal_id; const GoalID & goal_id = status.goal_info.goal_id.uuid; @@ -384,21 +381,22 @@ class Client : public ClientBase if ( goal_status == GoalStatus::STATUS_SUCCEEDED || goal_status == GoalStatus::STATUS_CANCELED || - goal_status == GoalStatus::STATUS_ABORTED - ) { + goal_status == GoalStatus::STATUS_ABORTED) + { goal_handles_.erase(goal_id); } } } - void make_result_aware(typename GoalHandle::SharedPtr goal_handle) { + void make_result_aware(typename GoalHandle::SharedPtr goal_handle) + { using GoalResultRequest = typename ACTION::GoalResultService::Request; auto goal_result_request = std::make_shared(); // goal_result_request.goal_id = goal_handle->get_goal_id(); goal_result_request->uuid = goal_handle->get_goal_id(); this->send_result_request( std::static_pointer_cast(goal_result_request), - [goal_handle, this] (std::shared_ptr response) mutable + [goal_handle, this](std::shared_ptr response) mutable { // Wrap the response in a struct with the fields a user cares about Result result; @@ -421,10 +419,9 @@ class Client : public ClientBase std::shared_future future(promise->get_future()); this->send_cancel_request( std::static_pointer_cast(cancel_request), - [promise] (std::shared_ptr response) mutable + [promise](std::shared_ptr response) mutable { - typename CancelResponse::SharedPtr cancel_response = - std::static_pointer_cast(response); + auto cancel_response = std::static_pointer_cast(response); promise->set_value(cancel_response); }); return future; diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp index 0127f5e31e..4f56a14352 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp @@ -62,8 +62,7 @@ class ClientGoalHandle } Result; using Feedback = typename ACTION::Feedback; - using FeedbackCallback = std::function; + using FeedbackCallback = std::function; virtual ~ClientGoalHandle(); diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp index f71b3c80f4..39fcc26e95 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -99,7 +99,7 @@ bool ClientGoalHandle::is_feedback_aware() { std::lock_guard guard(handle_mutex_); - return (feedback_callback_ != nullptr); + return feedback_callback_ != nullptr; } template diff --git a/rclcpp_action/include/rclcpp_action/create_client.hpp b/rclcpp_action/include/rclcpp_action/create_client.hpp index babb975028..7e385c0a82 100644 --- a/rclcpp_action/include/rclcpp_action/create_client.hpp +++ b/rclcpp_action/include/rclcpp_action/create_client.hpp @@ -47,7 +47,7 @@ create_client( return; } // API expects a shared pointer, give it one with a deleter that does nothing. - std::shared_ptr> fake_shared_ptr(ptr, [] (Client *) {}); + std::shared_ptr> fake_shared_ptr(ptr, [](Client *) {}); if (group_is_null) { // Was added to default group diff --git a/rclcpp_action/include/rclcpp_action/types.hpp b/rclcpp_action/include/rclcpp_action/types.hpp index 5f7244ffea..012b99df1d 100644 --- a/rclcpp_action/include/rclcpp_action/types.hpp +++ b/rclcpp_action/include/rclcpp_action/types.hpp @@ -23,8 +23,8 @@ #include -namespace rclcpp_action { - +namespace rclcpp_action +{ // using GoalID = unique_identifier_msgs::msg::UUID; using GoalID = std::array; using GoalStatus = action_msgs::msg::GoalStatus; @@ -32,18 +32,18 @@ using GoalInfo = action_msgs::msg::GoalInfo; } // namespace rclcpp_action -namespace std { - +namespace std +{ template<> -struct less { +struct less +{ bool operator()( const rclcpp_action::GoalID & id0, - const rclcpp_action::GoalID & id1) const { + const rclcpp_action::GoalID & id1) const + { // return (id0.uuid < id1.uuid); return id0 < id1; } }; - } // namespace std - #endif // RCLCPP_ACTION__TYPES_HPP_ diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index 722845b16a..9b3d27c507 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -35,9 +35,8 @@ class ClientBaseImpl const rosidl_action_type_support_t * type_support, const rcl_action_client_options_t & client_options) : node_handle(node_base->get_shared_rcl_node_handle()), - logger(rclcpp::get_logger(rcl_node_get_logger_name( - node_handle.get())).get_child("rclcpp")), - random_bytes_generator(std::random_device{}()) + logger(rclcpp::get_logger(rcl_node_get_logger_name(node_handle.get())).get_child("rclcpp")), + random_bytes_generator(std::random_device{} ()) { std::weak_ptr weak_node_handle(node_handle); client_handle = std::shared_ptr( @@ -97,8 +96,7 @@ class ClientBaseImpl std::shared_ptr node_handle{nullptr}; rclcpp::Logger logger; - using ResponseCallback = - std::function response)>; + using ResponseCallback = std::function response)>; std::map pending_goal_responses; std::mutex goal_requests_mutex; @@ -118,9 +116,7 @@ ClientBase::ClientBase( const std::string & action_name, const rosidl_action_type_support_t * type_support, const rcl_action_client_options_t & client_options) - : pimpl_(new ClientBaseImpl( - node_base, action_name, - type_support, client_options)) +: pimpl_(new ClientBaseImpl(node_base, action_name, type_support, client_options)) { } @@ -168,8 +164,8 @@ bool ClientBase::add_to_wait_set(rcl_wait_set_t * wait_set) { rcl_ret_t ret = rcl_action_wait_set_add_action_client( - wait_set, pimpl_->client_handle.get(), nullptr, nullptr); - return (RCL_RET_OK == ret); + wait_set, pimpl_->client_handle.get(), nullptr, nullptr); + return RCL_RET_OK == ret; } bool @@ -186,12 +182,12 @@ ClientBase::is_ready(rcl_wait_set_t * wait_set) rclcpp::exceptions::throw_from_rcl_error( ret, "failed to check for any ready entities"); } - return ( + return pimpl_->is_feedback_ready || pimpl_->is_status_ready || pimpl_->is_goal_response_ready || pimpl_->is_cancel_response_ready || - pimpl_->is_result_response_ready); + pimpl_->is_result_response_ready; } void From 7c2c69eeca5ff3afd1661096f9d54cfa8ce9a604 Mon Sep 17 00:00:00 2001 From: Samuel Servulo Date: Wed, 28 Nov 2018 20:38:10 -0200 Subject: [PATCH 33/56] [rclcpp_action] Adds tests for action client. --- .../include/rclcpp_action/client.hpp | 2 +- .../rclcpp_action/client_goal_handle.hpp | 3 + .../rclcpp_action/client_goal_handle_impl.hpp | 8 + .../include/rclcpp_action/exceptions.hpp | 9 + rclcpp_action/src/client.cpp | 6 +- rclcpp_action/test/test_client.cpp | 358 +++++++++++++++++- 6 files changed, 380 insertions(+), 6 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index ae3c57c72c..8931b6d531 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -233,7 +233,7 @@ class Client : public ClientBase { std::lock_guard lock(goal_handles_mutex_); if (goal_handles_.count(goal_handle->get_goal_id()) == 0) { - throw exceptions::UnknownGoalHandleError(goal_handle); + throw exceptions::UnknownGoalHandleError(); } if (!goal_handle->is_result_aware()) { this->make_result_aware(goal_handle); diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp index 4f56a14352..613a8de066 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -68,6 +69,8 @@ class ClientGoalHandle const GoalID & get_goal_id() const; + const rclcpp::Time & get_goal_stamp() const; + std::shared_future async_result(); diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp index 39fcc26e95..77248eb3ce 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -42,6 +42,14 @@ ClientGoalHandle::get_goal_id() const return info_.goal_id.uuid; } +template +const rclcpp::Time & +ClientGoalHandle::get_goal_stamp() const +{ + return info_.stamp; +} + + template std::shared_future::Result> ClientGoalHandle::async_result() diff --git a/rclcpp_action/include/rclcpp_action/exceptions.hpp b/rclcpp_action/include/rclcpp_action/exceptions.hpp index 777182554a..f6480935a4 100644 --- a/rclcpp_action/include/rclcpp_action/exceptions.hpp +++ b/rclcpp_action/include/rclcpp_action/exceptions.hpp @@ -31,6 +31,15 @@ class RejectedGoalError : public std::runtime_error } }; +class DuplicatedGoalUuidError : public std::invalid_argument +{ +public: + DuplicatedGoalUuidError() + : std::invalid_argument("") + { + } +}; + class UnknownGoalHandleError : public std::invalid_argument { public: diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index 9b3d27c507..db0beecec6 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -22,6 +22,7 @@ #include #include "rclcpp_action/client.hpp" +#include "rclcpp_action/exceptions.hpp" namespace rclcpp_action { @@ -215,7 +216,10 @@ ClientBase::send_goal_request(std::shared_ptr request, ResponseCallback ca if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send goal request"); } - assert(pimpl_->pending_goal_responses.count(sequence_number) == 0); + if(pimpl_->pending_goal_responses.count(sequence_number) != 0) + { + throw exceptions::DuplicatedGoalUuidError(); + } pimpl_->pending_goal_responses[sequence_number] = callback; } diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index 984b81fceb..42b97593f7 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -14,29 +14,379 @@ #include +#include +#include +#include + +#include + +#include #include #include +#include #include +#include +#include + #include +#include #include +#include +#include "rclcpp_action/exceptions.hpp" #include "rclcpp_action/create_client.hpp" #include "rclcpp_action/client.hpp" +#include "rclcpp_action/types.hpp" -class TestClient : public ::testing::Test +class TestActionClient : public ::testing::Test { protected: + using ActionType = test_msgs::action::Fibonacci; + using ActionGoal = ActionType::Goal; + using ActionGoalHandle = rclcpp_action::ClientGoalHandle; + using ActionGoalRequestService = ActionType::GoalRequestService; + using ActionGoalRequest = ActionGoalRequestService::Request; + using ActionGoalResponse = ActionGoalRequestService::Response; + using ActionGoalResultService = ActionType::GoalRequestService; + using ActionGoalResultRequest = ActionGoalResultService::Request; + using ActionGoalResultResponse = ActionGoalResultService::Response; + using ActionCancelGoalService = ActionType::CancelGoalService; + using ActionCancelGoalRequest = ActionType::CancelGoalService::Request; + using ActionCancelGoalResponse = ActionType::CancelGoalService::Response; + using ActionStatusMessage = ActionType::GoalStatusMessage; + using ActionFeedbackMessage = ActionType::Feedback; + using ActionFeedback = ActionType::Feedback; + static void SetUpTestCase() { rclcpp::init(0, nullptr); } + + void SetUp() + { + node = std::make_shared(node_name, namespace_name); + rcl_allocator_t allocator = rcl_get_default_allocator(); + + char * goal_service_name = nullptr; + rcl_ret_t ret = rcl_action_get_goal_service_name( + action_name, allocator, &goal_service_name); + ASSERT_EQ(RCL_RET_OK, ret); + goal_service = node->create_service( + goal_service_name, + [this] ( + const ActionGoalRequest::SharedPtr request, + const ActionGoalResponse::SharedPtr response) + { + response->stamp = clock.now(); + response->accepted = (request->goal.order >= 0); + if (response->accepted) { + goals[request->uuid] = {request, response}; + } + }); + ASSERT_FALSE(goal_service != nullptr); + allocator.deallocate(goal_service_name, allocator.state); + + char * result_service_name = nullptr; + ret = rcl_action_get_result_service_name( + action_name, allocator, &result_service_name); + ASSERT_EQ(RCL_RET_OK, ret); + result_service = node->create_service( + result_service_name, + [this]( + const ActionGoalResultRequest::SharedPtr request, + const ActionGoalResultResponse::SharedPtr response) + { + if (goals.count(request->uuid) == 1) { + auto goal_request = goals[request->uuid].first; + auto goal_response = goals[request->uuid].second; + ActionStatusMessage status_message; + rclcpp_action::GoalStatus goal_status; + goal_status.goal_info.uuid = goal_request->uuid; + goal_status.goal_info.stamp = goal_response->stamp; + goal_status.status = rclcpp_action::GoalStatus::STATUS_EXECUTING; + status_message.status_list.push_back(goal_status); + status_publisher->publish(status_message): + ActionFeedbackMessage feedback_message; + feedback_message->uuid = goal_request->uuid; + response->sequence.push_back(0); + feedback_message->sequence = response->sequence; + feedback_publisher->publish(feedback_message); + if (request->order > 0) + { + response->sequence.push_back(1); + feedback_message->sequence = response->sequence; + feedback_publisher->publish(feedback_message); + for (int i = 1; i < goal_request->order; ++i) + { + response->sequence.push_back(response->sequence[i] + response->sequence[i - 1]); + feedback_message->sequence = response->sequence; + feedback_publisher->publish(feedback_message); + } + } + response->status = rclcpp_action::GoalStatus::STATUS_SUCCEEDED; + status_message.status_list.back().status = rclcpp_action::GoalStatus::STATUS_SUCCEEDED; + status_publisher->publish(status_message): + goals.erase(request->uuid); + } else { + response->status = rclcpp_action::GoalStatus::STATUS_UNKNOWN; + } + }); + ASSERT_TRUE(result_service != nullptr); + allocator.deallocate(result_service_name, allocator.state); + + char * cancel_service_name = nullptr; + ret = rcl_action_get_cancel_service_name( + action_name, allocator, &cancel_service_name); + ASSERT_EQ(RCL_RET_OK, ret); + cancel_service = node->create_service( + cancel_service_name, + [this] ( + const ActionCancelRequest::SharedPtr request, + const ActionCancelResponse::SharedPtr response) mutable + { + ActionStatusMessage status_message; + bool cancel_all = ( + request->goal_info.stamp == zero_stamp && + request->goal_info.uuid == zero_uuid); + auto it = goals.begin(); + while (it != goals.end()) { + auto goal_request = it->second.first; + auto goal_response = it->second.second; + bool cancel_this = ( + request->goal_info.uuid == goal_request->uuid || + request->goal_info.stamp > goal_response->stamp); + if (cancel_all || cancel_this) + { + rclcpp_action::GoalStatus goal_status; + goal_status.goal_info.uuid = goal_request->uuid; + goal_status.goal_info.stamp = goal_response->stamp; + goal_status.status = rclcpp_action::GoalStatus::STATUS_CANCELED; + status_message.status_list.push_back(goal_status); + response->goals_canceling.push_back(goal_status.goal_info); + it = goals.erase(it); + } + } + status_publisher->publish(status_message); + }); + ASSERT_TRUE(cancel_service != nullptr); + allocator.deallocate(cancel_service_name, allocator.state); + + char * feedback_topic_name = nullptr; + ret = rcl_action_get_feedback_topic_name( + action_name, allocator, &feedback_topic_name); + ASSERT_EQ(RCL_RET_OK, ret); + feedback_publisher = node->create_publisher(feedback_topic_name); + ASSERT_TRUE(feedback_publisher != nullptr); + allocator.deallocate(feedback_topic_name, allocator.state); + + char * status_topic_name = nullptr; + ret = rcl_action_get_status_topic_name( + action_name, allocator, &status_topic_name); + ASSERT_EQ(RCL_RET_OK, ret); + status_publisher = node->create_publisher(status_topic_name); + ASSERT_TRUE(status_publisher != nullptr); + allocator.deallocate(status_topic_name, allocator.state); + + ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(clock.get_clock_handle())); + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(1))); + } + + void Teardown() + { + status_publisher.reset(); + feedback_publisher.reset(); + cancel_service.reset(); + result_service.reset(); + goal_servicer.reset(); + node.reset(); + } + + rclcpp::Node::SharedPtr node; + rclcpp::Clock clock{RCL_ROS_TIME}; + const char * const node_name{"fibonacci_action_test_node"}; + const char * const namespace_name{"/rclcpp_action/test/client"}; + const char * const action_name{"fibonacci_test"}; + typename rclcpp::Service::SharedPtr goal_service; + typename rclcpp::Service::SharedPtr result_service; + typename rclcpp::Service::SharedPtr cancel_service; + typename rclcpp::Publisher::SharedPtr feedback_publisher; + typename rclcpp::Publisher::SharedPtr status_publisher; + std::map goals; }; TEST_F(TestClient, construction_and_destruction) { - auto node = std::make_shared("my_node", "/rclcpp_action/test/client"); + ASSERT_NO_THROW(rclcpp_action::create_client(node, action_name).reset()); +} + +TEST_F(TestClient, async_send_goal_but_ignore_feedback_and_result) +{ + auto action_client = rclcpp_action::create_client(node, action_name); + + ActionGoal bad_goal; + bad_goal.order = -5; + auto future_goal_handle = action_client->async_send_goal(bad_goal, nullptr, true); + ASSERT_EQ( + rclcpp::executor::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future)); + EXPECT_THROW(future.get(), rclcpp_action::exceptions::RejectedGoalError); + + ActionGoal good_goal; + good_goal.order = 5; + future_goal_handle = action_client->async_send_goal(good_goal, nullptr, true); + ASSERT_EQ( + rclcpp::executor::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future)); + auto goal_handle = future.get(); + EXPECT_EQ(GoalStatus::ACCEPTED, goal_handle->get_status()); + EXPECT_FALSE(goal_handle->is_feedback_aware()); + EXPECT_FALSE(goal_handle->is_result_aware()); + EXPECT_THROW(goal_handle->async_result(), rclcpp_action::exceptions::UnawareGoalHandleError); +} + +TEST_F(TestClient, async_send_goal_and_ignore_feedback_but_wait_for_result) +{ + auto action_client = rclcpp_action::create_client(node, action_name); + + ActionGoal goal; + goal.order = 5; + auto future_goal_handle = action_client->async_send_goal(goal); + ASSERT_EQ( + rclcpp::executor::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future_goal_handle)); + auto goal_handle = future_goal_handle.get(); + EXPECT_EQ(GoalStatus::ACCEPTED, goal_handle->get_status()); + EXPECT_FALSE(goal_handle->is_feedback_aware()); + EXPECT_TRUE(goal_handle->is_result_aware()); + + auto future_result = goal_handle->async_result(); + ASSERT_EQ( + rclcpp::executor::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future_result)); + auto result = future_result.get(); + ASSERT_EQ(6, result->response.sequence.size()); + EXPECT_EQ(0, result->response.sequence[0]); + EXPECT_EQ(1, result->response.sequence[1]); + EXPECT_EQ(5, result->response.sequence[5]); +} + +TEST_F(TestClient, async_send_goal_with_feedback_and_result) +{ + auto action_client = rclcpp_action::create_client(node, action_name); + + ActionGoal goal; + goal.order = 4; + int feedback_count = 0; + auto future_goal_handle = action_client->async_send_goal( + goal, + [&feedback_count] ( + typename ActionGoalHandle::SharedPtr goal_handle, + const ActionFeedback & feedback) mutable + { + feedback_count++; + }); + + ASSERT_EQ( + rclcpp::executor::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future_goal_handle)); + auto goal_handle = future_goal_handle.get(); + EXPECT_EQ(GoalStatus::ACCEPTED, goal_handle->get_status()); + EXPECT_TRUE(goal_handle->is_feedback_aware()); + EXPECT_TRUE(goal_handle->is_result_aware()); + + auto future_result = goal_handle->async_result(); + ASSERT_EQ( + rclcpp::executor::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future_result)); + auto result = future_result.get(); + ASSERT_EQ(5, result->response.sequence.size()); + EXPECT_EQ(3, result->response.sequence.back()); + EXPECT_EQ(5, feedback_count); +} + +TEST_F(TestClient, async_cancel_one_goal) +{ + auto action_client = rclcpp_action::create_client(node, action_name); + + ActionGoal goal; + goal.order = 5; + auto future_goal_handle = action_client->async_send_goal(goal, nullptr, true); + ASSERT_EQ( + rclcpp::executor::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future_goal_handle)); + auto goal_handle = future_goal_handle.get(); + EXPECT_EQ(rclcpp_action::GoalStatus::ACCEPTED, goal_handle->get_status()); + + auto future_cancel = action_client->async_cancel_goal(goal_handle); + ASSERT_EQ( + rclcpp::executor::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future_cancel_all)); + auto cancel_response = future_cancel.get(); + ASSERT_EQ(1, cancel_response->goals_canceling.size()); + const rclcpp_action::GoalInfo & canceled_goal_info = cancel_response->goals_canceling[0]; + EXPECT_EQ(canceled_goal_info.uuid, goal_handle->get_goal_id()); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle->get_status()); +} + +TEST_F(TestClient, async_cancel_all_goals) +{ + auto action_client = rclcpp_action::create_client(node, action_name); + + ActionGoal goal; + goal.order = 6; + auto future_goal_handle0 = action_client->async_send_goal(goal, nullptr, true); + ASSERT_EQ( + rclcpp::executor::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future_goal_handle0)); + auto goal_handle0 = future_goal_handle0.get(); + + goal.order = 8; + auto future_goal_handle1 = action_client->async_send_goal(goal, nullptr, true); + ASSERT_EQ( + rclcpp::executor::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future_goal_handle1)); + auto goal_handle1 = future_goal_handle1.get(); + + auto future_cancel_all = action_client->async_cancel_all_goals(); + ASSERT_EQ( + rclcpp::executor::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future_cancel_all)); + auto cancel_response = future_cancel_all.get(); + + ASSERT_EQ(2, cancel_response->goals_canceling.size()); + EXPECT_EQ(goal_handle0->get_goal_id(), cancel_response->goals_canceling[0].uuid); + EXPECT_EQ(goal_handle1->get_goal_id(), cancel_response->goals_canceling[1].uuid); +} + +TEST_F(TestClient, async_cancel_some_goals) +{ + auto action_client = rclcpp_action::create_client(node, action_name); + + ActionGoal goal; + goal.order = 6; + auto future_goal_handle0 = action_client->async_send_goal(goal, nullptr, true); + ASSERT_EQ( + rclcpp::executor::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future_goal_handle0)); + auto goal_handle0 = future_goal_handle0.get(); + + // Move time forward + 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, nullptr, true); + ASSERT_EQ( + rclcpp::executor::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future_goal_handle1)); + auto goal_handle1 = future_goal_handle1.get(); + + auto future_cancel_some = action_client->async_cancel_goals_before(goal_handle1->get_goal_stamp()); + ASSERT_EQ( + rclcpp::executor::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future_cancel_some)); + auto cancel_response = future_cancel_some.get(); - auto ac = rclcpp_action::create_client(node, "fibonacci"); - (void)ac; + ASSERT_EQ(1, cancel_response->goals_canceling.size()); + EXPECT_EQ(goal_handle0->get_goal_id(), cancel_response->goals_canceling[0].uuid); } From 927115cbc84b9d0e5243329674ceb56c8ff68888 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 4 Dec 2018 20:16:42 -0800 Subject: [PATCH 34/56] [WIP] Failing action client tests. --- .../include/rclcpp_action/client.hpp | 8 +- .../rclcpp_action/client_goal_handle.hpp | 4 +- .../rclcpp_action/client_goal_handle_impl.hpp | 2 +- .../include/rclcpp_action/exceptions.hpp | 15 +- rclcpp_action/include/rclcpp_action/types.hpp | 11 +- rclcpp_action/src/client.cpp | 5 +- rclcpp_action/test/test_client.cpp | 241 ++++++++++-------- 7 files changed, 150 insertions(+), 136 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 8931b6d531..c339932ded 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -277,7 +277,9 @@ class Client : public ClientBase { auto cancel_request = std::make_shared(); // std::fill(cancel_request->goal_info.goal_id.uuid, 0u); - std::fill(cancel_request->goal_info.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); } @@ -287,7 +289,9 @@ class Client : public ClientBase { auto cancel_request = std::make_shared(); // std::fill(cancel_request->goal_info.goal_id.uuid, 0u); - std::fill(cancel_request->goal_info.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); } diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp index 613a8de066..e9486656c0 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp @@ -69,8 +69,8 @@ class ClientGoalHandle const GoalID & get_goal_id() const; - const rclcpp::Time & get_goal_stamp() const; - + rclcpp::Time get_goal_stamp() const; + std::shared_future async_result(); diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp index 77248eb3ce..c398e43dff 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -43,7 +43,7 @@ ClientGoalHandle::get_goal_id() const } template -const rclcpp::Time & +rclcpp::Time ClientGoalHandle::get_goal_stamp() const { return info_.stamp; diff --git a/rclcpp_action/include/rclcpp_action/exceptions.hpp b/rclcpp_action/include/rclcpp_action/exceptions.hpp index f6480935a4..4263d50b55 100644 --- a/rclcpp_action/include/rclcpp_action/exceptions.hpp +++ b/rclcpp_action/include/rclcpp_action/exceptions.hpp @@ -26,16 +26,7 @@ class RejectedGoalError : public std::runtime_error { public: RejectedGoalError() - : std::runtime_error("") - { - } -}; - -class DuplicatedGoalUuidError : public std::invalid_argument -{ -public: - DuplicatedGoalUuidError() - : std::invalid_argument("") + : std::runtime_error("Goal was rejected by the server.") { } }; @@ -44,7 +35,7 @@ class UnknownGoalHandleError : public std::invalid_argument { public: UnknownGoalHandleError() - : std::invalid_argument("") + : std::invalid_argument("Goal handle is not know to this client.") { } }; @@ -53,7 +44,7 @@ class UnawareGoalHandleError : public std::runtime_error { public: UnawareGoalHandleError() - : std::runtime_error("") + : std::runtime_error("Goal handle is not tracking the goal result.") { } }; diff --git a/rclcpp_action/include/rclcpp_action/types.hpp b/rclcpp_action/include/rclcpp_action/types.hpp index 012b99df1d..3ab9f4ce19 100644 --- a/rclcpp_action/include/rclcpp_action/types.hpp +++ b/rclcpp_action/include/rclcpp_action/types.hpp @@ -25,8 +25,8 @@ namespace rclcpp_action { -// using GoalID = unique_identifier_msgs::msg::UUID; -using GoalID = std::array; + +using GoalID = std::array; using GoalStatus = action_msgs::msg::GoalStatus; using GoalInfo = action_msgs::msg::GoalInfo; @@ -38,11 +38,10 @@ template<> struct less { bool operator()( - const rclcpp_action::GoalID & id0, - const rclcpp_action::GoalID & id1) const + const rclcpp_action::GoalID & lhs, + const rclcpp_action::GoalID & rhs) const { - // return (id0.uuid < id1.uuid); - return id0 < id1; + return lhs < rhs; } }; } // namespace std diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index db0beecec6..506f6e9413 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -216,10 +216,7 @@ ClientBase::send_goal_request(std::shared_ptr request, ResponseCallback ca if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send goal request"); } - if(pimpl_->pending_goal_responses.count(sequence_number) != 0) - { - throw exceptions::DuplicatedGoalUuidError(); - } + assert(pimpl_->pending_goal_responses.count(sequence_number) == 0); pimpl_->pending_goal_responses[sequence_number] = callback; } diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index 42b97593f7..7e10149b32 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -22,6 +22,8 @@ #include #include +#include +#include #include #include #include @@ -33,13 +35,18 @@ #include #include #include +#include +#include +#include #include "rclcpp_action/exceptions.hpp" #include "rclcpp_action/create_client.hpp" #include "rclcpp_action/client.hpp" #include "rclcpp_action/types.hpp" -class TestActionClient : public ::testing::Test +using namespace std::chrono_literals; + +class TestClient : public ::testing::Test { protected: using ActionType = test_msgs::action::Fibonacci; @@ -48,7 +55,7 @@ class TestActionClient : public ::testing::Test using ActionGoalRequestService = ActionType::GoalRequestService; using ActionGoalRequest = ActionGoalRequestService::Request; using ActionGoalResponse = ActionGoalRequestService::Response; - using ActionGoalResultService = ActionType::GoalRequestService; + using ActionGoalResultService = ActionType::GoalResultService; using ActionGoalResultRequest = ActionGoalResultService::Request; using ActionGoalResultResponse = ActionGoalResultService::Response; using ActionCancelGoalService = ActionType::CancelGoalService; @@ -65,68 +72,71 @@ class TestActionClient : public ::testing::Test void SetUp() { - node = std::make_shared(node_name, namespace_name); + server_node = std::make_shared(server_node_name, namespace_name); rcl_allocator_t allocator = rcl_get_default_allocator(); char * goal_service_name = nullptr; rcl_ret_t ret = rcl_action_get_goal_service_name( action_name, allocator, &goal_service_name); ASSERT_EQ(RCL_RET_OK, ret); - goal_service = node->create_service( + goal_service = server_node->create_service( goal_service_name, - [this] ( + [this]( const ActionGoalRequest::SharedPtr request, - const ActionGoalResponse::SharedPtr response) + ActionGoalResponse::SharedPtr response) { response->stamp = clock.now(); - response->accepted = (request->goal.order >= 0); + response->accepted = (request->order >= 0); if (response->accepted) { goals[request->uuid] = {request, response}; } }); - ASSERT_FALSE(goal_service != nullptr); + ASSERT_TRUE(goal_service != nullptr); allocator.deallocate(goal_service_name, allocator.state); char * result_service_name = nullptr; ret = rcl_action_get_result_service_name( action_name, allocator, &result_service_name); ASSERT_EQ(RCL_RET_OK, ret); - result_service = node->create_service( + result_service = server_node->create_service( result_service_name, [this]( const ActionGoalResultRequest::SharedPtr request, - const ActionGoalResultResponse::SharedPtr response) + ActionGoalResultResponse::SharedPtr response) { if (goals.count(request->uuid) == 1) { auto goal_request = goals[request->uuid].first; auto goal_response = goals[request->uuid].second; ActionStatusMessage status_message; rclcpp_action::GoalStatus goal_status; - goal_status.goal_info.uuid = goal_request->uuid; + goal_status.goal_info.goal_id.uuid = goal_request->uuid; goal_status.goal_info.stamp = goal_response->stamp; goal_status.status = rclcpp_action::GoalStatus::STATUS_EXECUTING; status_message.status_list.push_back(goal_status); - status_publisher->publish(status_message): + status_publisher->publish(status_message); + // executor.spin_once(); ActionFeedbackMessage feedback_message; - feedback_message->uuid = goal_request->uuid; + feedback_message.uuid = goal_request->uuid; response->sequence.push_back(0); - feedback_message->sequence = response->sequence; + feedback_message.sequence = response->sequence; feedback_publisher->publish(feedback_message); - if (request->order > 0) - { + // executor.spin_once(); + if (goal_request->order > 0) { response->sequence.push_back(1); - feedback_message->sequence = response->sequence; + feedback_message.sequence = response->sequence; feedback_publisher->publish(feedback_message); - for (int i = 1; i < goal_request->order; ++i) - { + // executor.spin_once(); + for (int i = 1; i < goal_request->order; ++i) { response->sequence.push_back(response->sequence[i] + response->sequence[i - 1]); - feedback_message->sequence = response->sequence; + feedback_message.sequence = response->sequence; feedback_publisher->publish(feedback_message); + // executor.spin_once(); } } + status_message.status_list[0].status = rclcpp_action::GoalStatus::STATUS_SUCCEEDED; + status_publisher->publish(status_message); + // executor.spin_once(); response->status = rclcpp_action::GoalStatus::STATUS_SUCCEEDED; - status_message.status_list.back().status = rclcpp_action::GoalStatus::STATUS_SUCCEEDED; - status_publisher->publish(status_message): goals.erase(request->uuid); } else { response->status = rclcpp_action::GoalStatus::STATUS_UNKNOWN; @@ -139,36 +149,40 @@ class TestActionClient : public ::testing::Test ret = rcl_action_get_cancel_service_name( action_name, allocator, &cancel_service_name); ASSERT_EQ(RCL_RET_OK, ret); - cancel_service = node->create_service( - cancel_service_name, - [this] ( - const ActionCancelRequest::SharedPtr request, - const ActionCancelResponse::SharedPtr response) mutable - { - ActionStatusMessage status_message; - bool cancel_all = ( - request->goal_info.stamp == zero_stamp && - request->goal_info.uuid == zero_uuid); - auto it = goals.begin(); - while (it != goals.end()) { - auto goal_request = it->second.first; - auto goal_response = it->second.second; - bool cancel_this = ( - request->goal_info.uuid == goal_request->uuid || - request->goal_info.stamp > goal_response->stamp); - if (cancel_all || cancel_this) - { - rclcpp_action::GoalStatus goal_status; - goal_status.goal_info.uuid = goal_request->uuid; - goal_status.goal_info.stamp = goal_response->stamp; - goal_status.status = rclcpp_action::GoalStatus::STATUS_CANCELED; - status_message.status_list.push_back(goal_status); - response->goals_canceling.push_back(goal_status.goal_info); - it = goals.erase(it); - } + cancel_service = server_node->create_service( + cancel_service_name, + [this]( + const ActionCancelGoalRequest::SharedPtr request, + ActionCancelGoalResponse::SharedPtr response) + { + const rclcpp::Time zero_stamp; + const rclcpp::Time cancel_stamp = request->goal_info.stamp; + rclcpp_action::GoalID zero_uuid; + std::fill(zero_uuid.begin(), zero_uuid.end(), 0u); + bool cancel_all = ( + request->goal_info.goal_id.uuid == zero_uuid + && cancel_stamp == zero_stamp); + ActionStatusMessage status_message; + auto it = goals.begin(); + while (it != goals.end()) { + auto goal_request = it->second.first; + auto goal_response = it->second.second; + const rclcpp::Time goal_stamp = goal_response->stamp; + bool cancel_this = ( + request->goal_info.goal_id.uuid == goal_request->uuid || + cancel_stamp > goal_stamp); + if (cancel_all || cancel_this) { + rclcpp_action::GoalStatus goal_status; + goal_status.goal_info.goal_id.uuid = goal_request->uuid; + goal_status.goal_info.stamp = goal_response->stamp; + goal_status.status = rclcpp_action::GoalStatus::STATUS_CANCELED; + status_message.status_list.push_back(goal_status); + response->goals_canceling.push_back(goal_status.goal_info); + it = goals.erase(it); } - status_publisher->publish(status_message); - }); + } + status_publisher->publish(status_message); + }); ASSERT_TRUE(cancel_service != nullptr); allocator.deallocate(cancel_service_name, allocator.state); @@ -176,7 +190,7 @@ class TestActionClient : public ::testing::Test ret = rcl_action_get_feedback_topic_name( action_name, allocator, &feedback_topic_name); ASSERT_EQ(RCL_RET_OK, ret); - feedback_publisher = node->create_publisher(feedback_topic_name); + feedback_publisher = server_node->create_publisher(feedback_topic_name); ASSERT_TRUE(feedback_publisher != nullptr); allocator.deallocate(feedback_topic_name, allocator.state); @@ -184,12 +198,14 @@ class TestActionClient : public ::testing::Test ret = rcl_action_get_status_topic_name( action_name, allocator, &status_topic_name); ASSERT_EQ(RCL_RET_OK, ret); - status_publisher = node->create_publisher(status_topic_name); + status_publisher = server_node->create_publisher(status_topic_name); ASSERT_TRUE(status_publisher != nullptr); allocator.deallocate(status_topic_name, allocator.state); - ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(clock.get_clock_handle())); - ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(1))); + executor.add_node(server_node); + + client_node = std::make_shared(client_node_name, namespace_name); + executor.add_node(client_node); } void Teardown() @@ -198,48 +214,57 @@ class TestActionClient : public ::testing::Test feedback_publisher.reset(); cancel_service.reset(); result_service.reset(); - goal_servicer.reset(); - node.reset(); + goal_service.reset(); + server_node.reset(); + client_node.reset(); } - rclcpp::Node::SharedPtr node; - rclcpp::Clock clock{RCL_ROS_TIME}; - const char * const node_name{"fibonacci_action_test_node"}; + rclcpp::Clock clock; + rclcpp::Node::SharedPtr server_node; + rclcpp::Node::SharedPtr client_node; + rclcpp::executors::MultiThreadedExecutor executor; + const char * const server_node_name{"fibonacci_action_test_server"}; + const char * const client_node_name{"fibonacci_action_test_client"}; const char * const namespace_name{"/rclcpp_action/test/client"}; const char * const action_name{"fibonacci_test"}; + typename rclcpp::Service::SharedPtr goal_service; typename rclcpp::Service::SharedPtr result_service; typename rclcpp::Service::SharedPtr cancel_service; typename rclcpp::Publisher::SharedPtr feedback_publisher; typename rclcpp::Publisher::SharedPtr status_publisher; - std::map goals; + std::map< + rclcpp_action::GoalID, + std::pair< + typename ActionGoalRequest::SharedPtr, + typename ActionGoalResponse::SharedPtr>> goals; }; TEST_F(TestClient, construction_and_destruction) { - ASSERT_NO_THROW(rclcpp_action::create_client(node, action_name).reset()); + ASSERT_NO_THROW(rclcpp_action::create_client(client_node, action_name).reset()); } TEST_F(TestClient, async_send_goal_but_ignore_feedback_and_result) { - auto action_client = rclcpp_action::create_client(node, action_name); + auto action_client = rclcpp_action::create_client(client_node, action_name); ActionGoal bad_goal; bad_goal.order = -5; auto future_goal_handle = action_client->async_send_goal(bad_goal, nullptr, true); ASSERT_EQ( rclcpp::executor::FutureReturnCode::SUCCESS, - rclcpp::spin_until_future_complete(node, future)); - EXPECT_THROW(future.get(), rclcpp_action::exceptions::RejectedGoalError); + executor.spin_until_future_complete(future_goal_handle)); + EXPECT_THROW(future_goal_handle.get(), rclcpp_action::exceptions::RejectedGoalError); ActionGoal good_goal; good_goal.order = 5; future_goal_handle = action_client->async_send_goal(good_goal, nullptr, true); ASSERT_EQ( rclcpp::executor::FutureReturnCode::SUCCESS, - rclcpp::spin_until_future_complete(node, future)); - auto goal_handle = future.get(); - EXPECT_EQ(GoalStatus::ACCEPTED, goal_handle->get_status()); + executor.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()); EXPECT_FALSE(goal_handle->is_feedback_aware()); EXPECT_FALSE(goal_handle->is_result_aware()); EXPECT_THROW(goal_handle->async_result(), rclcpp_action::exceptions::UnawareGoalHandleError); @@ -247,146 +272,144 @@ TEST_F(TestClient, async_send_goal_but_ignore_feedback_and_result) TEST_F(TestClient, async_send_goal_and_ignore_feedback_but_wait_for_result) { - auto action_client = rclcpp_action::create_client(node, action_name); + auto action_client = rclcpp_action::create_client(client_node, action_name); ActionGoal goal; goal.order = 5; auto future_goal_handle = action_client->async_send_goal(goal); ASSERT_EQ( rclcpp::executor::FutureReturnCode::SUCCESS, - rclcpp::spin_until_future_complete(node, future_goal_handle)); + executor.spin_until_future_complete(future_goal_handle)); auto goal_handle = future_goal_handle.get(); - EXPECT_EQ(GoalStatus::ACCEPTED, goal_handle->get_status()); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); EXPECT_FALSE(goal_handle->is_feedback_aware()); EXPECT_TRUE(goal_handle->is_result_aware()); auto future_result = goal_handle->async_result(); ASSERT_EQ( rclcpp::executor::FutureReturnCode::SUCCESS, - rclcpp::spin_until_future_complete(node, future_result)); + executor.spin_until_future_complete(future_result)); auto result = future_result.get(); - ASSERT_EQ(6, result->response.sequence.size()); - EXPECT_EQ(0, result->response.sequence[0]); - EXPECT_EQ(1, result->response.sequence[1]); - EXPECT_EQ(5, result->response.sequence[5]); + ASSERT_EQ(6ul, result.response->sequence.size()); + EXPECT_EQ(0, result.response->sequence[0]); + EXPECT_EQ(1, result.response->sequence[1]); + EXPECT_EQ(5, result.response->sequence[5]); } TEST_F(TestClient, async_send_goal_with_feedback_and_result) { - auto action_client = rclcpp_action::create_client(node, action_name); + auto action_client = rclcpp_action::create_client(client_node, action_name); ActionGoal goal; goal.order = 4; int feedback_count = 0; auto future_goal_handle = action_client->async_send_goal( goal, - [&feedback_count] ( + [&feedback_count]( typename ActionGoalHandle::SharedPtr goal_handle, const ActionFeedback & feedback) mutable { + (void)goal_handle; + (void)feedback; feedback_count++; }); ASSERT_EQ( rclcpp::executor::FutureReturnCode::SUCCESS, - rclcpp::spin_until_future_complete(node, future_goal_handle)); + executor.spin_until_future_complete(future_goal_handle)); auto goal_handle = future_goal_handle.get(); - EXPECT_EQ(GoalStatus::ACCEPTED, goal_handle->get_status()); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); EXPECT_TRUE(goal_handle->is_feedback_aware()); EXPECT_TRUE(goal_handle->is_result_aware()); auto future_result = goal_handle->async_result(); ASSERT_EQ( rclcpp::executor::FutureReturnCode::SUCCESS, - rclcpp::spin_until_future_complete(node, future_result)); + executor.spin_until_future_complete(future_result)); auto result = future_result.get(); - ASSERT_EQ(5, result->response.sequence.size()); - EXPECT_EQ(3, result->response.sequence.back()); + + ASSERT_EQ(5u, result.response->sequence.size()); + EXPECT_EQ(3, result.response->sequence.back()); EXPECT_EQ(5, feedback_count); } TEST_F(TestClient, async_cancel_one_goal) { - auto action_client = rclcpp_action::create_client(node, action_name); + auto action_client = rclcpp_action::create_client(client_node, action_name); ActionGoal goal; goal.order = 5; auto future_goal_handle = action_client->async_send_goal(goal, nullptr, true); ASSERT_EQ( rclcpp::executor::FutureReturnCode::SUCCESS, - rclcpp::spin_until_future_complete(node, future_goal_handle)); + executor.spin_until_future_complete(future_goal_handle)); auto goal_handle = future_goal_handle.get(); - EXPECT_EQ(rclcpp_action::GoalStatus::ACCEPTED, goal_handle->get_status()); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); auto future_cancel = action_client->async_cancel_goal(goal_handle); ASSERT_EQ( rclcpp::executor::FutureReturnCode::SUCCESS, - rclcpp::spin_until_future_complete(node, future_cancel_all)); - auto cancel_response = future_cancel.get(); - ASSERT_EQ(1, cancel_response->goals_canceling.size()); - const rclcpp_action::GoalInfo & canceled_goal_info = cancel_response->goals_canceling[0]; - EXPECT_EQ(canceled_goal_info.uuid, goal_handle->get_goal_id()); - EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle->get_status()); + executor.spin_until_future_complete(future_cancel)); + bool goal_canceled = future_cancel.get(); + EXPECT_TRUE(goal_canceled); } TEST_F(TestClient, async_cancel_all_goals) { - auto action_client = rclcpp_action::create_client(node, action_name); + auto action_client = rclcpp_action::create_client(client_node, action_name); ActionGoal goal; goal.order = 6; auto future_goal_handle0 = action_client->async_send_goal(goal, nullptr, true); ASSERT_EQ( rclcpp::executor::FutureReturnCode::SUCCESS, - rclcpp::spin_until_future_complete(node, future_goal_handle0)); + executor.spin_until_future_complete(future_goal_handle0)); auto goal_handle0 = future_goal_handle0.get(); goal.order = 8; auto future_goal_handle1 = action_client->async_send_goal(goal, nullptr, true); ASSERT_EQ( rclcpp::executor::FutureReturnCode::SUCCESS, - rclcpp::spin_until_future_complete(node, future_goal_handle1)); + executor.spin_until_future_complete(future_goal_handle1)); auto goal_handle1 = future_goal_handle1.get(); auto future_cancel_all = action_client->async_cancel_all_goals(); ASSERT_EQ( rclcpp::executor::FutureReturnCode::SUCCESS, - rclcpp::spin_until_future_complete(node, future_cancel_all)); + executor.spin_until_future_complete(future_cancel_all)); auto cancel_response = future_cancel_all.get(); - ASSERT_EQ(2, cancel_response->goals_canceling.size()); - EXPECT_EQ(goal_handle0->get_goal_id(), cancel_response->goals_canceling[0].uuid); - EXPECT_EQ(goal_handle1->get_goal_id(), cancel_response->goals_canceling[1].uuid); + 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); } TEST_F(TestClient, async_cancel_some_goals) { - auto action_client = rclcpp_action::create_client(node, action_name); + auto action_client = rclcpp_action::create_client(client_node, action_name); ActionGoal goal; goal.order = 6; auto future_goal_handle0 = action_client->async_send_goal(goal, nullptr, true); ASSERT_EQ( rclcpp::executor::FutureReturnCode::SUCCESS, - rclcpp::spin_until_future_complete(node, future_goal_handle0)); + executor.spin_until_future_complete(future_goal_handle0)); auto goal_handle0 = future_goal_handle0.get(); - // Move time forward - 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, nullptr, true); ASSERT_EQ( rclcpp::executor::FutureReturnCode::SUCCESS, - rclcpp::spin_until_future_complete(node, future_goal_handle1)); + executor.spin_until_future_complete(future_goal_handle1)); auto goal_handle1 = future_goal_handle1.get(); - auto future_cancel_some = action_client->async_cancel_goals_before(goal_handle1->get_goal_stamp()); + auto future_cancel_some = + action_client->async_cancel_goals_before(goal_handle1->get_goal_stamp()); ASSERT_EQ( rclcpp::executor::FutureReturnCode::SUCCESS, - rclcpp::spin_until_future_complete(node, future_cancel_some)); + executor.spin_until_future_complete(future_cancel_some)); auto cancel_response = future_cancel_some.get(); - ASSERT_EQ(1, cancel_response->goals_canceling.size()); - EXPECT_EQ(goal_handle0->get_goal_id(), cancel_response->goals_canceling[0].uuid); + ASSERT_EQ(1ul, cancel_response->goals_canceling.size()); + EXPECT_EQ(goal_handle0->get_goal_id(), cancel_response->goals_canceling[0].goal_id.uuid); } From c2c677510ac9456088e78fa0a112cd4d61307965 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 5 Dec 2018 00:04:28 -0800 Subject: [PATCH 35/56] [rclcpp_action] Action client tests passing. --- rclcpp_action/test/test_client.cpp | 135 +++++++++++++++++++---------- 1 file changed, 91 insertions(+), 44 deletions(-) diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index 7e10149b32..da847228f4 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -72,9 +73,10 @@ class TestClient : public ::testing::Test void SetUp() { - server_node = std::make_shared(server_node_name, namespace_name); rcl_allocator_t allocator = rcl_get_default_allocator(); + server_node = std::make_shared(server_node_name, namespace_name); + char * goal_service_name = nullptr; rcl_ret_t ret = rcl_action_get_goal_service_name( action_name, allocator, &goal_service_name); @@ -114,28 +116,28 @@ class TestClient : public ::testing::Test goal_status.status = rclcpp_action::GoalStatus::STATUS_EXECUTING; status_message.status_list.push_back(goal_status); status_publisher->publish(status_message); - // executor.spin_once(); + client_executor.spin_once(); ActionFeedbackMessage feedback_message; feedback_message.uuid = goal_request->uuid; - response->sequence.push_back(0); - feedback_message.sequence = response->sequence; + feedback_message.sequence.push_back(0); feedback_publisher->publish(feedback_message); - // executor.spin_once(); + client_executor.spin_once(); if (goal_request->order > 0) { - response->sequence.push_back(1); - feedback_message.sequence = response->sequence; + feedback_message.sequence.push_back(1); feedback_publisher->publish(feedback_message); - // executor.spin_once(); + client_executor.spin_once(); for (int i = 1; i < goal_request->order; ++i) { - response->sequence.push_back(response->sequence[i] + response->sequence[i - 1]); - feedback_message.sequence = response->sequence; + feedback_message.sequence.push_back( + feedback_message.sequence[i] + feedback_message.sequence[i - 1]); feedback_publisher->publish(feedback_message); - // executor.spin_once(); + client_executor.spin_once(); } } - status_message.status_list[0].status = rclcpp_action::GoalStatus::STATUS_SUCCEEDED; + goal_status.status = rclcpp_action::GoalStatus::STATUS_SUCCEEDED; + status_message.status_list[0] = goal_status; status_publisher->publish(status_message); - // executor.spin_once(); + client_executor.spin_once(); + response->sequence = feedback_message.sequence; response->status = rclcpp_action::GoalStatus::STATUS_SUCCEEDED; goals.erase(request->uuid); } else { @@ -155,10 +157,9 @@ class TestClient : public ::testing::Test const ActionCancelGoalRequest::SharedPtr request, ActionCancelGoalResponse::SharedPtr response) { - const rclcpp::Time zero_stamp; - const rclcpp::Time cancel_stamp = request->goal_info.stamp; rclcpp_action::GoalID zero_uuid; std::fill(zero_uuid.begin(), zero_uuid.end(), 0u); + const rclcpp::Time cancel_stamp = request->goal_info.stamp; bool cancel_all = ( request->goal_info.goal_id.uuid == zero_uuid && cancel_stamp == zero_stamp); @@ -179,9 +180,12 @@ class TestClient : public ::testing::Test status_message.status_list.push_back(goal_status); response->goals_canceling.push_back(goal_status.goal_info); it = goals.erase(it); + } else { + ++it; } } status_publisher->publish(status_message); + client_executor.spin_once(); }); ASSERT_TRUE(cancel_service != nullptr); allocator.deallocate(cancel_service_name, allocator.state); @@ -198,14 +202,16 @@ class TestClient : public ::testing::Test ret = rcl_action_get_status_topic_name( action_name, allocator, &status_topic_name); ASSERT_EQ(RCL_RET_OK, ret); - status_publisher = server_node->create_publisher(status_topic_name); + status_publisher = server_node->create_publisher(status_topic_name, rcl_action_qos_profile_status_default); ASSERT_TRUE(status_publisher != nullptr); allocator.deallocate(status_topic_name, allocator.state); + server_executor.add_node(server_node); - executor.add_node(server_node); + client_node = std::make_shared(client_node_name, namespace_name); + client_executor.add_node(client_node); - client_node = std::make_shared(client_node_name, namespace_name); - executor.add_node(client_node); + ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(clock.get_clock_handle())); + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(1))); } void Teardown() @@ -219,25 +225,28 @@ class TestClient : public ::testing::Test client_node.reset(); } - rclcpp::Clock clock; + rclcpp::Clock clock{RCL_ROS_TIME}; + const rclcpp::Time zero_stamp{0, 0, RCL_ROS_TIME}; + rclcpp::Node::SharedPtr server_node; + rclcpp::executors::SingleThreadedExecutor server_executor; rclcpp::Node::SharedPtr client_node; - rclcpp::executors::MultiThreadedExecutor executor; + rclcpp::executors::SingleThreadedExecutor client_executor; const char * const server_node_name{"fibonacci_action_test_server"}; const char * const client_node_name{"fibonacci_action_test_client"}; const char * const namespace_name{"/rclcpp_action/test/client"}; const char * const action_name{"fibonacci_test"}; - typename rclcpp::Service::SharedPtr goal_service; - typename rclcpp::Service::SharedPtr result_service; - typename rclcpp::Service::SharedPtr cancel_service; - typename rclcpp::Publisher::SharedPtr feedback_publisher; - typename rclcpp::Publisher::SharedPtr status_publisher; std::map< rclcpp_action::GoalID, std::pair< typename ActionGoalRequest::SharedPtr, typename ActionGoalResponse::SharedPtr>> goals; + typename rclcpp::Service::SharedPtr goal_service; + typename rclcpp::Service::SharedPtr result_service; + typename rclcpp::Service::SharedPtr cancel_service; + typename rclcpp::Publisher::SharedPtr feedback_publisher; + typename rclcpp::Publisher::SharedPtr status_publisher; }; TEST_F(TestClient, construction_and_destruction) @@ -252,17 +261,21 @@ TEST_F(TestClient, async_send_goal_but_ignore_feedback_and_result) ActionGoal bad_goal; bad_goal.order = -5; auto future_goal_handle = action_client->async_send_goal(bad_goal, nullptr, true); + server_executor.spin_once(); + server_executor.spin_some(); ASSERT_EQ( rclcpp::executor::FutureReturnCode::SUCCESS, - executor.spin_until_future_complete(future_goal_handle)); + client_executor.spin_until_future_complete(future_goal_handle)); EXPECT_THROW(future_goal_handle.get(), rclcpp_action::exceptions::RejectedGoalError); ActionGoal good_goal; good_goal.order = 5; future_goal_handle = action_client->async_send_goal(good_goal, nullptr, true); + server_executor.spin_once(); + server_executor.spin_some(); ASSERT_EQ( rclcpp::executor::FutureReturnCode::SUCCESS, - executor.spin_until_future_complete(future_goal_handle)); + client_executor.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()); EXPECT_FALSE(goal_handle->is_feedback_aware()); @@ -277,18 +290,21 @@ TEST_F(TestClient, async_send_goal_and_ignore_feedback_but_wait_for_result) ActionGoal goal; goal.order = 5; auto future_goal_handle = action_client->async_send_goal(goal); + server_executor.spin_once(); + server_executor.spin_some(); ASSERT_EQ( rclcpp::executor::FutureReturnCode::SUCCESS, - executor.spin_until_future_complete(future_goal_handle)); + client_executor.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()); EXPECT_FALSE(goal_handle->is_feedback_aware()); EXPECT_TRUE(goal_handle->is_result_aware()); - auto future_result = goal_handle->async_result(); + server_executor.spin_once(); + server_executor.spin_some(); ASSERT_EQ( rclcpp::executor::FutureReturnCode::SUCCESS, - executor.spin_until_future_complete(future_result)); + client_executor.spin_until_future_complete(future_result)); auto result = future_result.get(); ASSERT_EQ(6ul, result.response->sequence.size()); EXPECT_EQ(0, result.response->sequence[0]); @@ -313,19 +329,21 @@ TEST_F(TestClient, async_send_goal_with_feedback_and_result) (void)feedback; feedback_count++; }); - + server_executor.spin_once(); + server_executor.spin_some(); ASSERT_EQ( rclcpp::executor::FutureReturnCode::SUCCESS, - executor.spin_until_future_complete(future_goal_handle)); + client_executor.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()); EXPECT_TRUE(goal_handle->is_feedback_aware()); EXPECT_TRUE(goal_handle->is_result_aware()); - auto future_result = goal_handle->async_result(); + server_executor.spin_once(); + server_executor.spin_some(); ASSERT_EQ( rclcpp::executor::FutureReturnCode::SUCCESS, - executor.spin_until_future_complete(future_result)); + client_executor.spin_until_future_complete(future_result)); auto result = future_result.get(); ASSERT_EQ(5u, result.response->sequence.size()); @@ -340,16 +358,20 @@ TEST_F(TestClient, async_cancel_one_goal) ActionGoal goal; goal.order = 5; auto future_goal_handle = action_client->async_send_goal(goal, nullptr, true); + server_executor.spin_once(); + server_executor.spin_some(); ASSERT_EQ( rclcpp::executor::FutureReturnCode::SUCCESS, - executor.spin_until_future_complete(future_goal_handle)); + client_executor.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()); auto future_cancel = action_client->async_cancel_goal(goal_handle); + server_executor.spin_once(); + server_executor.spin_some(); ASSERT_EQ( rclcpp::executor::FutureReturnCode::SUCCESS, - executor.spin_until_future_complete(future_cancel)); + client_executor.spin_until_future_complete(future_cancel)); bool goal_canceled = future_cancel.get(); EXPECT_TRUE(goal_canceled); } @@ -361,25 +383,39 @@ TEST_F(TestClient, async_cancel_all_goals) ActionGoal goal; goal.order = 6; auto future_goal_handle0 = action_client->async_send_goal(goal, nullptr, true); + server_executor.spin_once(); + server_executor.spin_some(); ASSERT_EQ( rclcpp::executor::FutureReturnCode::SUCCESS, - executor.spin_until_future_complete(future_goal_handle0)); + client_executor.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, nullptr, true); + server_executor.spin_once(); + server_executor.spin_some(); ASSERT_EQ( rclcpp::executor::FutureReturnCode::SUCCESS, - executor.spin_until_future_complete(future_goal_handle1)); + client_executor.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))); + auto future_cancel_all = action_client->async_cancel_all_goals(); + server_executor.spin_once(); + server_executor.spin_some(); ASSERT_EQ( rclcpp::executor::FutureReturnCode::SUCCESS, - executor.spin_until_future_complete(future_cancel_all)); + client_executor.spin_until_future_complete(future_cancel_all)); auto cancel_response = future_cancel_all.get(); - ASSERT_EQ(2ul, cancel_response->goals_canceling.size()); + 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); } @@ -391,25 +427,36 @@ TEST_F(TestClient, async_cancel_some_goals) ActionGoal goal; goal.order = 6; auto future_goal_handle0 = action_client->async_send_goal(goal, nullptr, true); + server_executor.spin_once(); + server_executor.spin_some(); ASSERT_EQ( rclcpp::executor::FutureReturnCode::SUCCESS, - executor.spin_until_future_complete(future_goal_handle0)); + client_executor.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, nullptr, true); + server_executor.spin_once(); + server_executor.spin_some(); ASSERT_EQ( rclcpp::executor::FutureReturnCode::SUCCESS, - executor.spin_until_future_complete(future_goal_handle1)); + client_executor.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))); + auto future_cancel_some = action_client->async_cancel_goals_before(goal_handle1->get_goal_stamp()); + server_executor.spin_once(); + server_executor.spin_some(); ASSERT_EQ( rclcpp::executor::FutureReturnCode::SUCCESS, - executor.spin_until_future_complete(future_cancel_some)); + client_executor.spin_until_future_complete(future_cancel_some)); auto cancel_response = future_cancel_some.get(); 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()); } From ed1af61ea9a89b3f4e7021fe64fce506184c9582 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 5 Dec 2018 09:33:24 -0800 Subject: [PATCH 36/56] Spin both executors to make tests pass on my machine --- rclcpp_action/test/test_client.cpp | 83 +++++++++--------------------- 1 file changed, 23 insertions(+), 60 deletions(-) diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index da847228f4..83a405804d 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -225,6 +225,17 @@ class TestClient : public ::testing::Test client_node.reset(); } + template + void dual_spin_until_future_complete(std::shared_future & future) + { + std::future_status status; + do { + server_executor.spin_some(); + client_executor.spin_some(); + status = future.wait_for(std::chrono::seconds(0)); + } while(std::future_status::ready != status); + } + rclcpp::Clock clock{RCL_ROS_TIME}; const rclcpp::Time zero_stamp{0, 0, RCL_ROS_TIME}; @@ -290,21 +301,13 @@ TEST_F(TestClient, async_send_goal_and_ignore_feedback_but_wait_for_result) ActionGoal goal; goal.order = 5; auto future_goal_handle = action_client->async_send_goal(goal); - server_executor.spin_once(); - server_executor.spin_some(); - ASSERT_EQ( - rclcpp::executor::FutureReturnCode::SUCCESS, - client_executor.spin_until_future_complete(future_goal_handle)); + 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()); EXPECT_FALSE(goal_handle->is_feedback_aware()); EXPECT_TRUE(goal_handle->is_result_aware()); auto future_result = goal_handle->async_result(); - server_executor.spin_once(); - server_executor.spin_some(); - ASSERT_EQ( - rclcpp::executor::FutureReturnCode::SUCCESS, - client_executor.spin_until_future_complete(future_result)); + dual_spin_until_future_complete(future_result); auto result = future_result.get(); ASSERT_EQ(6ul, result.response->sequence.size()); EXPECT_EQ(0, result.response->sequence[0]); @@ -329,21 +332,13 @@ TEST_F(TestClient, async_send_goal_with_feedback_and_result) (void)feedback; feedback_count++; }); - server_executor.spin_once(); - server_executor.spin_some(); - ASSERT_EQ( - rclcpp::executor::FutureReturnCode::SUCCESS, - client_executor.spin_until_future_complete(future_goal_handle)); + 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()); EXPECT_TRUE(goal_handle->is_feedback_aware()); EXPECT_TRUE(goal_handle->is_result_aware()); auto future_result = goal_handle->async_result(); - server_executor.spin_once(); - server_executor.spin_some(); - ASSERT_EQ( - rclcpp::executor::FutureReturnCode::SUCCESS, - client_executor.spin_until_future_complete(future_result)); + dual_spin_until_future_complete(future_result); auto result = future_result.get(); ASSERT_EQ(5u, result.response->sequence.size()); @@ -358,20 +353,12 @@ TEST_F(TestClient, async_cancel_one_goal) ActionGoal goal; goal.order = 5; auto future_goal_handle = action_client->async_send_goal(goal, nullptr, true); - server_executor.spin_once(); - server_executor.spin_some(); - ASSERT_EQ( - rclcpp::executor::FutureReturnCode::SUCCESS, - client_executor.spin_until_future_complete(future_goal_handle)); + 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()); auto future_cancel = action_client->async_cancel_goal(goal_handle); - server_executor.spin_once(); - server_executor.spin_some(); - ASSERT_EQ( - rclcpp::executor::FutureReturnCode::SUCCESS, - client_executor.spin_until_future_complete(future_cancel)); + dual_spin_until_future_complete(future_cancel); bool goal_canceled = future_cancel.get(); EXPECT_TRUE(goal_canceled); } @@ -383,22 +370,14 @@ TEST_F(TestClient, async_cancel_all_goals) ActionGoal goal; goal.order = 6; auto future_goal_handle0 = action_client->async_send_goal(goal, nullptr, true); - server_executor.spin_once(); - server_executor.spin_some(); - ASSERT_EQ( - rclcpp::executor::FutureReturnCode::SUCCESS, - client_executor.spin_until_future_complete(future_goal_handle0)); + 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, nullptr, true); - server_executor.spin_once(); - server_executor.spin_some(); - ASSERT_EQ( - rclcpp::executor::FutureReturnCode::SUCCESS, - client_executor.spin_until_future_complete(future_goal_handle1)); + 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()) { @@ -408,11 +387,7 @@ TEST_F(TestClient, async_cancel_all_goals) ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(3))); auto future_cancel_all = action_client->async_cancel_all_goals(); - server_executor.spin_once(); - server_executor.spin_some(); - ASSERT_EQ( - rclcpp::executor::FutureReturnCode::SUCCESS, - client_executor.spin_until_future_complete(future_cancel_all)); + dual_spin_until_future_complete(future_cancel_all); auto cancel_response = future_cancel_all.get(); ASSERT_EQ(2ul, cancel_response->goals_canceling.size()); @@ -427,33 +402,21 @@ TEST_F(TestClient, async_cancel_some_goals) ActionGoal goal; goal.order = 6; auto future_goal_handle0 = action_client->async_send_goal(goal, nullptr, true); - server_executor.spin_once(); - server_executor.spin_some(); - ASSERT_EQ( - rclcpp::executor::FutureReturnCode::SUCCESS, - client_executor.spin_until_future_complete(future_goal_handle0)); + 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, nullptr, true); - server_executor.spin_once(); - server_executor.spin_some(); - ASSERT_EQ( - rclcpp::executor::FutureReturnCode::SUCCESS, - client_executor.spin_until_future_complete(future_goal_handle1)); + 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))); auto future_cancel_some = action_client->async_cancel_goals_before(goal_handle1->get_goal_stamp()); - server_executor.spin_once(); - server_executor.spin_some(); - ASSERT_EQ( - rclcpp::executor::FutureReturnCode::SUCCESS, - client_executor.spin_until_future_complete(future_cancel_some)); + dual_spin_until_future_complete(future_cancel_some); auto cancel_response = future_cancel_some.get(); ASSERT_EQ(1ul, cancel_response->goals_canceling.size()); From 923564b76e64acde0f38caacf5a658e56090090d Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 5 Dec 2018 09:34:19 -0800 Subject: [PATCH 37/56] Feedback callback uses shared pointer --- rclcpp_action/include/rclcpp_action/client.hpp | 5 +++-- rclcpp_action/include/rclcpp_action/client_goal_handle.hpp | 3 ++- rclcpp_action/test/test_client.cpp | 2 +- 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index c339932ded..1ad37f3701 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -159,7 +159,8 @@ class Client : public ClientBase using Feedback = typename ACTION::Feedback; using GoalHandle = ClientGoalHandle; using Result = typename GoalHandle::Result; - using FeedbackCallback = std::function; + using FeedbackCallback = + std::function)>; using CancelRequest = typename ACTION::CancelGoalService::Request; using CancelResponse = typename ACTION::CancelGoalService::Response; @@ -356,7 +357,7 @@ class Client : public ClientBase } const FeedbackCallback & callback = goal_handle->get_feedback_callback(); // callback(goal_handle, feedback_message->feedback); - callback(goal_handle, *feedback_message); + callback(goal_handle, feedback_message); } std::shared_ptr create_status_message() const override diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp index e9486656c0..cd2bec2974 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp @@ -63,7 +63,8 @@ class ClientGoalHandle } Result; using Feedback = typename ACTION::Feedback; - using FeedbackCallback = std::function; + using FeedbackCallback = + std::function)>; virtual ~ClientGoalHandle(); diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index 83a405804d..7043a4fe3e 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -326,7 +326,7 @@ TEST_F(TestClient, async_send_goal_with_feedback_and_result) goal, [&feedback_count]( typename ActionGoalHandle::SharedPtr goal_handle, - const ActionFeedback & feedback) mutable + const std::shared_ptr feedback) mutable { (void)goal_handle; (void)feedback; From 9e74ca4f53770a1f79155afcc96397c872f08ccf Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 5 Dec 2018 09:37:04 -0800 Subject: [PATCH 38/56] comment about why make_result_aware is called --- rclcpp_action/include/rclcpp_action/client.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 1ad37f3701..04f8defc07 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -236,6 +236,7 @@ class Client : public ClientBase if (goal_handles_.count(goal_handle->get_goal_id()) == 0) { throw exceptions::UnknownGoalHandleError(); } + // 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); } From f7a52eb57c4f42faf8f9f44bfb23dc6435814490 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 5 Dec 2018 09:54:38 -0800 Subject: [PATCH 39/56] Client documentation --- .../include/rclcpp_action/client.hpp | 67 +++++++++++++++++-- 1 file changed, 61 insertions(+), 6 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 04f8defc07..8aea449195 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -44,7 +44,13 @@ namespace rclcpp_action class ClientBaseImpl; /// Base Action Client implementation -/// It is responsible for interfacing with the C action client API. +/// \internal +/** + * This class should not be used directly by users wanting to create an aciton client. + * Instead users should use `rclcpp_action::Client<>`. + * + * Internally, this class is responsible for interfacing with the `rcl_action` API. + */ class ClientBase : public rclcpp::Waitable { public: @@ -59,96 +65,136 @@ class ClientBase : public rclcpp::Waitable RCLCPP_ACTION_PUBLIC virtual ~ClientBase(); + // ------------- + // Waitables API + + /// \internal RCLCPP_ACTION_PUBLIC size_t get_number_of_ready_subscriptions() override; + /// \internal RCLCPP_ACTION_PUBLIC size_t get_number_of_ready_timers() override; + /// \internal RCLCPP_ACTION_PUBLIC size_t get_number_of_ready_clients() override; + /// \internal RCLCPP_ACTION_PUBLIC size_t get_number_of_ready_services() override; + /// \internal RCLCPP_ACTION_PUBLIC size_t get_number_of_ready_guard_conditions() override; + /// \internal RCLCPP_ACTION_PUBLIC bool add_to_wait_set(rcl_wait_set_t * wait_set) override; + /// \internal RCLCPP_ACTION_PUBLIC bool is_ready(rcl_wait_set_t * wait_set) override; + /// \internal RCLCPP_ACTION_PUBLIC void execute() override; + // End Waitables API + // ----------------- + protected: + // ----------------------------------------------------- + // API for communication between ClientBase and Client<> using ResponseCallback = std::function response)>; + /// \internal RCLCPP_ACTION_PUBLIC rclcpp::Logger get_logger(); + /// \internal RCLCPP_ACTION_PUBLIC virtual GoalID generate_goal_id(); + /// \internal RCLCPP_ACTION_PUBLIC virtual void send_goal_request( std::shared_ptr request, ResponseCallback callback); + /// \internal RCLCPP_ACTION_PUBLIC virtual void send_result_request( std::shared_ptr request, ResponseCallback callback); + /// \internal RCLCPP_ACTION_PUBLIC virtual void send_cancel_request( std::shared_ptr request, ResponseCallback callback); -private: + /// \internal virtual std::shared_ptr create_goal_response() const = 0; + /// \internal virtual void handle_goal_response( const rmw_request_id_t & response_header, std::shared_ptr goal_response); + /// \internal virtual std::shared_ptr create_result_response() const = 0; + /// \internal virtual void handle_result_response( const rmw_request_id_t & response_header, std::shared_ptr result_response); + /// \internal virtual std::shared_ptr create_cancel_response() const = 0; + /// \internal virtual void handle_cancel_response( const rmw_request_id_t & response_header, std::shared_ptr cancel_response); + /// \internal virtual std::shared_ptr create_feedback_message() const = 0; + /// \internal virtual void handle_feedback_message(std::shared_ptr message) = 0; + /// \internal virtual std::shared_ptr create_status_message() const = 0; + /// \internal virtual void handle_status_message(std::shared_ptr message) = 0; + // End API for communication between ClientBase and Client<> + // --------------------------------------------------------- + +private: std::unique_ptr pimpl_; }; - -/// Templated Action Client class -/// It is responsible for getting the C action type support struct from the C++ type, and -/// calling user callbacks with goal handles of the appropriate type. +/// Action Client +/** + * This class creates an action client. + * + * Create an instance of this server using `rclcpp_action::create_client()`. + * + * Internally, this class is responsible for: + * - coverting between the C++ action type and generic types for `rclcpp_action::ClientBase`, and + * - calling user callbacks. + */ template class Client : public ClientBase { @@ -309,23 +355,27 @@ class Client : public ClientBase } private: + /// \internal std::shared_ptr create_goal_response() const override { using GoalResponse = typename ACTION::GoalRequestService::Response; return std::shared_ptr(new GoalResponse()); } + /// \internal std::shared_ptr create_result_response() const override { using GoalResultResponse = typename ACTION::GoalResultService::Response; return std::shared_ptr(new GoalResultResponse()); } + /// \internal std::shared_ptr create_cancel_response() const override { return std::shared_ptr(new CancelResponse()); } + /// \internal std::shared_ptr create_feedback_message() const override { // using FeedbackMessage = typename ACTION::FeedbackMessage; @@ -333,6 +383,7 @@ class Client : public ClientBase return std::shared_ptr(new Feedback()); } + /// \internal void handle_feedback_message(std::shared_ptr message) override { std::lock_guard guard(goal_handles_mutex_); @@ -361,12 +412,14 @@ class Client : public ClientBase callback(goal_handle, feedback_message); } + /// \internal std::shared_ptr create_status_message() const override { using GoalStatusMessage = typename ACTION::GoalStatusMessage; return std::shared_ptr(new GoalStatusMessage()); } + /// \internal void handle_status_message(std::shared_ptr message) override { std::lock_guard guard(goal_handles_mutex_); @@ -394,6 +447,7 @@ class Client : public ClientBase } } + /// \internal void make_result_aware(typename GoalHandle::SharedPtr goal_handle) { using GoalResultRequest = typename ACTION::GoalResultService::Request; @@ -417,6 +471,7 @@ class Client : public ClientBase goal_handle->set_result_awareness(true); } + /// \internal std::shared_future async_cancel(typename CancelRequest::SharedPtr cancel_request) { From 8b173d5d2a3553318ca747c075cba250452a4a61 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 5 Dec 2018 09:57:36 -0800 Subject: [PATCH 40/56] Execute one thing at a time --- rclcpp_action/src/client.cpp | 18 ++++++------------ 1 file changed, 6 insertions(+), 12 deletions(-) diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index 506f6e9413..df46fb1b4f 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -305,9 +305,7 @@ ClientBase::execute() } else { this->handle_feedback_message(feedback_message); } - } - - if (pimpl_->is_status_ready) { + } else if (pimpl_->is_status_ready) { std::shared_ptr status_message = this->create_status_message(); rcl_ret_t ret = rcl_action_take_status( pimpl_->client_handle.get(), status_message.get()); @@ -317,9 +315,7 @@ ClientBase::execute() } else { this->handle_status_message(status_message); } - } - - if (pimpl_->is_goal_response_ready) { + } else if (pimpl_->is_goal_response_ready) { rmw_request_id_t response_header; std::shared_ptr goal_response = this->create_goal_response(); rcl_ret_t ret = rcl_action_take_goal_response( @@ -330,9 +326,7 @@ ClientBase::execute() } else { this->handle_goal_response(response_header, goal_response); } - } - - if (pimpl_->is_result_response_ready) { + } else if (pimpl_->is_result_response_ready) { rmw_request_id_t response_header; std::shared_ptr result_response = this->create_result_response(); rcl_ret_t ret = rcl_action_take_result_response( @@ -343,9 +337,7 @@ ClientBase::execute() } else { this->handle_result_response(response_header, result_response); } - } - - if (pimpl_->is_cancel_response_ready) { + } else if (pimpl_->is_cancel_response_ready) { rmw_request_id_t response_header; std::shared_ptr cancel_response = this->create_cancel_response(); rcl_ret_t ret = rcl_action_take_cancel_response( @@ -356,6 +348,8 @@ ClientBase::execute() } else { this->handle_cancel_response(response_header, cancel_response); } + } else { + throw std::runtime_error("Executing action client but nothing is ready"); } } From f1ed490c80b9181c5f5501c7bcd1e3ec095a826f Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 5 Dec 2018 10:06:51 -0800 Subject: [PATCH 41/56] Return nullptr instead of throwing RejectedGoalError --- rclcpp_action/include/rclcpp_action/client.hpp | 2 +- rclcpp_action/include/rclcpp_action/exceptions.hpp | 10 ---------- rclcpp_action/test/test_client.cpp | 2 +- 3 files changed, 2 insertions(+), 12 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 8aea449195..d749e67a9a 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -250,7 +250,7 @@ class Client : public ClientBase using GoalResponse = typename ACTION::GoalRequestService::Response; auto goal_response = std::static_pointer_cast(response); if (!goal_response->accepted) { - promise->set_exception(std::make_exception_ptr(exceptions::RejectedGoalError())); + promise->set_value(nullptr); return; } GoalInfo goal_info; diff --git a/rclcpp_action/include/rclcpp_action/exceptions.hpp b/rclcpp_action/include/rclcpp_action/exceptions.hpp index 4263d50b55..15bb165450 100644 --- a/rclcpp_action/include/rclcpp_action/exceptions.hpp +++ b/rclcpp_action/include/rclcpp_action/exceptions.hpp @@ -21,16 +21,6 @@ namespace rclcpp_action { namespace exceptions { - -class RejectedGoalError : public std::runtime_error -{ -public: - RejectedGoalError() - : std::runtime_error("Goal was rejected by the server.") - { - } -}; - class UnknownGoalHandleError : public std::invalid_argument { public: diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index 7043a4fe3e..23cc85c085 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -277,7 +277,7 @@ TEST_F(TestClient, async_send_goal_but_ignore_feedback_and_result) ASSERT_EQ( rclcpp::executor::FutureReturnCode::SUCCESS, client_executor.spin_until_future_complete(future_goal_handle)); - EXPECT_THROW(future_goal_handle.get(), rclcpp_action::exceptions::RejectedGoalError); + EXPECT_EQ(nullptr, future_goal_handle.get().get()); ActionGoal good_goal; good_goal.order = 5; From 4dc00d9e8e87ae675250e911c465c04a7c76e67f Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 5 Dec 2018 10:22:54 -0800 Subject: [PATCH 42/56] ClientGoalHandle worries about feedback awareness --- .../include/rclcpp_action/client.hpp | 14 +++------- .../rclcpp_action/client_goal_handle.hpp | 10 ++++--- .../rclcpp_action/client_goal_handle_impl.hpp | 27 +++++++++++++------ 3 files changed, 28 insertions(+), 23 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index d749e67a9a..13714013d2 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -205,8 +205,7 @@ class Client : public ClientBase using Feedback = typename ACTION::Feedback; using GoalHandle = ClientGoalHandle; using Result = typename GoalHandle::Result; - using FeedbackCallback = - std::function)>; + using FeedbackCallback = typename ClientGoalHandle::FeedbackCallback; using CancelRequest = typename ACTION::CancelGoalService::Request; using CancelResponse = typename ACTION::CancelGoalService::Response; @@ -401,15 +400,8 @@ class Client : public ClientBase return; } typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id]; - if (!goal_handle->is_feedback_aware()) { - RCLCPP_DEBUG( - this->get_logger(), - "Received feedback for goal but it ignores it."); - return; - } - const FeedbackCallback & callback = goal_handle->get_feedback_callback(); - // callback(goal_handle, feedback_message->feedback); - callback(goal_handle, feedback_message); + // goal_handle->call_feedback_callback(goal_handle, feedback_message->feedback); + goal_handle->call_feedback_callback(goal_handle, feedback_message); } /// \internal diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp index cd2bec2974..92bb033c1d 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp @@ -64,7 +64,7 @@ class ClientGoalHandle using Feedback = typename ACTION::Feedback; using FeedbackCallback = - std::function)>; + std::function)>; virtual ~ClientGoalHandle(); @@ -75,9 +75,6 @@ class ClientGoalHandle std::shared_future async_result(); - FeedbackCallback - get_feedback_callback(); - int8_t get_status(); @@ -96,6 +93,11 @@ class ClientGoalHandle void set_feedback_callback(FeedbackCallback callback); + void + call_feedback_callback( + ClientGoalHandle::SharedPtr shared_this, + typename std::shared_ptr feedback_message); + void set_result_awareness(bool awareness); diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp index c398e43dff..d07e37a225 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -70,14 +70,6 @@ ClientGoalHandle::set_result(const Result & result) result_promise_.set_value(result); } -template -typename ClientGoalHandle::FeedbackCallback -ClientGoalHandle::get_feedback_callback() -{ - std::lock_guard guard(handle_mutex_); - return feedback_callback_; -} - template void ClientGoalHandle::set_feedback_callback(FeedbackCallback callback) @@ -136,6 +128,25 @@ ClientGoalHandle::invalidate() exceptions::UnawareGoalHandleError())); } +template +void +ClientGoalHandle::call_feedback_callback( + ClientGoalHandle::SharedPtr shared_this, + typename std::shared_ptr feedback_message) +{ + if (shared_this.get() != this) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp_action"), "Sent feedback to wrong goal handle."); + return; + } + std::lock_guard guard(handle_mutex_); + if (feedback_callback_ == nullptr) { + // Normal, some feedback messages may arrive after the goal result. + RCLCPP_DEBUG(rclcpp::get_logger("rclcpp_action"), "Received feedback but goal ignores it."); + return; + } + feedback_callback_(shared_this, feedback_message); +} + } // namespace rclcpp_action #endif // RCLCPP_ACTION__CLIENT_GOAL_HANDLE_IMPL_HPP_ From 7489e75f2e4b1e09875bf52a0b43087cd392e937 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 5 Dec 2018 10:26:03 -0800 Subject: [PATCH 43/56] cpplint + uncrustify --- .../rclcpp_action/client_goal_handle_impl.hpp | 2 ++ rclcpp_action/test/test_client.cpp | 13 +++++++------ 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp index d07e37a225..93c1ec60b6 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -17,6 +17,8 @@ #include +#include + #include "rclcpp_action/exceptions.hpp" namespace rclcpp_action diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index 23cc85c085..b24499e9f2 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -161,8 +161,8 @@ class TestClient : public ::testing::Test std::fill(zero_uuid.begin(), zero_uuid.end(), 0u); const rclcpp::Time cancel_stamp = request->goal_info.stamp; bool cancel_all = ( - request->goal_info.goal_id.uuid == zero_uuid - && cancel_stamp == zero_stamp); + request->goal_info.goal_id.uuid == zero_uuid && + cancel_stamp == zero_stamp); ActionStatusMessage status_message; auto it = goals.begin(); while (it != goals.end()) { @@ -202,7 +202,8 @@ class TestClient : public ::testing::Test ret = rcl_action_get_status_topic_name( action_name, allocator, &status_topic_name); ASSERT_EQ(RCL_RET_OK, ret); - status_publisher = server_node->create_publisher(status_topic_name, rcl_action_qos_profile_status_default); + status_publisher = server_node->create_publisher( + status_topic_name, rcl_action_qos_profile_status_default); ASSERT_TRUE(status_publisher != nullptr); allocator.deallocate(status_topic_name, allocator.state); server_executor.add_node(server_node); @@ -233,12 +234,12 @@ class TestClient : public ::testing::Test server_executor.spin_some(); client_executor.spin_some(); status = future.wait_for(std::chrono::seconds(0)); - } while(std::future_status::ready != status); + } while (std::future_status::ready != status); } rclcpp::Clock clock{RCL_ROS_TIME}; const rclcpp::Time zero_stamp{0, 0, RCL_ROS_TIME}; - + rclcpp::Node::SharedPtr server_node; rclcpp::executors::SingleThreadedExecutor server_executor; rclcpp::Node::SharedPtr client_node; @@ -390,7 +391,7 @@ TEST_F(TestClient, async_cancel_all_goals) dual_spin_until_future_complete(future_cancel_all); auto cancel_response = future_cancel_all.get(); - ASSERT_EQ(2ul, cancel_response->goals_canceling.size()); + 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); } From f2dc2abd878ebced3da8bb36ce1df30e045f4b30 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 5 Dec 2018 10:33:29 -0800 Subject: [PATCH 44/56] Use node logging interface --- rclcpp_action/include/rclcpp_action/client.hpp | 8 +++++--- rclcpp_action/include/rclcpp_action/create_client.hpp | 3 ++- rclcpp_action/src/client.cpp | 8 ++++++-- 3 files changed, 13 insertions(+), 6 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 13714013d2..bedba2bc12 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -54,10 +54,10 @@ class ClientBaseImpl; class ClientBase : public rclcpp::Waitable { public: - // TODO(sloretz) NodeLoggingInterface when it can be gotten off a node RCLCPP_ACTION_PUBLIC ClientBase( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + 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); @@ -211,18 +211,20 @@ class Client : public ClientBase Client( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, const std::string & action_name ) - : Client(node_base, action_name, rcl_action_client_get_default_options()) + : Client(node_base, node_logging, action_name, rcl_action_client_get_default_options()) { } Client( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, const std::string & action_name, const rcl_action_client_options_t & client_options ) : ClientBase( - node_base, action_name, + node_base, node_logging, action_name, rosidl_typesupport_cpp::get_action_type_support_handle(), client_options) { diff --git a/rclcpp_action/include/rclcpp_action/create_client.hpp b/rclcpp_action/include/rclcpp_action/create_client.hpp index 7e385c0a82..599d45872c 100644 --- a/rclcpp_action/include/rclcpp_action/create_client.hpp +++ b/rclcpp_action/include/rclcpp_action/create_client.hpp @@ -63,7 +63,8 @@ create_client( }; std::shared_ptr> action_client( - new Client(node->get_node_base_interface(), name), deleter); + new Client(node->get_node_base_interface(), node->get_node_logging_interface(), name), + deleter); node->get_node_waitables_interface()->add_waitable(action_client, group); return action_client; diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index df46fb1b4f..fa1184a636 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -14,6 +14,8 @@ #include #include +#include +#include #include #include @@ -32,11 +34,12 @@ class ClientBaseImpl public: ClientBaseImpl( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + 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 & client_options) : node_handle(node_base->get_shared_rcl_node_handle()), - logger(rclcpp::get_logger(rcl_node_get_logger_name(node_handle.get())).get_child("rclcpp")), + logger(node_logging->get_logger()), random_bytes_generator(std::random_device{} ()) { std::weak_ptr weak_node_handle(node_handle); @@ -114,10 +117,11 @@ class ClientBaseImpl ClientBase::ClientBase( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + 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 & client_options) -: pimpl_(new ClientBaseImpl(node_base, action_name, type_support, client_options)) +: pimpl_(new ClientBaseImpl(node_base, node_logging, action_name, type_support, client_options)) { } From 8e0da8ff726a9eb5b7fb00677e34436e7574f619 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 5 Dec 2018 10:35:37 -0800 Subject: [PATCH 45/56] ACTION -> ActionT --- .../include/rclcpp_action/client.hpp | 36 +++++------ .../rclcpp_action/client_goal_handle.hpp | 12 ++-- .../rclcpp_action/client_goal_handle_impl.hpp | 60 +++++++++---------- .../include/rclcpp_action/create_client.hpp | 12 ++-- 4 files changed, 60 insertions(+), 60 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index bedba2bc12..ba27e329b7 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -195,19 +195,19 @@ class ClientBase : public rclcpp::Waitable * - coverting between the C++ action type and generic types for `rclcpp_action::ClientBase`, and * - calling user callbacks. */ -template +template class Client : public ClientBase { public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Client) - using Goal = typename ACTION::Goal; - using Feedback = typename ACTION::Feedback; - using GoalHandle = ClientGoalHandle; + using Goal = typename ActionT::Goal; + using Feedback = typename ActionT::Feedback; + using GoalHandle = ClientGoalHandle; using Result = typename GoalHandle::Result; - using FeedbackCallback = typename ClientGoalHandle::FeedbackCallback; - using CancelRequest = typename ACTION::CancelGoalService::Request; - using CancelResponse = typename ACTION::CancelGoalService::Response; + using FeedbackCallback = typename ClientGoalHandle::FeedbackCallback; + using CancelRequest = typename ActionT::CancelGoalService::Request; + using CancelResponse = typename ActionT::CancelGoalService::Response; Client( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, @@ -225,7 +225,7 @@ class Client : public ClientBase ) : ClientBase( node_base, node_logging, action_name, - rosidl_typesupport_cpp::get_action_type_support_handle(), + rosidl_typesupport_cpp::get_action_type_support_handle(), client_options) { } @@ -237,7 +237,7 @@ class Client : public ClientBase // Put promise in the heap to move it around. auto promise = std::make_shared>(); std::shared_future future(promise->get_future()); - using GoalRequest = typename ACTION::GoalRequestService::Request; + using GoalRequest = typename ActionT::GoalRequestService::Request; // auto goal_request = std::make_shared(); // goal_request->goal_id = this->generate_goal_id(); // goal_request->goal = goal; @@ -248,7 +248,7 @@ class Client : public ClientBase [this, goal_request, callback, ignore_result, promise]( std::shared_ptr response) mutable { - using GoalResponse = typename ACTION::GoalRequestService::Response; + using GoalResponse = typename ActionT::GoalRequestService::Response; auto goal_response = std::static_pointer_cast(response); if (!goal_response->accepted) { promise->set_value(nullptr); @@ -359,14 +359,14 @@ class Client : public ClientBase /// \internal std::shared_ptr create_goal_response() const override { - using GoalResponse = typename ACTION::GoalRequestService::Response; + using GoalResponse = typename ActionT::GoalRequestService::Response; return std::shared_ptr(new GoalResponse()); } /// \internal std::shared_ptr create_result_response() const override { - using GoalResultResponse = typename ACTION::GoalResultService::Response; + using GoalResultResponse = typename ActionT::GoalResultService::Response; return std::shared_ptr(new GoalResultResponse()); } @@ -379,7 +379,7 @@ class Client : public ClientBase /// \internal std::shared_ptr create_feedback_message() const override { - // using FeedbackMessage = typename ACTION::FeedbackMessage; + // using FeedbackMessage = typename ActionT::FeedbackMessage; // return std::shared_ptr(new FeedbackMessage()); return std::shared_ptr(new Feedback()); } @@ -388,7 +388,7 @@ class Client : public ClientBase void handle_feedback_message(std::shared_ptr message) override { std::lock_guard guard(goal_handles_mutex_); - // using FeedbackMessage = typename ACTION::FeedbackMessage; + // using FeedbackMessage = typename ActionT::FeedbackMessage; // typename FeedbackMessage::SharedPtr feedback_message = // std::static_pointer_cast(message); typename Feedback::SharedPtr feedback_message = @@ -409,7 +409,7 @@ class Client : public ClientBase /// \internal std::shared_ptr create_status_message() const override { - using GoalStatusMessage = typename ACTION::GoalStatusMessage; + using GoalStatusMessage = typename ActionT::GoalStatusMessage; return std::shared_ptr(new GoalStatusMessage()); } @@ -417,7 +417,7 @@ class Client : public ClientBase void handle_status_message(std::shared_ptr message) override { std::lock_guard guard(goal_handles_mutex_); - using GoalStatusMessage = typename ACTION::GoalStatusMessage; + using GoalStatusMessage = typename ActionT::GoalStatusMessage; auto status_message = std::static_pointer_cast(message); for (const GoalStatus & status : status_message->status_list) { // const GoalID & goal_id = status.goal_info.goal_id; @@ -444,7 +444,7 @@ class Client : public ClientBase /// \internal void make_result_aware(typename GoalHandle::SharedPtr goal_handle) { - using GoalResultRequest = typename ACTION::GoalResultService::Request; + using GoalResultRequest = typename ActionT::GoalResultService::Request; auto goal_result_request = std::make_shared(); // goal_result_request.goal_id = goal_handle->get_goal_id(); goal_result_request->uuid = goal_handle->get_goal_id(); @@ -454,7 +454,7 @@ class Client : public ClientBase { // Wrap the response in a struct with the fields a user cares about Result result; - using GoalResultResponse = typename ACTION::GoalResultService::Response; + using GoalResultResponse = typename ActionT::GoalResultService::Response; result.response = std::static_pointer_cast(response); result.goal_id = goal_handle->get_goal_id(); result.code = static_cast(result.response->status); diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp index 92bb033c1d..17f2f42320 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp @@ -42,10 +42,10 @@ enum class ResultCode : int8_t // Forward declarations -template +template class Client; -template +template class ClientGoalHandle { public: @@ -59,10 +59,10 @@ class ClientGoalHandle /// A status to indicate if the goal was canceled, aborted, or suceeded ResultCode code; /// User defined fields sent back with an action - typename ACTION::Result::SharedPtr response; + typename ActionT::Result::SharedPtr response; } Result; - using Feedback = typename ACTION::Feedback; + using Feedback = typename ActionT::Feedback; using FeedbackCallback = std::function)>; @@ -86,7 +86,7 @@ class ClientGoalHandle private: // The templated Client creates goal handles - friend Client; + friend Client; ClientGoalHandle(const GoalInfo & info, FeedbackCallback callback); @@ -95,7 +95,7 @@ class ClientGoalHandle void call_feedback_callback( - ClientGoalHandle::SharedPtr shared_this, + ClientGoalHandle::SharedPtr shared_this, typename std::shared_ptr feedback_message); void diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp index 93c1ec60b6..4557277e41 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -24,37 +24,37 @@ namespace rclcpp_action { -template -ClientGoalHandle::ClientGoalHandle( +template +ClientGoalHandle::ClientGoalHandle( const GoalInfo & info, FeedbackCallback callback) : info_(info), result_future_(result_promise_.get_future()), feedback_callback_(callback) { } -template -ClientGoalHandle::~ClientGoalHandle() +template +ClientGoalHandle::~ClientGoalHandle() { } -template +template const GoalID & -ClientGoalHandle::get_goal_id() const +ClientGoalHandle::get_goal_id() const { // return info_.goal_id; return info_.goal_id.uuid; } -template +template rclcpp::Time -ClientGoalHandle::get_goal_stamp() const +ClientGoalHandle::get_goal_stamp() const { return info_.stamp; } -template -std::shared_future::Result> -ClientGoalHandle::async_result() +template +std::shared_future::Result> +ClientGoalHandle::async_result() { std::lock_guard guard(handle_mutex_); if (!is_result_aware_) { @@ -63,66 +63,66 @@ ClientGoalHandle::async_result() return result_future_; } -template +template void -ClientGoalHandle::set_result(const Result & result) +ClientGoalHandle::set_result(const Result & result) { std::lock_guard guard(handle_mutex_); status_ = static_cast(result.code); result_promise_.set_value(result); } -template +template void -ClientGoalHandle::set_feedback_callback(FeedbackCallback callback) +ClientGoalHandle::set_feedback_callback(FeedbackCallback callback) { std::lock_guard guard(handle_mutex_); feedback_callback_ = callback; } -template +template int8_t -ClientGoalHandle::get_status() +ClientGoalHandle::get_status() { std::lock_guard guard(handle_mutex_); return status_; } -template +template void -ClientGoalHandle::set_status(int8_t status) +ClientGoalHandle::set_status(int8_t status) { std::lock_guard guard(handle_mutex_); status_ = status; } -template +template bool -ClientGoalHandle::is_feedback_aware() +ClientGoalHandle::is_feedback_aware() { std::lock_guard guard(handle_mutex_); return feedback_callback_ != nullptr; } -template +template bool -ClientGoalHandle::is_result_aware() +ClientGoalHandle::is_result_aware() { std::lock_guard guard(handle_mutex_); return is_result_aware_; } -template +template void -ClientGoalHandle::set_result_awareness(bool awareness) +ClientGoalHandle::set_result_awareness(bool awareness) { std::lock_guard guard(handle_mutex_); is_result_aware_ = awareness; } -template +template void -ClientGoalHandle::invalidate() +ClientGoalHandle::invalidate() { std::lock_guard guard(handle_mutex_); status_ = GoalStatus::STATUS_UNKNOWN; @@ -130,10 +130,10 @@ ClientGoalHandle::invalidate() exceptions::UnawareGoalHandleError())); } -template +template void -ClientGoalHandle::call_feedback_callback( - ClientGoalHandle::SharedPtr shared_this, +ClientGoalHandle::call_feedback_callback( + ClientGoalHandle::SharedPtr shared_this, typename std::shared_ptr feedback_message) { if (shared_this.get() != this) { diff --git a/rclcpp_action/include/rclcpp_action/create_client.hpp b/rclcpp_action/include/rclcpp_action/create_client.hpp index 599d45872c..730083f24e 100644 --- a/rclcpp_action/include/rclcpp_action/create_client.hpp +++ b/rclcpp_action/include/rclcpp_action/create_client.hpp @@ -25,8 +25,8 @@ namespace rclcpp_action { -template -typename Client::SharedPtr +template +typename Client::SharedPtr create_client( rclcpp::Node::SharedPtr node, const std::string & name, @@ -37,7 +37,7 @@ create_client( std::weak_ptr weak_group = group; bool group_is_null = (nullptr == group.get()); - auto deleter = [weak_node, weak_group, group_is_null](Client * ptr) + auto deleter = [weak_node, weak_group, group_is_null](Client * ptr) { if (nullptr == ptr) { return; @@ -47,7 +47,7 @@ create_client( return; } // API expects a shared pointer, give it one with a deleter that does nothing. - std::shared_ptr> fake_shared_ptr(ptr, [](Client *) {}); + std::shared_ptr> fake_shared_ptr(ptr, [](Client *) {}); if (group_is_null) { // Was added to default group @@ -62,8 +62,8 @@ create_client( delete ptr; }; - std::shared_ptr> action_client( - new Client(node->get_node_base_interface(), node->get_node_logging_interface(), name), + std::shared_ptr> action_client( + new Client(node->get_node_base_interface(), node->get_node_logging_interface(), name), deleter); node->get_node_waitables_interface()->add_waitable(action_client, group); From 62c27a4fe034e0b567495e4dfd9524decc295e0c Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 5 Dec 2018 10:37:04 -0800 Subject: [PATCH 46/56] Make ClientBase constructor protected --- rclcpp_action/include/rclcpp_action/client.hpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index ba27e329b7..93b140877c 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -54,14 +54,6 @@ class ClientBaseImpl; class ClientBase : public rclcpp::Waitable { public: - RCLCPP_ACTION_PUBLIC - ClientBase( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, - 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); - RCLCPP_ACTION_PUBLIC virtual ~ClientBase(); @@ -112,6 +104,14 @@ class ClientBase : public rclcpp::Waitable // ----------------- protected: + RCLCPP_ACTION_PUBLIC + ClientBase( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + 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); + // ----------------------------------------------------- // API for communication between ClientBase and Client<> using ResponseCallback = std::function response)>; From cc2d03810af5f6f5695233f76704159295b23e81 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 5 Dec 2018 10:41:34 -0800 Subject: [PATCH 47/56] Return types on different line --- .../include/rclcpp_action/client.hpp | 86 +++++++++++++------ .../rclcpp_action/client_goal_handle.hpp | 6 +- 2 files changed, 66 insertions(+), 26 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 93b140877c..f2d29df621 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -122,61 +122,89 @@ class ClientBase : public rclcpp::Waitable /// \internal RCLCPP_ACTION_PUBLIC - virtual GoalID generate_goal_id(); + virtual + GoalID + generate_goal_id(); /// \internal RCLCPP_ACTION_PUBLIC - virtual void send_goal_request( + virtual + void + send_goal_request( std::shared_ptr request, ResponseCallback callback); /// \internal RCLCPP_ACTION_PUBLIC - virtual void send_result_request( + virtual + void + send_result_request( std::shared_ptr request, ResponseCallback callback); /// \internal RCLCPP_ACTION_PUBLIC - virtual void send_cancel_request( + virtual + void + send_cancel_request( std::shared_ptr request, ResponseCallback callback); /// \internal - virtual std::shared_ptr create_goal_response() const = 0; + virtual + std::shared_ptr + create_goal_response() const = 0; /// \internal - virtual void handle_goal_response( + virtual + void + handle_goal_response( const rmw_request_id_t & response_header, std::shared_ptr goal_response); /// \internal - virtual std::shared_ptr create_result_response() const = 0; + virtual + std::shared_ptr + create_result_response() const = 0; /// \internal - virtual void handle_result_response( + virtual + void + handle_result_response( const rmw_request_id_t & response_header, std::shared_ptr result_response); /// \internal - virtual std::shared_ptr create_cancel_response() const = 0; + virtual + std::shared_ptr + create_cancel_response() const = 0; /// \internal - virtual void handle_cancel_response( + virtual + void + handle_cancel_response( const rmw_request_id_t & response_header, std::shared_ptr cancel_response); /// \internal - virtual std::shared_ptr create_feedback_message() const = 0; + virtual + std::shared_ptr + create_feedback_message() const = 0; /// \internal - virtual void handle_feedback_message(std::shared_ptr message) = 0; + virtual + void + handle_feedback_message(std::shared_ptr message) = 0; /// \internal - virtual std::shared_ptr create_status_message() const = 0; + virtual + std::shared_ptr + create_status_message() const = 0; /// \internal - virtual void handle_status_message(std::shared_ptr message) = 0; + virtual + void + handle_status_message(std::shared_ptr message) = 0; // End API for communication between ClientBase and Client<> // --------------------------------------------------------- @@ -231,7 +259,8 @@ class Client : public ClientBase } RCLCPP_ACTION_PUBLIC - std::shared_future async_send_goal( + std::shared_future + async_send_goal( const Goal & goal, FeedbackCallback callback = nullptr, bool ignore_result = false) { // Put promise in the heap to move it around. @@ -345,7 +374,8 @@ class Client : public ClientBase return async_cancel(cancel_request); } - virtual ~Client() + virtual + ~Client() { std::lock_guard guard(goal_handles_mutex_); auto it = goal_handles_.begin(); @@ -357,27 +387,31 @@ class Client : public ClientBase private: /// \internal - std::shared_ptr create_goal_response() const override + std::shared_ptr + create_goal_response() const override { using GoalResponse = typename ActionT::GoalRequestService::Response; return std::shared_ptr(new GoalResponse()); } /// \internal - std::shared_ptr create_result_response() const override + std::shared_ptr + create_result_response() const override { using GoalResultResponse = typename ActionT::GoalResultService::Response; return std::shared_ptr(new GoalResultResponse()); } /// \internal - std::shared_ptr create_cancel_response() const override + std::shared_ptr + create_cancel_response() const override { return std::shared_ptr(new CancelResponse()); } /// \internal - std::shared_ptr create_feedback_message() const override + std::shared_ptr + create_feedback_message() const override { // using FeedbackMessage = typename ActionT::FeedbackMessage; // return std::shared_ptr(new FeedbackMessage()); @@ -385,7 +419,8 @@ class Client : public ClientBase } /// \internal - void handle_feedback_message(std::shared_ptr message) override + void + handle_feedback_message(std::shared_ptr message) override { std::lock_guard guard(goal_handles_mutex_); // using FeedbackMessage = typename ActionT::FeedbackMessage; @@ -407,14 +442,16 @@ class Client : public ClientBase } /// \internal - std::shared_ptr create_status_message() const override + std::shared_ptr + create_status_message() const override { using GoalStatusMessage = typename ActionT::GoalStatusMessage; return std::shared_ptr(new GoalStatusMessage()); } /// \internal - void handle_status_message(std::shared_ptr message) override + void + handle_status_message(std::shared_ptr message) override { std::lock_guard guard(goal_handles_mutex_); using GoalStatusMessage = typename ActionT::GoalStatusMessage; @@ -442,7 +479,8 @@ class Client : public ClientBase } /// \internal - void make_result_aware(typename GoalHandle::SharedPtr goal_handle) + void + make_result_aware(typename GoalHandle::SharedPtr goal_handle) { using GoalResultRequest = typename ActionT::GoalResultService::Request; auto goal_result_request = std::make_shared(); diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp index 17f2f42320..a4b595304e 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp @@ -68,9 +68,11 @@ class ClientGoalHandle virtual ~ClientGoalHandle(); - const GoalID & get_goal_id() const; + const GoalID & + get_goal_id() const; - rclcpp::Time get_goal_stamp() const; + rclcpp::Time + get_goal_stamp() const; std::shared_future async_result(); From 5797e51b4201af7422d4dc42f817f7742de4e948 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 5 Dec 2018 10:48:40 -0800 Subject: [PATCH 48/56] Avoid passing const reference to temporary --- rclcpp_action/include/rclcpp_action/client.hpp | 12 ++---------- 1 file changed, 2 insertions(+), 10 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index f2d29df621..9b76035d46 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -240,16 +240,8 @@ class Client : public ClientBase Client( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, - const std::string & action_name - ) - : Client(node_base, node_logging, action_name, rcl_action_client_get_default_options()) - { - } - - Client( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, - const std::string & action_name, const rcl_action_client_options_t & client_options + const std::string & action_name, + const rcl_action_client_options_t client_options = rcl_action_client_get_default_options() ) : ClientBase( node_base, node_logging, action_name, From df0a3fa300cc0c76820ce246558bf9ef7847dcbf Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 5 Dec 2018 11:03:23 -0800 Subject: [PATCH 49/56] Child logger rclcpp_action --- rclcpp_action/src/client.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index fa1184a636..9e8669f643 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -39,7 +39,7 @@ class ClientBaseImpl const rosidl_action_type_support_t * type_support, const rcl_action_client_options_t & client_options) : node_handle(node_base->get_shared_rcl_node_handle()), - logger(node_logging->get_logger()), + logger(node_logging->get_logger().get_child("rclcpp_acton")), random_bytes_generator(std::random_device{} ()) { std::weak_ptr weak_node_handle(node_handle); @@ -50,7 +50,7 @@ class ClientBaseImpl if (handle) { if (RCL_RET_OK != rcl_action_client_fini(client, handle.get())) { RCLCPP_ERROR( - rclcpp::get_logger(rcl_node_get_logger_name(handle.get())).get_child("rclcpp"), + rclcpp::get_logger(rcl_node_get_logger_name(handle.get())).get_child("rclcpp_action"), "Error in destruction of rcl action client handle: %s", rcl_get_error_string().str); rcl_reset_error(); } From fa391a1f8fb164ec3739d6cf933eaf55bd47a362 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 5 Dec 2018 11:04:34 -0800 Subject: [PATCH 50/56] Child logger rclcpp_action --- rclcpp_action/src/client.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index 9e8669f643..c4f9c05134 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -56,7 +56,7 @@ class ClientBaseImpl } } else { RCLCPP_ERROR( - rclcpp::get_logger("rclcpp"), + rclcpp::get_logger("rclcpp_action"), "Error in destruction of rcl action client handle: " "the Node Handle was destructed too early. You will leak memory"); } From 175213e3cf92a32e7f516c5cc9e8a84a65c48d36 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 5 Dec 2018 12:11:07 -0800 Subject: [PATCH 51/56] possible windows fixes --- rclcpp_action/include/rclcpp_action/client.hpp | 2 +- rclcpp_action/include/rclcpp_action/client_goal_handle.hpp | 5 +++-- .../include/rclcpp_action/client_goal_handle_impl.hpp | 2 +- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 9b76035d46..f8d2705d77 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -227,7 +227,7 @@ template class Client : public ClientBase { public: - RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Client) + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Client) using Goal = typename ActionT::Goal; using Feedback = typename ActionT::Feedback; diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp index a4b595304e..03ae985861 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp @@ -64,7 +64,8 @@ class ClientGoalHandle using Feedback = typename ActionT::Feedback; using FeedbackCallback = - std::function)>; + std::function::SharedPtr, const std::shared_ptr)>; virtual ~ClientGoalHandle(); @@ -97,7 +98,7 @@ class ClientGoalHandle void call_feedback_callback( - ClientGoalHandle::SharedPtr shared_this, + typename ClientGoalHandle::SharedPtr shared_this, typename std::shared_ptr feedback_message); void diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp index 4557277e41..8b06d8ae16 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -133,7 +133,7 @@ ClientGoalHandle::invalidate() template void ClientGoalHandle::call_feedback_callback( - ClientGoalHandle::SharedPtr shared_this, + typename ClientGoalHandle::SharedPtr shared_this, typename std::shared_ptr feedback_message) { if (shared_this.get() != this) { From 7cb39d816995f7909e662c4cd93161e53a24781b Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 5 Dec 2018 12:11:37 -0800 Subject: [PATCH 52/56] remove excess space --- rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp index 8b06d8ae16..25542c9fc0 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -51,7 +51,6 @@ ClientGoalHandle::get_goal_stamp() const return info_.stamp; } - template std::shared_future::Result> ClientGoalHandle::async_result() From 05cfd8ead2fe6ef50fa0b907e524c6cdbb5a65fe Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 5 Dec 2018 12:12:27 -0800 Subject: [PATCH 53/56] swap argument order --- rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp index 25542c9fc0..e473b6b8a6 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -140,7 +140,7 @@ ClientGoalHandle::call_feedback_callback( return; } std::lock_guard guard(handle_mutex_); - if (feedback_callback_ == nullptr) { + if (nullptr == feedback_callback_) { // Normal, some feedback messages may arrive after the goal result. RCLCPP_DEBUG(rclcpp::get_logger("rclcpp_action"), "Received feedback but goal ignores it."); return; From 9b63b4756e731158c8ce8d6bac0d4cf455a74360 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 5 Dec 2018 12:16:45 -0800 Subject: [PATCH 54/56] Misc test additions --- rclcpp_action/test/test_client.cpp | 14 ++++---------- 1 file changed, 4 insertions(+), 10 deletions(-) diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index b24499e9f2..22c3020452 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -273,21 +273,13 @@ TEST_F(TestClient, async_send_goal_but_ignore_feedback_and_result) ActionGoal bad_goal; bad_goal.order = -5; auto future_goal_handle = action_client->async_send_goal(bad_goal, nullptr, true); - server_executor.spin_once(); - server_executor.spin_some(); - ASSERT_EQ( - rclcpp::executor::FutureReturnCode::SUCCESS, - client_executor.spin_until_future_complete(future_goal_handle)); + dual_spin_until_future_complete(future_goal_handle); EXPECT_EQ(nullptr, future_goal_handle.get().get()); ActionGoal good_goal; good_goal.order = 5; future_goal_handle = action_client->async_send_goal(good_goal, nullptr, true); - server_executor.spin_once(); - server_executor.spin_some(); - ASSERT_EQ( - rclcpp::executor::FutureReturnCode::SUCCESS, - client_executor.spin_until_future_complete(future_goal_handle)); + 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()); EXPECT_FALSE(goal_handle->is_feedback_aware()); @@ -394,6 +386,8 @@ TEST_F(TestClient, async_cancel_all_goals) 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) From a4b1cb43a79228b2ca671f04eeee3bc827382730 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 5 Dec 2018 12:44:33 -0800 Subject: [PATCH 55/56] Windows independent_bits_engine can't do uint8_t --- rclcpp_action/src/client.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index c4f9c05134..c8b3de30d2 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -112,7 +112,7 @@ class ClientBaseImpl std::mutex cancel_requests_mutex; std::independent_bits_engine< - std::default_random_engine, 8, uint8_t> random_bytes_generator; + std::default_random_engine, 8, unsigned int> random_bytes_generator; }; ClientBase::ClientBase( From ffa4dce2f3a64f278153a307a9b668589fb330a4 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 5 Dec 2018 13:40:31 -0800 Subject: [PATCH 56/56] Windows link issues --- rclcpp_action/include/rclcpp_action/client.hpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index f8d2705d77..531f705d75 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -156,6 +156,7 @@ class ClientBase : public rclcpp::Waitable create_goal_response() const = 0; /// \internal + RCLCPP_ACTION_PUBLIC virtual void handle_goal_response( @@ -168,6 +169,7 @@ class ClientBase : public rclcpp::Waitable create_result_response() const = 0; /// \internal + RCLCPP_ACTION_PUBLIC virtual void handle_result_response( @@ -180,6 +182,7 @@ class ClientBase : public rclcpp::Waitable create_cancel_response() const = 0; /// \internal + RCLCPP_ACTION_PUBLIC virtual void handle_cancel_response( @@ -250,7 +253,6 @@ class Client : public ClientBase { } - RCLCPP_ACTION_PUBLIC std::shared_future async_send_goal( const Goal & goal, FeedbackCallback callback = nullptr, bool ignore_result = false) @@ -296,7 +298,6 @@ class Client : public ClientBase return future; } - RCLCPP_ACTION_PUBLIC std::shared_future async_get_result(typename GoalHandle::SharedPtr goal_handle) { @@ -311,7 +312,6 @@ class Client : public ClientBase return goal_handle->async_result(); } - RCLCPP_ACTION_PUBLIC std::shared_future async_cancel_goal(typename GoalHandle::SharedPtr goal_handle) { @@ -341,7 +341,6 @@ class Client : public ClientBase return future; } - RCLCPP_ACTION_PUBLIC std::shared_future async_cancel_all_goals() { @@ -353,7 +352,6 @@ class Client : public ClientBase return async_cancel(cancel_request); } - RCLCPP_ACTION_PUBLIC std::shared_future async_cancel_goals_before(const rclcpp::Time & stamp) {