From 91167393ea4ac97ea7d561d563f303c70c6d9fc7 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 5 Dec 2018 14:51:23 -0800 Subject: [PATCH] [rclcpp_action] Action client implementation (#594) * WIP * Removed async_cancel from action ClintGoalHandle API * Added status handler to action client goal handler * Added result handler to action client goal handler * Identation fix * Added get/set for action client goal handler * Changed action client goal handler attrs from rcl to cpp versions * Added check methods to action client goal handler * Removed rcl_client pointer from action client goal handler * Added basic waitable interface to action client * Updated waitable execute from action client * Added throw for rcl calls in action client * Removed duplicated ready flags from action client * Minor fix * Added header to action ClientBaseImpl execute * Mich's update to action client interface * Added trailing suffix to client pimpl attrs * Towards a consistent action client * Misc fixes for the action client * Yet more misc fixes for the action client * Few more fixes and shortcuts to deal with missing type support. * Fixed lint errors in action headers and client * Fixes to action client internal workflow. * Misc fixes to get client example to build * More misck client fixes * Remove debug print * replace logging with throw_from_rcl_error * Wrap result object given by client to user * Fix a couple bugs trying to cancel goals * Use unique_indentifier_msgs * create_client accepts group and removes waitable * Uncrustify fixes * [rclcpp_action] Adds tests for action client. * [WIP] Failing action client tests. * [rclcpp_action] Action client tests passing. * Spin both executors to make tests pass on my machine * Feedback callback uses shared pointer * comment about why make_result_aware is called * Client documentation * Execute one thing at a time * Return nullptr instead of throwing RejectedGoalError * ClientGoalHandle worries about feedback awareness * cpplint + uncrustify * Use node logging interface * ACTION -> ActionT * Make ClientBase constructor protected * Return types on different line * Avoid passing const reference to temporary * Child logger rclcpp_action * Child logger rclcpp_action * possible windows fixes * remove excess space * swap argument order * Misc test additions * Windows independent_bits_engine can't do uint8_t * Windows link issues --- .../include/rclcpp_action/client.hpp | 499 ++++++++++++++++-- .../rclcpp_action/client_goal_handle.hpp | 101 +++- .../rclcpp_action/client_goal_handle_impl.hpp | 134 ++++- .../include/rclcpp_action/create_client.hpp | 50 +- .../include/rclcpp_action/exceptions.hpp | 46 ++ rclcpp_action/include/rclcpp_action/types.hpp | 48 ++ rclcpp_action/src/client.cpp | 337 +++++++++++- rclcpp_action/test/test_client.cpp | 386 +++++++++++++- 8 files changed, 1510 insertions(+), 91 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 8a227f0..531f705 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -15,17 +15,26 @@ #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" @@ -35,65 +44,475 @@ namespace rclcpp_action class ClientBaseImpl; /// Base Action Client implementation -/// It is responsible for interfacing with the C action client API. -class ClientBase +/// \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: - // 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); - RCLCPP_ACTION_PUBLIC virtual ~ClientBase(); - // TODO(sloretz) add a link between this class and callbacks in the templated + // ------------- + // 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: + 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)>; + + /// \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); + + /// \internal + virtual + std::shared_ptr + create_goal_response() const = 0; + + /// \internal + RCLCPP_ACTION_PUBLIC + 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 + RCLCPP_ACTION_PUBLIC + 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 + RCLCPP_ACTION_PUBLIC + 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. -template +/// 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 { public: - RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Client) + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Client) - using FeedbackCallback = std::function>, const typename ACTION::Feedback)>; + 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 ActionT::CancelGoalService::Request; + using CancelResponse = typename ActionT::CancelGoalService::Response; Client( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, - const std::string & name + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + const std::string & action_name, + const rcl_action_client_options_t client_options = rcl_action_client_get_default_options() ) : ClientBase( - node_base, - name, - rosidl_typesupport_cpp::get_action_type_support_handle()) - { - // TODO(sloretz) what's the link that causes a feedback topic to be called ? - } - - ClientGoalHandle - async_send_goal(typename ACTION::Goal * goal, FeedbackCallback callback = nullptr); - - RCLCPP_ACTION_PUBLIC - std::future - async_cancel_all_goals(); - - RCLCPP_ACTION_PUBLIC - std::future - async_cancel_goals_before(rclcpp::Time); - - virtual ~Client() + node_base, node_logging, action_name, + rosidl_typesupport_cpp::get_action_type_support_handle(), + client_options) { } + + 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. + auto promise = std::make_shared>(); + std::shared_future future(promise->get_future()); + using GoalRequest = typename ActionT::GoalRequestService::Request; + // 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, callback, ignore_result, promise]( + std::shared_ptr response) mutable + { + using GoalResponse = typename ActionT::GoalRequestService::Response; + auto goal_response = std::static_pointer_cast(response); + if (!goal_response->accepted) { + promise->set_value(nullptr); + return; + } + GoalInfo goal_info; + // goal_info.goal_id = goal_request->goal_id; + 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)); + if (!ignore_result) { + try { + this->make_result_aware(goal_handle); + } catch (...) { + promise->set_exception(std::current_exception()); + return; + } + } + std::lock_guard guard(goal_handles_mutex_); + goal_handles_[goal_handle->get_goal_id()] = goal_handle; + promise->set_value(goal_handle); + }); + return future; + } + + std::shared_future + 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(); + } + // 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); + } + return goal_handle->async_result(); + } + + std::shared_future + 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(); + } + // 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.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 + { + 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]; + // goal_canceled = (canceled_goal_info.goal_id == goal_handle->get_goal_id()); + goal_canceled = (canceled_goal_info.goal_id.uuid == goal_handle->get_goal_id()); + } + promise->set_value(goal_canceled); + }); + return future; + } + + std::shared_future + async_cancel_all_goals() + { + auto cancel_request = std::make_shared(); + // std::fill(cancel_request->goal_info.goal_id.uuid, 0u); + std::fill( + cancel_request->goal_info.goal_id.uuid.begin(), + cancel_request->goal_info.goal_id.uuid.end(), 0u); + return async_cancel(cancel_request); + } + + 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); + 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); + } + + virtual + ~Client() + { + std::lock_guard guard(goal_handles_mutex_); + auto it = goal_handles_.begin(); + while (it != goal_handles_.end()) { + it->second->invalidate(); + it = goal_handles_.erase(it); + } + } + +private: + /// \internal + 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 + { + using GoalResultResponse = typename ActionT::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 ActionT::FeedbackMessage; + // return std::shared_ptr(new FeedbackMessage()); + return std::shared_ptr(new Feedback()); + } + + /// \internal + void + handle_feedback_message(std::shared_ptr message) override + { + std::lock_guard guard(goal_handles_mutex_); + // using FeedbackMessage = typename ActionT::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(), + "Received feedback for unknown goal. Ignoring..."); + return; + } + typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id]; + // goal_handle->call_feedback_callback(goal_handle, feedback_message->feedback); + goal_handle->call_feedback_callback(goal_handle, feedback_message); + } + + /// \internal + 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 + { + std::lock_guard guard(goal_handles_mutex_); + 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; + const GoalID & goal_id = status.goal_info.goal_id.uuid; + if (goal_handles_.count(goal_id) == 0) { + RCLCPP_DEBUG( + this->get_logger(), + "Received status for unknown goal. Ignoring..."); + continue; + } + 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_SUCCEEDED || + goal_status == GoalStatus::STATUS_CANCELED || + goal_status == GoalStatus::STATUS_ABORTED) + { + goal_handles_.erase(goal_id); + } + } + } + + /// \internal + void + make_result_aware(typename GoalHandle::SharedPtr goal_handle) + { + 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(); + this->send_result_request( + 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 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); + goal_handle->set_result(result); + std::lock_guard lock(goal_handles_mutex_); + goal_handles_.erase(goal_handle->get_goal_id()); + }); + goal_handle->set_result_awareness(true); + } + + /// \internal + std::shared_future + async_cancel(typename CancelRequest::SharedPtr cancel_request) + { + // 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::shared_ptr response) mutable + { + auto 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 87a663c..03ae985 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp @@ -17,39 +17,112 @@ #include -#include -#include +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp_action/types.hpp" #include "rclcpp_action/visibility_control.hpp" 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 +template class Client; -template +template class ClientGoalHandle { public: + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ClientGoalHandle) + + // 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 ActionT::Result::SharedPtr response; + } Result; + + using Feedback = typename ActionT::Feedback; + using FeedbackCallback = + std::function::SharedPtr, const std::shared_ptr)>; + virtual ~ClientGoalHandle(); - /// TODO(sloretz) examples have this on client as `async_cancel_goal(goal_handle)` - std::future - async_cancel(); + const GoalID & + get_goal_id() const; - std::future + rclcpp::Time + get_goal_stamp() const; + + std::shared_future async_result(); + int8_t + get_status(); + + bool + is_feedback_aware(); + + bool + is_result_aware(); + private: - // The templated Server creates goal handles - friend Client; + // The templated Client creates goal handles + friend Client; - ClientGoalHandle(rcl_action_client_t * rcl_client, const rcl_action_goal_info_t rcl_info); + ClientGoalHandle(const GoalInfo & info, FeedbackCallback 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_; + void + set_feedback_callback(FeedbackCallback callback); + + void + call_feedback_callback( + typename ClientGoalHandle::SharedPtr shared_this, + typename std::shared_ptr feedback_message); + + void + set_result_awareness(bool awareness); + + void + set_status(int8_t status); + + void + set_result(const Result & result); + + void + 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::STATUS_ACCEPTED}; + + 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 cd2bf1a..e473b6b 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -17,35 +17,137 @@ #include +#include + +#include "rclcpp_action/exceptions.hpp" + namespace rclcpp_action { -template -ClientGoalHandle::ClientGoalHandle( - rcl_action_client_t * rcl_client, - const rcl_action_goal_info_t rcl_info -) -: rcl_client_(rcl_client), rcl_info_(rcl_info) + +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 -std::future -ClientGoalHandle::async_cancel() +template +const GoalID & +ClientGoalHandle::get_goal_id() const { - throw std::runtime_error("Failed to cancel goal"); + // return info_.goal_id; + return info_.goal_id.uuid; } -template -std::future -ClientGoalHandle::async_result() +template +rclcpp::Time +ClientGoalHandle::get_goal_stamp() const { - throw std::runtime_error("Failed to get result future"); + return info_.stamp; } + +template +std::shared_future::Result> +ClientGoalHandle::async_result() +{ + std::lock_guard guard(handle_mutex_); + if (!is_result_aware_) { + throw exceptions::UnawareGoalHandleError(); + } + return result_future_; +} + +template +void +ClientGoalHandle::set_result(const Result & result) +{ + std::lock_guard guard(handle_mutex_); + status_ = static_cast(result.code); + result_promise_.set_value(result); +} + +template +void +ClientGoalHandle::set_feedback_callback(FeedbackCallback callback) +{ + std::lock_guard guard(handle_mutex_); + feedback_callback_ = callback; +} + +template +int8_t +ClientGoalHandle::get_status() +{ + std::lock_guard guard(handle_mutex_); + return status_; +} + +template +void +ClientGoalHandle::set_status(int8_t status) +{ + std::lock_guard guard(handle_mutex_); + status_ = status; +} + +template +bool +ClientGoalHandle::is_feedback_aware() +{ + std::lock_guard guard(handle_mutex_); + return feedback_callback_ != nullptr; +} + +template +bool +ClientGoalHandle::is_result_aware() +{ + std::lock_guard guard(handle_mutex_); + return is_result_aware_; +} + +template +void +ClientGoalHandle::set_result_awareness(bool awareness) +{ + std::lock_guard guard(handle_mutex_); + is_result_aware_ = awareness; +} + +template +void +ClientGoalHandle::invalidate() +{ + std::lock_guard guard(handle_mutex_); + status_ = GoalStatus::STATUS_UNKNOWN; + result_promise_.set_exception(std::make_exception_ptr( + exceptions::UnawareGoalHandleError())); +} + +template +void +ClientGoalHandle::call_feedback_callback( + typename 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 (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; + } + feedback_callback_(shared_this, feedback_message); +} + } // namespace rclcpp_action #endif // RCLCPP_ACTION__CLIENT_GOAL_HANDLE_IMPL_HPP_ diff --git a/rclcpp_action/include/rclcpp_action/create_client.hpp b/rclcpp_action/include/rclcpp_action/create_client.hpp index 8f6381c..730083f 100644 --- a/rclcpp_action/include/rclcpp_action/create_client.hpp +++ b/rclcpp_action/include/rclcpp_action/create_client.hpp @@ -25,16 +25,50 @@ namespace rclcpp_action { -template -typename Client::SharedPtr +template +typename Client::SharedPtr create_client( - rclcpp::Node * node, - const std::string & name) + rclcpp::Node::SharedPtr node, + const std::string & name, + rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr) { - return Client::make_shared( - node->get_node_base_interface(), - 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(), node->get_node_logging_interface(), name), + deleter); + + node->get_node_waitables_interface()->add_waitable(action_client, group); + return action_client; } } // 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 0000000..15bb165 --- /dev/null +++ b/rclcpp_action/include/rclcpp_action/exceptions.hpp @@ -0,0 +1,46 @@ +// 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 UnknownGoalHandleError : public std::invalid_argument +{ +public: + UnknownGoalHandleError() + : std::invalid_argument("Goal handle is not know to this client.") + { + } +}; + +class UnawareGoalHandleError : public std::runtime_error +{ +public: + UnawareGoalHandleError() + : std::runtime_error("Goal handle is not tracking the goal result.") + { + } +}; + +} // 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 0000000..3ab9f4c --- /dev/null +++ b/rclcpp_action/include/rclcpp_action/types.hpp @@ -0,0 +1,48 @@ +// 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 + +#include +#include + +#include + + +namespace rclcpp_action +{ + +using GoalID = std::array; +using GoalStatus = action_msgs::msg::GoalStatus; +using GoalInfo = action_msgs::msg::GoalInfo; + +} // namespace rclcpp_action + +namespace std +{ +template<> +struct less +{ + bool operator()( + const rclcpp_action::GoalID & lhs, + const rclcpp_action::GoalID & rhs) const + { + return lhs < rhs; + } +}; +} // namespace std +#endif // RCLCPP_ACTION__TYPES_HPP_ diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index a094e63..c8b3de3 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -12,30 +12,349 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include +#include +#include +#include +#include +#include +#include +#include #include -using rclcpp_action::ClientBase; +#include "rclcpp_action/client.hpp" +#include "rclcpp_action/exceptions.hpp" namespace rclcpp_action { + 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(node_logging->get_logger().get_child("rclcpp_acton")), + random_bytes_generator(std::random_device{} ()) + { + std::weak_ptr weak_node_handle(node_handle); + client_handle = std::shared_ptr( + new rcl_action_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_action"), + "Error in destruction of rcl action client handle: %s", rcl_get_error_string().str); + rcl_reset_error(); + } + } else { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp_action"), + "Error in destruction of rcl action client handle: " + "the Node Handle was destructed too early. You will leak memory"); + } + delete 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(), &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(), + &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 rcl action client details"); + } + } + + 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}; + + 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}; + + std::shared_ptr client_handle{nullptr}; + std::shared_ptr node_handle{nullptr}; + rclcpp::Logger logger; + + using ResponseCallback = std::function response)>; + + std::map pending_goal_responses; + std::mutex goal_requests_mutex; + + std::map pending_result_responses; + std::mutex result_requests_mutex; + + std::map pending_cancel_responses; + std::mutex cancel_requests_mutex; + + std::independent_bits_engine< + std::default_random_engine, 8, unsigned int> random_bytes_generator; }; -} ClientBase::ClientBase( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, - const std::string & name, - const rosidl_action_type_support_t * type_support) + 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, node_logging, action_name, type_support, client_options)) { - // TODO(sloretz) use rcl_action API when available - (void)node_base; - (void)name; - (void)type_support; } ClientBase::~ClientBase() { } + +rclcpp::Logger +ClientBase::get_logger() +{ + return pimpl_->logger; +} + +size_t +ClientBase::get_number_of_ready_subscriptions() +{ + return pimpl_->num_subscriptions; +} + +size_t +ClientBase::get_number_of_ready_guard_conditions() +{ + return pimpl_->num_guard_conditions; +} + +size_t +ClientBase::get_number_of_ready_timers() +{ + return pimpl_->num_timers; +} + +size_t +ClientBase::get_number_of_ready_clients() +{ + return pimpl_->num_clients; +} + +size_t +ClientBase::get_number_of_ready_services() +{ + return pimpl_->num_services; +} + +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; +} + +bool +ClientBase::is_ready(rcl_wait_set_t * wait_set) +{ + rcl_ret_t ret = rcl_action_client_wait_set_get_entities_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 any ready entities"); + } + 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::handle_goal_response( + const rmw_request_id_t & response_header, + std::shared_ptr response) +{ + 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..."); + 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 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); + 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 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..."); + 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 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); + 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 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..."); + 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 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); + 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)); + std::generate( + goal_id.begin(), goal_id.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( + pimpl_->client_handle.get(), feedback_message.get()); + pimpl_->is_feedback_ready = false; + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "error taking feedback"); + } else { + this->handle_feedback_message(feedback_message); + } + } 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()); + pimpl_->is_status_ready = false; + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "error taking status"); + } else { + this->handle_status_message(status_message); + } + } 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( + pimpl_->client_handle.get(), &response_header, goal_response.get()); + pimpl_->is_goal_response_ready = false; + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "error taking goal response"); + } else { + this->handle_goal_response(response_header, goal_response); + } + } 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( + pimpl_->client_handle.get(), &response_header, result_response.get()); + pimpl_->is_result_response_ready = false; + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "error taking result response"); + } else { + this->handle_result_response(response_header, result_response); + } + } 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( + pimpl_->client_handle.get(), &response_header, cancel_response.get()); + pimpl_->is_cancel_response_ready = false; + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "error taking cancel response"); + } else { + this->handle_cancel_response(response_header, cancel_response); + } + } else { + throw std::runtime_error("Executing action client but nothing is ready"); + } +} + +} // namespace rclcpp_action diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index 72cb792..22c3020 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -14,29 +14,407 @@ #include +#include +#include +#include + +#include +#include + +#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" + +using namespace std::chrono_literals; class TestClient : 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::GoalResultService; + 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() + { + 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); + ASSERT_EQ(RCL_RET_OK, ret); + goal_service = server_node->create_service( + goal_service_name, + [this]( + const ActionGoalRequest::SharedPtr request, + ActionGoalResponse::SharedPtr response) + { + response->stamp = clock.now(); + response->accepted = (request->order >= 0); + if (response->accepted) { + goals[request->uuid] = {request, response}; + } + }); + 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 = server_node->create_service( + result_service_name, + [this]( + const ActionGoalResultRequest::SharedPtr request, + 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.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); + client_executor.spin_once(); + ActionFeedbackMessage feedback_message; + feedback_message.uuid = goal_request->uuid; + feedback_message.sequence.push_back(0); + feedback_publisher->publish(feedback_message); + client_executor.spin_once(); + if (goal_request->order > 0) { + feedback_message.sequence.push_back(1); + feedback_publisher->publish(feedback_message); + client_executor.spin_once(); + for (int i = 1; i < goal_request->order; ++i) { + feedback_message.sequence.push_back( + feedback_message.sequence[i] + feedback_message.sequence[i - 1]); + feedback_publisher->publish(feedback_message); + client_executor.spin_once(); + } + } + goal_status.status = rclcpp_action::GoalStatus::STATUS_SUCCEEDED; + status_message.status_list[0] = goal_status; + status_publisher->publish(status_message); + client_executor.spin_once(); + response->sequence = feedback_message.sequence; + response->status = rclcpp_action::GoalStatus::STATUS_SUCCEEDED; + 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 = server_node->create_service( + cancel_service_name, + [this]( + const ActionCancelGoalRequest::SharedPtr request, + ActionCancelGoalResponse::SharedPtr response) + { + 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); + 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); + } else { + ++it; + } + } + status_publisher->publish(status_message); + client_executor.spin_once(); + }); + 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 = server_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 = 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); + + client_node = std::make_shared(client_node_name, namespace_name); + client_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() + { + status_publisher.reset(); + feedback_publisher.reset(); + cancel_service.reset(); + result_service.reset(); + goal_service.reset(); + server_node.reset(); + 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}; + + rclcpp::Node::SharedPtr server_node; + rclcpp::executors::SingleThreadedExecutor server_executor; + rclcpp::Node::SharedPtr client_node; + 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"}; + + 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) { - auto node = std::make_shared("my_node", "/rclcpp_action/test/client"); - - auto ac = rclcpp_action::create_client(node.get(), "fibonacci"); - (void)ac; + 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(client_node, action_name); + + ActionGoal bad_goal; + bad_goal.order = -5; + auto future_goal_handle = action_client->async_send_goal(bad_goal, nullptr, true); + 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); + 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_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(client_node, action_name); + + ActionGoal goal; + goal.order = 5; + auto future_goal_handle = action_client->async_send_goal(goal); + 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(); + 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]); + 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(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 std::shared_ptr feedback) mutable + { + (void)goal_handle; + (void)feedback; + feedback_count++; + }); + 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(); + dual_spin_until_future_complete(future_result); + auto result = future_result.get(); + + 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(client_node, action_name); + + ActionGoal goal; + goal.order = 5; + auto future_goal_handle = action_client->async_send_goal(goal, nullptr, true); + 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); + dual_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(client_node, action_name); + + ActionGoal goal; + goal.order = 6; + auto future_goal_handle0 = action_client->async_send_goal(goal, nullptr, true); + 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); + 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()) { + 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(); + dual_spin_until_future_complete(future_cancel_all); + auto cancel_response = future_cancel_all.get(); + + 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) +{ + 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); + 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); + 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()); + dual_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()); }