From 137a0fa7697a6e54c402b2b923dd8086cd80d334 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Tue, 25 Feb 2025 17:08:09 +0800 Subject: [PATCH 1/3] Implement action generic client Signed-off-by: Barry Xu --- rclcpp_action/CMakeLists.txt | 19 +- .../include/rclcpp_action/client.hpp | 282 +--- .../include/rclcpp_action/client_base.hpp | 319 +++++ .../rclcpp_action/create_generic_client.hpp | 80 ++ .../include/rclcpp_action/generic_client.hpp | 319 +++++ .../generic_client_goal_handle.hpp | 154 +++ .../src/{client.cpp => client_base.cpp} | 4 +- rclcpp_action/src/create_generic_client.cpp | 84 ++ rclcpp_action/src/generic_client.cpp | 475 +++++++ .../src/generic_client_goal_handle.cpp | 155 +++ rclcpp_action/test/test_generic_client.cpp | 1164 +++++++++++++++++ 11 files changed, 2770 insertions(+), 285 deletions(-) create mode 100644 rclcpp_action/include/rclcpp_action/client_base.hpp create mode 100644 rclcpp_action/include/rclcpp_action/create_generic_client.hpp create mode 100644 rclcpp_action/include/rclcpp_action/generic_client.hpp create mode 100644 rclcpp_action/include/rclcpp_action/generic_client_goal_handle.hpp rename rclcpp_action/src/{client.cpp => client_base.cpp} (99%) create mode 100644 rclcpp_action/src/create_generic_client.cpp create mode 100644 rclcpp_action/src/generic_client.cpp create mode 100644 rclcpp_action/src/generic_client_goal_handle.cpp create mode 100644 rclcpp_action/test/test_generic_client.cpp diff --git a/rclcpp_action/CMakeLists.txt b/rclcpp_action/CMakeLists.txt index 0fc1065d55..2741d099c7 100644 --- a/rclcpp_action/CMakeLists.txt +++ b/rclcpp_action/CMakeLists.txt @@ -23,7 +23,10 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() add_library(${PROJECT_NAME} - src/client.cpp + src/client_base.cpp + src/create_generic_client.cpp + src/generic_client.cpp + src/generic_client_goal_handle.cpp src/qos.cpp src/server.cpp src/server_goal_handle.cpp @@ -93,6 +96,20 @@ if(BUILD_TESTING) ) endif() + ament_add_gtest(test_generic_client test/test_generic_client.cpp TIMEOUT 180) + ament_add_test_label(test_generic_client mimick) + if(TARGET test_generic_client) + target_link_libraries(test_generic_client + ${PROJECT_NAME} + mimick + rcl::rcl + rcl_action::rcl_action + rclcpp::rclcpp + rcutils::rcutils + ${test_msgs_TARGETS} + ) + endif() + ament_add_gtest(test_server test/test_server.cpp TIMEOUT 180) ament_add_test_label(test_server mimick) if(TARGET test_server) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index e95f86f2a6..ff821a4db2 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -41,294 +41,14 @@ #include "rosidl_runtime_c/action_type_support_struct.h" #include "rosidl_typesupport_cpp/action_type_support.hpp" +#include "rclcpp_action/client_base.hpp" #include "rclcpp_action/client_goal_handle.hpp" #include "rclcpp_action/exceptions.hpp" #include "rclcpp_action/types.hpp" #include "rclcpp_action/visibility_control.hpp" - namespace rclcpp_action { -// Forward declaration -class ClientBaseImpl; - -/// Base Action Client implementation -/// \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: - RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ClientBase) - - RCLCPP_ACTION_PUBLIC - virtual ~ClientBase(); - - /// Enum to identify entities belonging to the action client - enum class EntityType : std::size_t - { - GoalClient, - ResultClient, - CancelClient, - FeedbackSubscription, - StatusSubscription, - }; - - /// Return true if there is an action server that is ready to take goal requests. - RCLCPP_ACTION_PUBLIC - bool - action_server_is_ready() const; - - /// Wait for action_server_is_ready() to become true, or until the given timeout is reached. - template - bool - wait_for_action_server( - std::chrono::duration timeout = std::chrono::duration(-1)) - { - return wait_for_action_server_nanoseconds( - std::chrono::duration_cast(timeout) - ); - } - - // ------------- - // 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 - void - add_to_wait_set(rcl_wait_set_t & wait_set) override; - - /// \internal - RCLCPP_ACTION_PUBLIC - bool - is_ready(const rcl_wait_set_t & wait_set) override; - - /// \internal - RCLCPP_ACTION_PUBLIC - std::shared_ptr - take_data() override; - - /// \internal - RCLCPP_ACTION_PUBLIC - std::shared_ptr - take_data_by_entity_id(size_t id) override; - - /// \internal - RCLCPP_ACTION_PUBLIC - void - execute(const std::shared_ptr & data) override; - - /// \internal - /// Set a callback to be called when action client entities have an event - /** - * The callback receives a size_t which is the number of messages received - * since the last time this callback was called. - * Normally this is 1, but can be > 1 if messages were received before any - * callback was set. - * - * The callback also receives an int identifier argument, which identifies - * the action client entity which is ready. - * This implies that the provided callback can use the identifier to behave - * differently depending on which entity triggered the waitable to become ready. - * - * Calling it again will clear any previously set callback. - * - * An exception will be thrown if the callback is not callable. - * - * This function is thread-safe. - * - * If you want more information available in the callback, like the subscription - * or other information, you may use a lambda with captures or std::bind. - * - * \param[in] callback functor to be called when a new message is received. - */ - RCLCPP_ACTION_PUBLIC - void - set_on_ready_callback(std::function callback) override; - - /// Unset the callback registered for new events, if any. - RCLCPP_ACTION_PUBLIC - void - clear_on_ready_callback() override; - - RCLCPP_ACTION_PUBLIC - std::vector> - get_timers() const override; - - // End Waitables API - // ----------------- - -protected: - RCLCPP_ACTION_PUBLIC - ClientBase( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, - rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, - const std::string & action_name, - const rosidl_action_type_support_t * type_support, - const rcl_action_client_options_t & options); - - /// Wait for action_server_is_ready() to become true, or until the given timeout is reached. - RCLCPP_ACTION_PUBLIC - bool - wait_for_action_server_nanoseconds(std::chrono::nanoseconds timeout); - - // ----------------------------------------------------- - // 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 - GoalUUID - 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<> - // --------------------------------------------------------- - - /// \internal - /// Set a callback to be called when the specified entity is ready - RCLCPP_ACTION_PUBLIC - void - set_on_ready_callback( - EntityType entity_type, - rcl_event_callback_t callback, - const void * user_data); - - // Mutex to protect the callbacks storage. - std::recursive_mutex listener_mutex_; - // Storage for std::function callbacks to keep them in scope - std::unordered_map> entity_type_to_on_ready_callback_; - -private: - std::unique_ptr pimpl_; - - /// Set a std::function callback to be called when the specified entity is ready - RCLCPP_ACTION_PUBLIC - void - set_callback_to_entity( - EntityType entity_type, - std::function callback); - - bool on_ready_callback_set_{false}; -}; - /// Action Client /** * This class creates an action client. diff --git a/rclcpp_action/include/rclcpp_action/client_base.hpp b/rclcpp_action/include/rclcpp_action/client_base.hpp new file mode 100644 index 0000000000..9f8783d88d --- /dev/null +++ b/rclcpp_action/include/rclcpp_action/client_base.hpp @@ -0,0 +1,319 @@ +// 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__CLIENT_BASE_HPP_ +#define RCLCPP_ACTION__CLIENT_BASE_HPP_ + +#include +#include +#include +#include +#include + +#include "rcl_action/action_client.h" + +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" +#include "rclcpp/node_interfaces/node_graph_interface.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/waitable.hpp" + +#include "rclcpp_action/visibility_control.hpp" +#include "rclcpp_action/types.hpp" + +namespace rclcpp_action +{ +// Forward declaration +class ClientBaseImpl; + +/// Base Action Client implementation +/// \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: + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ClientBase) + + RCLCPP_ACTION_PUBLIC + virtual ~ClientBase(); + + /// Enum to identify entities belonging to the action client + enum class EntityType : std::size_t + { + GoalClient, + ResultClient, + CancelClient, + FeedbackSubscription, + StatusSubscription, + }; + + /// Return true if there is an action server that is ready to take goal requests. + RCLCPP_ACTION_PUBLIC + bool + action_server_is_ready() const; + + /// Wait for action_server_is_ready() to become true, or until the given timeout is reached. + template + bool + wait_for_action_server( + std::chrono::duration timeout = std::chrono::duration(-1)) + { + return wait_for_action_server_nanoseconds( + std::chrono::duration_cast(timeout) + ); + } + + // ------------- + // 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 + void + add_to_wait_set(rcl_wait_set_t & wait_set) override; + + /// \internal + RCLCPP_ACTION_PUBLIC + bool + is_ready(const rcl_wait_set_t & wait_set) override; + + /// \internal + RCLCPP_ACTION_PUBLIC + std::shared_ptr + take_data() override; + + /// \internal + RCLCPP_ACTION_PUBLIC + std::shared_ptr + take_data_by_entity_id(size_t id) override; + + /// \internal + RCLCPP_ACTION_PUBLIC + void + execute(const std::shared_ptr & data) override; + + /// \internal + /// Set a callback to be called when action client entities have an event + /** + * The callback receives a size_t which is the number of messages received + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if messages were received before any + * callback was set. + * + * The callback also receives an int identifier argument, which identifies + * the action client entity which is ready. + * This implies that the provided callback can use the identifier to behave + * differently depending on which entity triggered the waitable to become ready. + * + * Calling it again will clear any previously set callback. + * + * An exception will be thrown if the callback is not callable. + * + * This function is thread-safe. + * + * If you want more information available in the callback, like the subscription + * or other information, you may use a lambda with captures or std::bind. + * + * \param[in] callback functor to be called when a new message is received. + */ + RCLCPP_ACTION_PUBLIC + void + set_on_ready_callback(std::function callback) override; + + /// Unset the callback registered for new events, if any. + RCLCPP_ACTION_PUBLIC + void + clear_on_ready_callback() override; + + RCLCPP_ACTION_PUBLIC + std::vector> + get_timers() const override; + + // End Waitables API + // ----------------- + +protected: + RCLCPP_ACTION_PUBLIC + ClientBase( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + const std::string & action_name, + const rosidl_action_type_support_t * type_support, + const rcl_action_client_options_t & options); + + /// Wait for action_server_is_ready() to become true, or until the given timeout is reached. + RCLCPP_ACTION_PUBLIC + bool + wait_for_action_server_nanoseconds(std::chrono::nanoseconds timeout); + + // ----------------------------------------------------- + // 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 + GoalUUID + 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<> + // --------------------------------------------------------- + + /// \internal + /// Set a callback to be called when the specified entity is ready + RCLCPP_ACTION_PUBLIC + void + set_on_ready_callback( + EntityType entity_type, + rcl_event_callback_t callback, + const void * user_data); + + // Mutex to protect the callbacks storage. + std::recursive_mutex listener_mutex_; + // Storage for std::function callbacks to keep them in scope + std::unordered_map> entity_type_to_on_ready_callback_; + +private: + std::unique_ptr pimpl_; + + /// Set a std::function callback to be called when the specified entity is ready + RCLCPP_ACTION_PUBLIC + void + set_callback_to_entity( + EntityType entity_type, + std::function callback); + + bool on_ready_callback_set_{false}; +}; +} // namespace rclcpp_action + +#endif // RCLCPP_ACTION__CLIENT_BASE_HPP_ diff --git a/rclcpp_action/include/rclcpp_action/create_generic_client.hpp b/rclcpp_action/include/rclcpp_action/create_generic_client.hpp new file mode 100644 index 0000000000..b256ab13bd --- /dev/null +++ b/rclcpp_action/include/rclcpp_action/create_generic_client.hpp @@ -0,0 +1,80 @@ +// Copyright 2025 Sony Group Corporation. +// +// 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__CREATE_GENERIC_CLIENT_HPP_ +#define RCLCPP_ACTION__CREATE_GENERIC_CLIENT_HPP_ + +#include + +#include "rclcpp_action/generic_client.hpp" + +namespace rclcpp_action +{ +/// Create an action generic client. +/** + * This function is equivalent to \sa create_generic_client()` however is using the individual + * node interfaces to create the client. + * + * \param[in] node_base_interface The node base interface of the corresponding node. + * \param[in] node_graph_interface The node graph interface of the corresponding node. + * \param[in] node_logging_interface The node logging interface of the corresponding node. + * \param[in] node_waitables_interface The node waitables interface of the corresponding node. + * \param[in] name The action name. + * \param[in] type The action type. + * \param[in] group The action client will be added to this callback group. + * If `nullptr`, then the action client is added to the default callback group. + * \param[in] options Options to pass to the underlying `rcl_action_client_t`. + */ +typename GenericClient::SharedPtr +create_generic_client( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface, + rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_interface, + const std::string & name, + const std::string & type, + rclcpp::CallbackGroup::SharedPtr group = nullptr, + const rcl_action_client_options_t & options = rcl_action_client_get_default_options()); + +/// Create an action generic client. +/** + * \param[in] node The action client will be added to this node. + * \param[in] name The action name. + * \param[in] type The action type. + * \param[in] group The action client will be added to this callback group. + * If `nullptr`, then the action client is added to the default callback group. + * \param[in] options Options to pass to the underlying `rcl_action_client_t`. + */ +template +typename GenericClient::SharedPtr +create_generic_client( + NodeT node, + const std::string & name, + const std::string & type, + rclcpp::CallbackGroup::SharedPtr group = nullptr, + const rcl_action_client_options_t & options = rcl_action_client_get_default_options()) +{ + return rclcpp_action::create_generic_client( + node->get_node_base_interface(), + node->get_node_graph_interface(), + node->get_node_logging_interface(), + node->get_node_waitables_interface(), + name, + type, + group, + options); +} +} // namespace rclcpp_action + +#endif // RCLCPP_ACTION__CREATE_GENERIC_CLIENT_HPP_ diff --git a/rclcpp_action/include/rclcpp_action/generic_client.hpp b/rclcpp_action/include/rclcpp_action/generic_client.hpp new file mode 100644 index 0000000000..0e701099a4 --- /dev/null +++ b/rclcpp_action/include/rclcpp_action/generic_client.hpp @@ -0,0 +1,319 @@ +// Copyright 2025 Sony Group Corporation. +// +// 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__GENERIC_CLIENT_HPP_ +#define RCLCPP_ACTION__GENERIC_CLIENT_HPP_ + +#include +#include +#include + +#include "action_msgs/srv/cancel_goal.hpp" +#include "action_msgs/msg/goal_info.hpp" +#include "action_msgs/msg/goal_status_array.hpp" + +#include "rclcpp_action/client_base.hpp" +#include "rclcpp_action/generic_client_goal_handle.hpp" +#include "rcpputils/shared_library.hpp" +#include "rosidl_typesupport_introspection_cpp/identifier.hpp" +#include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" + +#include "unique_identifier_msgs/msg/uuid.hpp" + +namespace rclcpp_action +{ +/// Action Generic Client +/** + * This class creates an action generic client. + * + * To create an instance of an action client use `rclcpp_action::create_generic_client()`. + */ +class GenericClient : public ClientBase +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(GenericClient) + + using Goal = void *; // Deserialized data pointer of goal + using GoalRequest = void *; // Deserialized data pointer of goal request (uuid + Goal) + using CancelRequest = typename action_msgs::srv::CancelGoal_Request; + using CancelResponse = typename action_msgs::srv::CancelGoal_Response; + using WrappedResult = typename GenericClientGoalHandle::WrappedResult; + using GoalResponseCallback = std::function; + using FeedbackCallback = typename GenericClientGoalHandle::FeedbackCallback; + using ResultCallback = typename GenericClientGoalHandle::ResultCallback; + using CancelCallback = std::function; + + using IntrospectionMessageMembersPtr = + const rosidl_typesupport_introspection_cpp::MessageMembers *; + + /// Options for sending a goal. + /** + * This struct is used to pass parameters to the function `async_send_goal`. + */ + struct SendGoalOptions + { + SendGoalOptions() + : goal_response_callback(nullptr), + feedback_callback(nullptr), + result_callback(nullptr) + { + } + + /// Function called when the goal is accepted or rejected. + /** + * Takes a single argument that is a goal handle shared pointer. + * If the goal is accepted, then the pointer points to a valid goal handle. + * If the goal is rejected, then pointer has the value `nullptr`. + */ + GoalResponseCallback goal_response_callback; + + /// Function called whenever feedback is received for the goal. + FeedbackCallback feedback_callback; + + /// Function called when the result for the goal is received. + ResultCallback result_callback; + }; + + /// Construct an action generic client. + /** + * This constructs an action generic client, but it will not work until it has been added to a + * node. + * Use `rclcpp_action::create_generic_client()` to both construct and add to a node. + * + * \param[in] node_base A pointer to the base interface of a node. + * \param[in] node_graph A pointer to an interface that allows getting graph information about + * a node. + * \param[in] node_logging A pointer to an interface that allows getting a node's logger. + * \param[in] action_name The action name. + * \param[in] typesupport_lib A pointer to type support library for the action type + * \param[in] action_typesupport_handle the action type support handle + * \param[in] client_options Options to pass to the underlying `rcl_action::rcl_action_client_t`. + */ + GenericClient( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + const std::string & action_name, + std::shared_ptr typesupport_lib, + const rosidl_action_type_support_t * action_typesupport_handle, + const rcl_action_client_options_t & client_options = rcl_action_client_get_default_options()); + + /// Send an action goal and asynchronously get the result. + /** + * If the goal is accepted by an action server, the returned future is set to a `GenericClientGoalHandle::SharedPtr`. + * If the goal is rejected by an action server, then the future is set to a `nullptr`. + * + * The goal handle in the future is used to monitor the status of the goal and get the final result. + * + * If callbacks were set in @param options, you will receive callbacks, as long as you hold a reference + * to the shared pointer contained in the returned future, or rclcpp_action::GenericClient is destroyed. Dropping + * the shared pointer to the goal handle will not cancel the goal. In order to cancel it, you must explicitly + * call async_cancel_goal. + * + * WARNING this method has inconsistent behaviour and a memory leak bug. + * If you set the result callback in @param options, the handle will be self referencing, and you will receive + * callbacks even though you do not hold a reference to the shared pointer. In this case, the self reference will + * be deleted if the result callback was received. If there is no result callback, there will be a memory leak. + * + * To prevent the memory leak, you may call stop_callbacks() explicit. This will delete the self reference. + * + * \param[in] goal The goal. + * \param[in] goal_size The size of goal. + * \param[in] options Options for sending the goal request. Contains references to callbacks for + * the goal response (accepted/rejected), feedback, and the final result. + * \return A future that completes when the goal has been accepted or rejected. + * If the goal is rejected, then the result will be a `nullptr`. + */ + std::shared_future + async_send_goal( + const Goal goal, + size_t goal_size, + const SendGoalOptions & options = SendGoalOptions()); + + /// Send an action goal request and asynchronously get the result. + /** + * \param[in] goal The goal request (uuid+goal). + * \param[in] options Options for sending the goal request. Contains references to callbacks for + * the goal response (accepted/rejected), feedback, and the final result. + * \return A future that completes when the goal has been accepted or rejected. + * If the goal is rejected, then the result will be a `nullptr`. + */ + std::shared_future + async_send_goal( + const GoalRequest goal_request, + const SendGoalOptions & options = SendGoalOptions()); + + /// Asynchronously get the result for an active goal. + /** + * \throws exceptions::UnknownGoalHandleError If the goal unknown or already reached a terminal + * state, or if there was an error requesting the result. + * \param[in] goal_handle The goal handle for which to get the result. + * \param[in] result_callback Optional callback that is called when the result is received. + * \return A future that is set to the goal result when the goal is finished. + */ + std::shared_future + async_get_result( + typename GenericClientGoalHandle::SharedPtr goal_handle, + ResultCallback result_callback = nullptr); + + /// Asynchronously request a goal be canceled. + /** + * \throws exceptions::UnknownGoalHandleError If the goal is unknown or already reached a + * terminal state. + * \param[in] goal_handle The goal handle requesting to be canceled. + * \param[in] cancel_callback Optional callback that is called when the response is received. + * The callback takes one parameter: a shared pointer to the CancelResponse message. + * \return A future to a CancelResponse message that is set when the request has been + * acknowledged by an action server. + * See + * + * action_msgs/CancelGoal.srv. + */ + std::shared_future + async_cancel_goal( + typename GenericClientGoalHandle::SharedPtr goal_handle, + CancelCallback cancel_callback = nullptr); + + /// Asynchronously request for all goals to be canceled. + /** + * \param[in] cancel_callback Optional callback that is called when the response is received. + * The callback takes one parameter: a shared pointer to the CancelResponse message. + * \return A future to a CancelResponse message that is set when the request has been + * acknowledged by an action server. + * See + * + * action_msgs/CancelGoal.srv. + */ + std::shared_future + async_cancel_all_goals(CancelCallback cancel_callback = nullptr); + + /// Stops the callbacks for the goal in a thread safe way + /** + * This will NOT cancel the goal, it will only stop the callbacks. + * + * After the call to this function, it is guaranteed that there + * will be no more callbacks from the goal. This is not guaranteed + * if multiple threads are involved, and the goal_handle is just + * dropped. + * + * \param[in] goal_handle The goal were the callbacks shall be stopped + */ + void + stop_callbacks(typename GenericClientGoalHandle::SharedPtr goal_handle); + + /// Stops the callbacks for the goal in a thread safe way + /** + * For further information see stop_callbacks(typename GenericGoalHandle::SharedPtr goal_handle) + */ + void + stop_callbacks(const GoalUUID & goal_id); + + /// Asynchronously request all goals at or before a specified time be canceled. + /** + * \param[in] stamp The timestamp for the cancel goal request. + * \param[in] cancel_callback Optional callback that is called when the response is received. + * The callback takes one parameter: a shared pointer to the CancelResponse message. + * \return A future to a CancelResponse message that is set when the request has been + * acknowledged by an action server. + * See + * + * action_msgs/CancelGoal.srv. + */ + std::shared_future + async_cancel_goals_before( + const rclcpp::Time & stamp, + CancelCallback cancel_callback = nullptr); + + virtual + ~GenericClient(); + +private: + /// \internal + std::shared_ptr + create_message(IntrospectionMessageMembersPtr message_members) const; + + /// \internal + std::shared_ptr + create_goal_response() const override + { + return create_message(goal_service_response_type_members_); + } + + /// \internal + std::shared_ptr + create_result_request() const + { + return create_message(result_service_request_type_members_); + } + + /// \internal + std::shared_ptr + create_result_response() const override + { + return create_message(result_service_response_type_members_); + } + + /// \internal + std::shared_ptr + create_cancel_response() const override + { + return create_message(cancel_service_response_type_members_); + } + + /// \internal + std::shared_ptr + create_feedback_message() const override + { + return create_message(feedback_type_members_); + } + + /// \internal + void + handle_feedback_message(std::shared_ptr message) override; + + /// \internal + std::shared_ptr + create_status_message() const override + { + using GoalStatusMessage = action_msgs::msg::GoalStatusArray; + return std::shared_ptr(new GoalStatusMessage()); + } + + /// \internal + void + handle_status_message(std::shared_ptr message) override; + + /// \internal + void + make_result_aware(typename GenericClientGoalHandle::SharedPtr goal_handle); + + /// \internal + std::shared_future + async_cancel( + typename CancelRequest::SharedPtr cancel_request, + CancelCallback cancel_callback = nullptr); + + std::shared_ptr ts_lib_; + IntrospectionMessageMembersPtr goal_service_request_type_members_; + IntrospectionMessageMembersPtr goal_service_response_type_members_; + IntrospectionMessageMembersPtr result_service_request_type_members_; + IntrospectionMessageMembersPtr result_service_response_type_members_; + IntrospectionMessageMembersPtr cancel_service_response_type_members_; + IntrospectionMessageMembersPtr feedback_type_members_; + + std::map goal_handles_; + std::recursive_mutex goal_handles_mutex_; +}; +} // namespace rclcpp_action +#endif // RCLCPP_ACTION__GENERIC_CLIENT_HPP_ diff --git a/rclcpp_action/include/rclcpp_action/generic_client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/generic_client_goal_handle.hpp new file mode 100644 index 0000000000..cbe8b884cf --- /dev/null +++ b/rclcpp_action/include/rclcpp_action/generic_client_goal_handle.hpp @@ -0,0 +1,154 @@ +// Copyright 2025 Sony Group Corporation. +// +// 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__GENERIC_CLIENT_GOAL_HANDLE_HPP_ +#define RCLCPP_ACTION__GENERIC_CLIENT_GOAL_HANDLE_HPP_ + +#include +#include +#include + +#include "action_msgs/msg/goal_status.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/time.hpp" + +#include "rclcpp_action/exceptions.hpp" +#include "rclcpp_action/types.hpp" +#include "rclcpp_action/visibility_control.hpp" + +namespace rclcpp_action +{ +// Forward declarations +class GenericClient; + +class GenericClientGoalHandle +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(GenericClientGoalHandle) + + /// 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 + }; + + // A wrapper that defines the result of an action + struct WrappedResult + { + /// The unique identifier of the goal + GoalUUID goal_id; + /// A status to indicate if the goal was canceled, aborted, or succeeded + ResultCode code; + /// User defined fields sent back with an action + const void * result; + /// hold shared pointer for result response message. + std::shared_ptr result_response; + }; + + using FeedbackCallback = + std::function; + using ResultCallback = std::function; + + virtual ~GenericClientGoalHandle(); + + /// Get the unique ID for the goal. + const GoalUUID & + get_goal_id() const; + + /// Get the time when the goal was accepted. + rclcpp::Time + get_goal_stamp() const; + + /// Get the goal status code. + int8_t + get_status(); + + /// Check if an action client has subscribed to feedback for the goal. + bool + is_feedback_aware(); + + /// Check if an action client has requested the result for the goal. + bool + is_result_aware(); + +private: + // The templated Client creates goal handles + friend class GenericClient; + + GenericClientGoalHandle( + const GoalInfo & info, + FeedbackCallback feedback_callback, + ResultCallback result_callback); + + void + set_feedback_callback(FeedbackCallback callback); + + void + set_result_callback(ResultCallback callback); + + void + call_feedback_callback( + GenericClientGoalHandle::SharedPtr shared_this, + const void * feedback_message); + + /// Get a future to the goal result. + /** + * This method should not be called if the `ignore_result` flag was set when + * sending the original goal request (see Client::async_send_goal). + * + * `is_result_aware()` can be used to check if it is safe to call this method. + * + * \throws exceptions::UnawareGoalHandleError If the the goal handle is unaware of the result. + * \return A future to the result. + */ + std::shared_future + async_get_result(); + + /// Returns the previous value of awareness + bool + set_result_awareness(bool awareness); + + void + set_status(int8_t status); + + void + set_result(const WrappedResult & wrapped_result); + + void + invalidate(const exceptions::UnawareGoalHandleError & ex); + + bool + is_invalidated() const; + + GoalInfo info_; + + std::exception_ptr invalidate_exception_{nullptr}; + + bool is_result_aware_{false}; + std::promise result_promise_; + std::shared_future result_future_; + + FeedbackCallback feedback_callback_{nullptr}; + ResultCallback result_callback_{nullptr}; + int8_t status_{GoalStatus::STATUS_ACCEPTED}; + + std::recursive_mutex handle_mutex_; +}; +} // namespace rclcpp_action +#endif // RCLCPP_ACTION__GENERIC_CLIENT_GOAL_HANDLE_HPP_ diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client_base.cpp similarity index 99% rename from rclcpp_action/src/client.cpp rename to rclcpp_action/src/client_base.cpp index 1516ee9865..77cd5a39ef 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client_base.cpp @@ -17,7 +17,6 @@ #include #include #include -#include #include #include @@ -26,8 +25,7 @@ #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/node_interfaces/node_logging_interface.hpp" -#include "rclcpp_action/client.hpp" -#include "rclcpp_action/exceptions.hpp" +#include "rclcpp_action/client_base.hpp" namespace rclcpp_action { diff --git a/rclcpp_action/src/create_generic_client.cpp b/rclcpp_action/src/create_generic_client.cpp new file mode 100644 index 0000000000..d83d879864 --- /dev/null +++ b/rclcpp_action/src/create_generic_client.cpp @@ -0,0 +1,84 @@ +// Copyright 2025 Sony Group Corporation. +// +// 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. + +#include + +#include "rclcpp/node.hpp" +#include "rclcpp/typesupport_helpers.hpp" + +#include "rclcpp_action/create_generic_client.hpp" +#include "rclcpp_action/generic_client.hpp" + +namespace rclcpp_action +{ +typename GenericClient::SharedPtr +create_generic_client( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface, + rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_interface, + const std::string & name, + const std::string & type, + rclcpp::CallbackGroup::SharedPtr group, + const rcl_action_client_options_t & options) +{ + std::weak_ptr weak_node = + 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](GenericClient * ptr) + { + if (nullptr == ptr) { + return; + } + auto shared_node = weak_node.lock(); + if (shared_node) { + // API expects a shared pointer, give it one with a deleter that does nothing. + std::shared_ptr fake_shared_ptr(ptr, [](GenericClient *) {}); + + if (group_is_null) { + // Was added to default group + shared_node->remove_waitable(fake_shared_ptr, nullptr); + } else { + // Was added to a specific group + auto shared_group = weak_group.lock(); + if (shared_group) { + shared_node->remove_waitable(fake_shared_ptr, shared_group); + } + } + } + delete ptr; + }; + + auto typesupport_lib = rclcpp::get_typesupport_library(type, "rosidl_typesupport_cpp"); + auto action_typesupport_handle = rclcpp::get_action_typesupport_handle( + type, "rosidl_typesupport_cpp", *typesupport_lib); + + std::shared_ptr action_client( + new GenericClient( + node_base_interface, + node_graph_interface, + node_logging_interface, + name, + typesupport_lib, + action_typesupport_handle, + options), + deleter); + + node_waitables_interface->add_waitable(action_client, group); + return action_client; +} + +} // namespace rclcpp_action diff --git a/rclcpp_action/src/generic_client.cpp b/rclcpp_action/src/generic_client.cpp new file mode 100644 index 0000000000..fbeb3bb7bd --- /dev/null +++ b/rclcpp_action/src/generic_client.cpp @@ -0,0 +1,475 @@ +// Copyright 2025 Sony Group Corporation. +// +// 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. + +#include + +#include "rclcpp_action/generic_client.hpp" + +namespace rclcpp_action +{ +GenericClient::GenericClient( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + const std::string & action_name, + std::shared_ptr typesupport_lib, + const rosidl_action_type_support_t * action_typesupport_handle, + const rcl_action_client_options_t & client_options) +: ClientBase( + node_base, node_graph, node_logging, action_name, + action_typesupport_handle, + client_options), + ts_lib_(std::move(typesupport_lib)) +{ + auto goal_service_request_type_support_intro = get_message_typesupport_handle( + action_typesupport_handle->goal_service_type_support->request_typesupport, + rosidl_typesupport_introspection_cpp::typesupport_identifier); + goal_service_request_type_members_ = + static_cast(goal_service_request_type_support_intro->data); + + auto goal_service_response_type_support_intro = get_message_typesupport_handle( + action_typesupport_handle->goal_service_type_support->response_typesupport, + rosidl_typesupport_introspection_cpp::typesupport_identifier); + goal_service_response_type_members_ = + static_cast(goal_service_response_type_support_intro->data); + + auto result_service_request_type_support_intro = get_message_typesupport_handle( + action_typesupport_handle->result_service_type_support->request_typesupport, + rosidl_typesupport_introspection_cpp::typesupport_identifier); + result_service_request_type_members_ = + static_cast(result_service_request_type_support_intro->data); + + auto result_service_response_type_support_intro = get_message_typesupport_handle( + action_typesupport_handle->result_service_type_support->response_typesupport, + rosidl_typesupport_introspection_cpp::typesupport_identifier); + result_service_response_type_members_ = + static_cast(result_service_response_type_support_intro->data); + + auto cancel_service_reponse_type_support_intro = get_message_typesupport_handle( + action_typesupport_handle->cancel_service_type_support->response_typesupport, + rosidl_typesupport_introspection_cpp::typesupport_identifier); + cancel_service_response_type_members_ = + static_cast(cancel_service_reponse_type_support_intro->data); + + auto feedback_type_support_intro = get_message_typesupport_handle( + action_typesupport_handle->feedback_message_type_support, + rosidl_typesupport_introspection_cpp::typesupport_identifier); + feedback_type_members_ = + static_cast(feedback_type_support_intro->data); +} + +std::shared_future +GenericClient::async_send_goal( + const Goal goal, size_t goal_size, const SendGoalOptions & options) +{ + // uuid + goal + size_t goal_request_size = sizeof(rclcpp_action::GoalUUID) + goal_size; + std::shared_ptr goal_request_msg( + new uint8_t[goal_request_size], std::default_delete()); + + rclcpp_action::GoalUUID * uuid = + reinterpret_cast(goal_request_msg.get()); + *uuid = this->generate_goal_id(); + + std::memcpy( + goal_request_msg.get() + sizeof(rclcpp_action::GoalUUID), + goal, goal_size); + + return async_send_goal(static_cast(goal_request_msg.get()), options); +} + +std::shared_future +GenericClient::async_send_goal( + const GoalRequest goal_request, + const SendGoalOptions & options) +{ + // Put promise in the heap to move it around. + auto promise = std::make_shared>(); + std::shared_future future(promise->get_future()); + + auto do_nothing = [](void *ptr) { + (void)ptr; + }; + std::shared_ptr goal_request_msg(const_cast(goal_request), do_nothing); + + GoalUUID uuid = *reinterpret_cast(goal_request); + + this->send_goal_request( + goal_request_msg, + [this, uuid, options, promise](std::shared_ptr response) mutable + { + size_t response_accepted_offset = 0; + size_t response_timestamp_offset = 0; + for(uint32_t i = 0; i < goal_service_response_type_members_->member_count_; i++) { + if (!std::strcmp(goal_service_response_type_members_->members_[i].name_, "accepted")) { + response_accepted_offset = goal_service_response_type_members_->members_[i].offset_; + continue; + } + if (!std::strcmp(goal_service_response_type_members_->members_[i].name_, "stamp")) { + response_timestamp_offset = goal_service_response_type_members_->members_[i].offset_; + continue; + } + } + + bool response_accepted = false; + std::memcpy( + static_cast(&response_accepted), + static_cast(static_cast(response.get()) + response_accepted_offset), + sizeof(bool)); + + if (!response_accepted) { + promise->set_value(nullptr); + if (options.goal_response_callback) { + options.goal_response_callback(nullptr); + } + return; + } + + action_msgs::msg::GoalInfo goal_info; + goal_info.goal_id.uuid = uuid; + std::memcpy( + static_cast(&goal_info.stamp), + static_cast(static_cast(response.get()) + response_timestamp_offset), + sizeof(goal_info.stamp)); + + // Do not use std::make_shared as friendship cannot be forwarded. + std::shared_ptr goal_handle( + new GenericClientGoalHandle( + goal_info, options.feedback_callback, options.result_callback)); + { + std::lock_guard guard(goal_handles_mutex_); + goal_handles_[goal_handle->get_goal_id()] = goal_handle; + } + promise->set_value(goal_handle); + if (options.goal_response_callback) { + options.goal_response_callback(goal_handle); + } + + if (options.result_callback) { + this->make_result_aware(goal_handle); + } + }); + + // TODO(jacobperron): Encapsulate into it's own function and + // consider exposing an option to disable this cleanup + // To prevent the list from growing out of control, forget about any goals + // with no more user references + { + std::lock_guard guard(goal_handles_mutex_); + auto goal_handle_it = goal_handles_.begin(); + while (goal_handle_it != goal_handles_.end()) { + if (!goal_handle_it->second.lock()) { + RCLCPP_DEBUG( + this->get_logger(), + "Dropping weak reference to goal handle during send_goal()"); + goal_handle_it = goal_handles_.erase(goal_handle_it); + } else { + ++goal_handle_it; + } + } + } + + return future; +} + +std::shared_future +GenericClient::async_get_result( + typename GenericClientGoalHandle::SharedPtr goal_handle, + ResultCallback result_callback) +{ + std::lock_guard lock(goal_handles_mutex_); + if (goal_handles_.count(goal_handle->get_goal_id()) == 0) { + throw exceptions::UnknownGoalHandleError(); + } + if (goal_handle->is_invalidated()) { + // This case can happen if there was a failure to send the result request + // during the goal response callback + throw goal_handle->invalidate_exception_; + } + if (result_callback) { + // This will override any previously registered callback + goal_handle->set_result_callback(result_callback); + } + this->make_result_aware(goal_handle); + return goal_handle->async_get_result(); +} + +std::shared_future +GenericClient::async_cancel_goal( + typename GenericClientGoalHandle::SharedPtr goal_handle, + CancelCallback cancel_callback) +{ + std::lock_guard lock(goal_handles_mutex_); + if (goal_handles_.count(goal_handle->get_goal_id()) == 0) { + throw exceptions::UnknownGoalHandleError(); + } + auto cancel_request = std::make_shared(); + cancel_request->goal_info.goal_id.uuid = goal_handle->get_goal_id(); + return async_cancel(cancel_request, cancel_callback); +} + +std::shared_future +GenericClient::async_cancel_all_goals(CancelCallback cancel_callback) +{ + auto cancel_request = std::make_shared(); + std::fill( + cancel_request->goal_info.goal_id.uuid.begin(), + cancel_request->goal_info.goal_id.uuid.end(), 0u); + return async_cancel(cancel_request, cancel_callback); +} + +void +GenericClient::stop_callbacks(typename GenericClientGoalHandle::SharedPtr goal_handle) +{ + goal_handle->set_feedback_callback(typename GenericClientGoalHandle::FeedbackCallback()); + goal_handle->set_result_callback(typename GenericClientGoalHandle::ResultCallback()); + + std::lock_guard guard(goal_handles_mutex_); + const GoalUUID & goal_id = goal_handle->get_goal_id(); + auto it = goal_handles_.find(goal_id); + if (goal_handles_.end() == it) { + // someone else already deleted the entry + // e.g. the result callback + RCLCPP_DEBUG( + this->get_logger(), + "Given goal is unknown. Ignoring..."); + return; + } + goal_handles_.erase(it); +} + +void GenericClient::stop_callbacks(const GoalUUID & goal_id) +{ + typename GenericClientGoalHandle::SharedPtr goal_handle; + { + std::lock_guard guard(goal_handles_mutex_); + auto it = goal_handles_.find(goal_id); + if (goal_handles_.end() == it) { + // someone else already deleted the entry + // e.g. the result callback + RCLCPP_DEBUG( + this->get_logger(), + "Given goal is unknown. Ignoring..."); + return; + } + + goal_handle = it->second.lock(); + } + + if (goal_handle) { + stop_callbacks(goal_handle); + } +} + +std::shared_future +GenericClient::async_cancel_goals_before( + const rclcpp::Time & stamp, + CancelCallback cancel_callback) +{ + 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, cancel_callback); +} + +GenericClient::~GenericClient() +{ + std::lock_guard guard(goal_handles_mutex_); + auto it = goal_handles_.begin(); + while (it != goal_handles_.end()) { + typename GenericClientGoalHandle::SharedPtr goal_handle = it->second.lock(); + if (goal_handle) { + goal_handle->invalidate(exceptions::UnawareGoalHandleError()); + } + it = goal_handles_.erase(it); + } +} + +std::shared_ptr +GenericClient::create_message(IntrospectionMessageMembersPtr message_members) const +{ + void * message = new uint8_t[message_members->size_of_]; + message_members->init_function(message, rosidl_runtime_cpp::MessageInitialization::ZERO); + return std::shared_ptr( + message, + [message_members](void * p) + { + message_members->fini_function(p); + delete[] reinterpret_cast(p); + }); +} + +void +GenericClient::handle_feedback_message(std::shared_ptr message) +{ + size_t goal_id_offset = 0; + size_t feedback_offset = 0; + for (uint32_t i = 0; i < feedback_type_members_->member_count_; i++) { + if (!strcmp(feedback_type_members_->members_[i].name_, "goal_id")) { + goal_id_offset = feedback_type_members_->members_[i].offset_; + continue; + } + if (!strcmp(feedback_type_members_->members_[i].name_, "feedback")) { + feedback_offset = feedback_type_members_->members_[i].offset_; + continue; + } + } + + auto * uuid = reinterpret_cast( + static_cast(message.get()) + goal_id_offset); + + std::lock_guard guard(goal_handles_mutex_); + const GoalUUID & goal_id = uuid->uuid; + if (goal_handles_.count(goal_id) == 0) { + RCLCPP_DEBUG( + this->get_logger(), + "Received feedback for unknown goal. Ignoring..."); + return; + } + typename GenericClientGoalHandle::SharedPtr goal_handle = goal_handles_[goal_id].lock(); + // Forget about the goal if there are no more user references + if (!goal_handle) { + RCLCPP_DEBUG( + this->get_logger(), + "Dropping weak reference to goal handle during feedback callback"); + goal_handles_.erase(goal_id); + return; + } + + void * feedback = static_cast(reinterpret_cast(message.get()) + feedback_offset); + goal_handle->call_feedback_callback(goal_handle, feedback); +} + +void +GenericClient::handle_status_message(std::shared_ptr message) +{ + std::lock_guard guard(goal_handles_mutex_); + using GoalStatusMessage = action_msgs::msg::GoalStatusArray; + auto status_message = std::static_pointer_cast(message); + for (const GoalStatus & status : status_message->status_list) { + const GoalUUID & 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 GenericClientGoalHandle::SharedPtr goal_handle = goal_handles_[goal_id].lock(); + // Forget about the goal if there are no more user references + if (!goal_handle) { + RCLCPP_DEBUG( + this->get_logger(), + "Dropping weak reference to goal handle during status callback"); + goal_handles_.erase(goal_id); + continue; + } + goal_handle->set_status(status.status); + } +} + +void +GenericClient::make_result_aware(typename GenericClientGoalHandle::SharedPtr goal_handle) +{ + // Avoid making more than one request + if (goal_handle->set_result_awareness(true)) { + return; + } + + auto goal_result_request = create_result_request(); + + size_t goal_id_offset = 0; + for (uint32_t i = 0; i < goal_service_request_type_members_->member_count_; i++) { + if (!strcmp(goal_service_request_type_members_->members_[i].name_, "goal_id")) { + goal_id_offset = goal_service_request_type_members_->members_[i].offset_; + break; + } + } + + // Set uuid for result request message + auto * uuid = reinterpret_cast( + static_cast(goal_result_request.get()) + goal_id_offset); + uuid->uuid = goal_handle->get_goal_id(); + + try { + 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 + WrappedResult wrapped_result; + + // Get the offsets for the status and result fields + size_t status_offset; + size_t result_offset; + for (uint32_t i = 0; i < result_service_response_type_members_->member_count_; i++) { + if (!std::strcmp(result_service_response_type_members_->members_[i].name_, "result")) { + result_offset = result_service_response_type_members_->members_[i].offset_; + continue; + } + if (!std::strcmp(result_service_response_type_members_->members_[i].name_, "status")) { + status_offset = result_service_response_type_members_->members_[i].offset_; + continue; + } + } + + // the result part will not be copied here since it is hard to quickly obtain the size of + // the result part. Instead, a pointer will be used to reference the result part. + wrapped_result.result = reinterpret_cast(response.get()) + result_offset; + + std::memcpy( + static_cast(&wrapped_result.code), + static_cast(reinterpret_cast(response.get()) + status_offset), + sizeof(int8_t)); // ROS_TYPE_INT8 + + wrapped_result.goal_id = goal_handle->get_goal_id(); + + // Ensure that the original result response message is not released since result part + // is not copied. + wrapped_result.result_response = response; + + goal_handle->set_result(wrapped_result); + + std::lock_guard lock(goal_handles_mutex_); + goal_handles_.erase(goal_handle->get_goal_id()); + }); + } catch (rclcpp::exceptions::RCLError & ex) { + // This will cause an exception when the user tries to access the result + goal_handle->invalidate(exceptions::UnawareGoalHandleError(ex.message)); + } +} + +std::shared_future +GenericClient::async_cancel( + typename CancelRequest::SharedPtr cancel_request, + CancelCallback cancel_callback) +{ + // 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), + [cancel_callback, promise](std::shared_ptr response) mutable + { + auto cancel_response = std::static_pointer_cast(response); + promise->set_value(cancel_response); + if (cancel_callback) { + cancel_callback(cancel_response); + } + }); + return future; +} +} // namespace rclcpp_action diff --git a/rclcpp_action/src/generic_client_goal_handle.cpp b/rclcpp_action/src/generic_client_goal_handle.cpp new file mode 100644 index 0000000000..1f614f1177 --- /dev/null +++ b/rclcpp_action/src/generic_client_goal_handle.cpp @@ -0,0 +1,155 @@ +// Copyright 2025 Sony Group Corporation. +// +// 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. + +#include "rclcpp_action/generic_client_goal_handle.hpp" + +#include "rclcpp/logging.hpp" + +namespace rclcpp_action +{ +GenericClientGoalHandle::GenericClientGoalHandle( + const GoalInfo & info, FeedbackCallback feedback_callback, ResultCallback result_callback) +: info_(info), + result_future_(result_promise_.get_future()), + feedback_callback_(feedback_callback), + result_callback_(result_callback) +{ +} + +GenericClientGoalHandle::~GenericClientGoalHandle() +{} + +const GoalUUID & +GenericClientGoalHandle::get_goal_id() const +{ + return info_.goal_id.uuid; +} + +rclcpp::Time +GenericClientGoalHandle::get_goal_stamp() const +{ + return info_.stamp; +} + +std::shared_future +GenericClientGoalHandle::async_get_result() +{ + std::lock_guard guard(handle_mutex_); + if (!is_result_aware_) { + throw exceptions::UnawareGoalHandleError(); + } + return result_future_; +} + +void +GenericClientGoalHandle::set_result(const WrappedResult & wrapped_result) +{ + std::lock_guard guard(handle_mutex_); + status_ = static_cast(wrapped_result.code); + result_promise_.set_value(wrapped_result); + if (result_callback_) { + result_callback_(wrapped_result); + result_callback_ = ResultCallback(); + } +} + +void +GenericClientGoalHandle::set_feedback_callback(FeedbackCallback callback) +{ + std::lock_guard guard(handle_mutex_); + feedback_callback_ = callback; +} + +void +GenericClientGoalHandle::set_result_callback(ResultCallback callback) +{ + std::lock_guard guard(handle_mutex_); + result_callback_ = callback; +} + +int8_t +GenericClientGoalHandle::get_status() +{ + std::lock_guard guard(handle_mutex_); + return status_; +} + +void +GenericClientGoalHandle::set_status(int8_t status) +{ + std::lock_guard guard(handle_mutex_); + status_ = status; +} + +bool +GenericClientGoalHandle::is_feedback_aware() +{ + std::lock_guard guard(handle_mutex_); + return feedback_callback_ != nullptr; +} + +bool +GenericClientGoalHandle::is_result_aware() +{ + std::lock_guard guard(handle_mutex_); + return is_result_aware_; +} + +bool +GenericClientGoalHandle::set_result_awareness(bool awareness) +{ + std::lock_guard guard(handle_mutex_); + bool previous = is_result_aware_; + is_result_aware_ = awareness; + return previous; +} + +void +GenericClientGoalHandle::invalidate(const exceptions::UnawareGoalHandleError & ex) +{ + std::lock_guard guard(handle_mutex_); + // Guard against multiple calls + if (is_invalidated()) { + return; + } + is_result_aware_ = false; + invalidate_exception_ = std::make_exception_ptr(ex); + status_ = GoalStatus::STATUS_UNKNOWN; + result_promise_.set_exception(invalidate_exception_); +} + +bool +GenericClientGoalHandle::is_invalidated() const +{ + return invalidate_exception_ != nullptr; +} + +void +GenericClientGoalHandle::call_feedback_callback( + typename GenericClientGoalHandle::SharedPtr shared_this, + const void * 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 diff --git a/rclcpp_action/test/test_generic_client.cpp b/rclcpp_action/test/test_generic_client.cpp new file mode 100644 index 0000000000..4c70ef6c79 --- /dev/null +++ b/rclcpp_action/test/test_generic_client.cpp @@ -0,0 +1,1164 @@ +// Copyright 2025 Sony Group Corporation. +// +// 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. + +#include +#include + +#include "gtest/gtest.h" + +#include "rcl/allocator.h" +#include "rcl/time.h" +#include "rcl/types.h" + +#include "rcl_action/default_qos.h" +#include "rcl_action/names.h" +#include "rcl_action/wait.h" + +#include "rclcpp/node.hpp" +#include "rclcpp/executors.hpp" + +#include "rclcpp_action/client_goal_handle.hpp" +#include "rclcpp_action/create_generic_client.hpp" +#include "rclcpp_action/generic_client.hpp" +#include "rclcpp_action/qos.hpp" +#include "rclcpp_action/server.hpp" + +#include "test_msgs/action/fibonacci.hpp" + +#include "mocking_utils/patch.hpp" + +using namespace std::chrono_literals; + +// Refer to the cases in test_client.cpp to implement tests for GenericClient + +const auto WAIT_FOR_SERVER_TIMEOUT = 10s; + +class TestGenericClient : public ::testing::Test +{ +protected: + using ActionType = test_msgs::action::Fibonacci; + using ActionGoal = ActionType::Goal; + using ActionResult = ActionType::Result; + using ActionGoalHandle = rclcpp_action::ClientGoalHandle; + using ActionGoalRequestService = ActionType::Impl::SendGoalService; + using ActionGoalRequest = ActionGoalRequestService::Request; + using ActionGoalResponse = ActionGoalRequestService::Response; + using ActionGoalResultService = ActionType::Impl::GetResultService; + using ActionGoalResultRequest = ActionGoalResultService::Request; + using ActionGoalResultResponse = ActionGoalResultService::Response; + using ActionCancelGoalService = ActionType::Impl::CancelGoalService; + using ActionCancelGoalRequest = ActionType::Impl::CancelGoalService::Request; + using ActionCancelGoalResponse = ActionType::Impl::CancelGoalService::Response; + using ActionStatusMessage = ActionType::Impl::GoalStatusMessage; + using ActionFeedbackMessage = ActionType::Impl::FeedbackMessage; + using ActionFeedback = ActionType::Feedback; + + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + void SetUpServer() + { + 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->goal.order >= 0); + if (response->accepted) { + goals[request->goal_id.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->goal_id.uuid) == 1) { + auto goal_request = goals[request->goal_id.uuid].first; + auto goal_response = goals[request->goal_id.uuid].second; + ActionStatusMessage status_message; + rclcpp_action::GoalStatus goal_status; + goal_status.goal_info.goal_id.uuid = goal_request->goal_id.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.goal_id.uuid = goal_request->goal_id.uuid; + feedback_message.feedback.sequence.push_back(0); + feedback_publisher->publish(feedback_message); + client_executor.spin_once(); + if (goal_request->goal.order > 0) { + feedback_message.feedback.sequence.push_back(1); + feedback_publisher->publish(feedback_message); + client_executor.spin_once(); + for (size_t i = 1; i < static_cast(goal_request->goal.order); ++i) { + feedback_message.feedback.sequence.push_back( + feedback_message.feedback.sequence[i] + + feedback_message.feedback.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->result.sequence = feedback_message.feedback.sequence; + response->status = rclcpp_action::GoalStatus::STATUS_SUCCEEDED; + goals.erase(request->goal_id.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::GoalUUID 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->goal_id.uuid || + cancel_stamp > goal_stamp); + if (cancel_all || cancel_this) { + rclcpp_action::GoalStatus goal_status; + goal_status.goal_info.goal_id.uuid = goal_request->goal_id.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, 10); + 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, rclcpp_action::DefaultActionStatusQoS()); + ASSERT_TRUE(status_publisher != nullptr); + allocator.deallocate(status_topic_name, allocator.state); + server_executor.add_node(server_node); + } + + void SetUp() override + { + 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))); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void TearDownServer() + { + status_publisher.reset(); + feedback_publisher.reset(); + cancel_service.reset(); + result_service.reset(); + goal_service.reset(); + server_node.reset(); + } + + void TearDown() override + { + 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::GoalUUID, + 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(TestGenericClient, construction_with_free_function) { + { + ASSERT_NO_THROW({ + auto client = rclcpp_action::create_generic_client( + client_node->get_node_base_interface(), + client_node->get_node_graph_interface(), + client_node->get_node_logging_interface(), + client_node->get_node_waitables_interface(), + "fibonacci", + "test_msgs/action/Fibonacci", + nullptr); + }); + } + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_client_init, RCL_RET_ERROR); + ASSERT_THROW( + { + auto client = rclcpp_action::create_generic_client( + client_node->get_node_base_interface(), + client_node->get_node_graph_interface(), + client_node->get_node_logging_interface(), + client_node->get_node_waitables_interface(), + action_name, + "test_msgs/action/Fibonacci", + nullptr); + }, rclcpp::exceptions::RCLError); + } + { + ASSERT_NO_THROW({ + auto client = rclcpp_action::create_generic_client( + client_node, + action_name, + "test_msgs/action/Fibonacci", + nullptr); + }); + } + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_client_init, RCL_RET_ERROR); + ASSERT_THROW( + { + auto client = rclcpp_action::create_generic_client( + client_node, + action_name, + "test_msgs/action/Fibonacci", + nullptr); + }, rclcpp::exceptions::RCLError); + } +} + +TEST_F(TestGenericClient, construction_and_destruction_after_node) +{ + ASSERT_NO_THROW( + { + auto action_client = rclcpp_action::create_generic_client( + client_node->get_node_base_interface(), + client_node->get_node_graph_interface(), + client_node->get_node_logging_interface(), + client_node->get_node_waitables_interface(), + action_name, + "test_msgs/action/Fibonacci", + nullptr); + client_node.reset(); + }); +} + +TEST_F(TestGenericClient, construction_and_destruction_callback_group) +{ + auto group = client_node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + const rcl_action_client_options_t & options = rcl_action_client_get_default_options(); + ASSERT_NO_THROW( + rclcpp_action::create_generic_client( + client_node->get_node_base_interface(), + client_node->get_node_graph_interface(), + client_node->get_node_logging_interface(), + client_node->get_node_waitables_interface(), + action_name, + "test_msgs/action/Fibonacci", + group, + options).reset()); +} + +TEST_F(TestGenericClient, wait_for_action_server) +{ + auto action_generic_client = rclcpp_action::create_generic_client( + client_node, + action_name, + "test_msgs/action/Fibonacci"); + EXPECT_FALSE(action_generic_client->wait_for_action_server(0ms)); + EXPECT_FALSE(action_generic_client->wait_for_action_server(10ms)); + SetUpServer(); + ASSERT_TRUE(action_generic_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + TearDownServer(); + + client_node.reset(); // Drop node before action client + EXPECT_THROW(action_generic_client->wait_for_action_server(0ms), + rclcpp::exceptions::InvalidNodeError); +} + +TEST_F(TestGenericClient, wait_for_action_server_rcl_errors) +{ + auto action_generic_client = rclcpp_action::create_generic_client( + client_node, + action_name, + "test_msgs/action/Fibonacci"); + SetUpServer(); + ASSERT_TRUE(action_generic_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_server_is_available, RCL_RET_NODE_INVALID); + EXPECT_THROW(action_generic_client->action_server_is_ready(), rclcpp::exceptions::RCLError); + + auto mock_context_is_valid = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_context_is_valid, false); + EXPECT_FALSE(action_generic_client->action_server_is_ready()); + } + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_server_is_available, RCL_RET_ERROR); + EXPECT_THROW(action_generic_client->action_server_is_ready(), rclcpp::exceptions::RCLError); + } + TearDownServer(); +} + +TEST_F(TestGenericClient, is_ready) { + auto action_generic_client = rclcpp_action::create_generic_client( + client_node, + action_name, + "test_msgs/action/Fibonacci"); + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + auto rcl_context = client_node->get_node_base_interface()->get_context()->get_rcl_context().get(); + ASSERT_EQ( + RCL_RET_OK, + rcl_wait_set_init(&wait_set, 10, 10, 10, 10, 10, 10, rcl_context, allocator)); + ASSERT_NO_THROW(action_generic_client->add_to_wait_set(wait_set)); + EXPECT_TRUE(action_generic_client->is_ready(wait_set)); + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_client_wait_set_get_entities_ready, RCL_RET_ERROR); + EXPECT_THROW(action_generic_client->is_ready(wait_set), rclcpp::exceptions::RCLError); + } + client_node.reset(); // Drop node before action client +} + +class TestGenericClientAgainstServer : public TestGenericClient +{ +protected: + void SetUp() override + { + SetUpServer(); + TestGenericClient::SetUp(); + } + + void TearDown() override + { + TestGenericClient::TearDown(); + TearDownServer(); + } +}; + +TEST_F(TestGenericClientAgainstServer, async_send_goal_no_callbacks) +{ + auto action_generic_client = rclcpp_action::create_generic_client( + client_node, + action_name, + "test_msgs/action/Fibonacci"); + ASSERT_TRUE(action_generic_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal bad_goal; + bad_goal.order = -5; + auto future_goal_handle = + action_generic_client->async_send_goal(&bad_goal, sizeof(bad_goal)); + 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_generic_client->async_send_goal(&good_goal, sizeof(good_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_FALSE(goal_handle->is_result_aware()); +} + +TEST_F(TestGenericClientAgainstServer, async_send_goal_request_no_callbacks) +{ + auto action_generic_client = rclcpp_action::create_generic_client( + client_node, + action_name, + "test_msgs/action/Fibonacci"); + ASSERT_TRUE(action_generic_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoalRequest bad_goal_request; + bad_goal_request.goal.order = -5; + auto future_goal_handle = + action_generic_client->async_send_goal(&bad_goal_request); + dual_spin_until_future_complete(future_goal_handle); + EXPECT_EQ(nullptr, future_goal_handle.get().get()); + + ActionGoalRequest good_goal_request; + good_goal_request.goal.order = 5; + future_goal_handle = + action_generic_client->async_send_goal(&good_goal_request); + 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()); +} + +TEST_F(TestGenericClientAgainstServer, bad_goal_handles) +{ + auto action_generic_client0 = rclcpp_action::create_generic_client( + client_node, + action_name, + "test_msgs/action/Fibonacci"); + ASSERT_TRUE(action_generic_client0->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + goal.order = 0; + auto future_goal_handle = action_generic_client0->async_send_goal(&goal, sizeof(goal)); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + + auto action_generic_client1 = rclcpp_action::create_generic_client( + client_node, + action_name, + "test_msgs/action/Fibonacci"); + using rclcpp_action::exceptions::UnknownGoalHandleError; + EXPECT_THROW(action_generic_client1->async_get_result(goal_handle), UnknownGoalHandleError); + EXPECT_THROW(action_generic_client1->async_cancel_goal(goal_handle), UnknownGoalHandleError); +} + +TEST_F(TestGenericClientAgainstServer, async_send_goal_no_callbacks_wait_for_result) +{ + auto action_generic_client = rclcpp_action::create_generic_client( + client_node, + action_name, + "test_msgs/action/Fibonacci"); + ASSERT_TRUE(action_generic_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + goal.order = 5; + auto future_goal_handle = action_generic_client->async_send_goal(&goal, sizeof(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_FALSE(goal_handle->is_result_aware()); + auto future_result = action_generic_client->async_get_result(goal_handle); + EXPECT_TRUE(goal_handle->is_result_aware()); + dual_spin_until_future_complete(future_result); + auto wrapped_result = future_result.get(); + const ActionResult * result = static_cast(wrapped_result.result); + EXPECT_EQ(wrapped_result.code, rclcpp_action::GenericClientGoalHandle::ResultCode::SUCCEEDED); + EXPECT_EQ(6ul, result->sequence.size()); + EXPECT_EQ(0, result->sequence[0]); + EXPECT_EQ(1, result->sequence[1]); + EXPECT_EQ(5, result->sequence[5]); +} + +TEST_F(TestGenericClientAgainstServer, async_send_goal_no_callbacks_then_invalidate) +{ + auto action_generic_client = rclcpp_action::create_generic_client( + client_node, + action_name, + "test_msgs/action/Fibonacci"); + ASSERT_TRUE(action_generic_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + goal.order = 5; + auto future_goal_handle = action_generic_client->async_send_goal(&goal, sizeof(goal)); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + ASSERT_NE(nullptr, goal_handle); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); + auto future_result = action_generic_client->async_get_result(goal_handle); + EXPECT_TRUE(goal_handle->is_result_aware()); + + action_generic_client.reset(); // Ensure goal handle is invalidated once client goes out of scope + + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_UNKNOWN, goal_handle->get_status()); + using rclcpp_action::exceptions::UnawareGoalHandleError; + EXPECT_THROW(future_result.get(), UnawareGoalHandleError); +} + +TEST_F(TestGenericClientAgainstServer, async_send_goal_with_goal_response_callback_wait_for_result) +{ + auto action_generic_client = rclcpp_action::create_generic_client( + client_node, + action_name, + "test_msgs/action/Fibonacci"); + ASSERT_TRUE(action_generic_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + bool goal_response_received = false; + auto send_goal_ops = rclcpp_action::GenericClient::SendGoalOptions(); + send_goal_ops.goal_response_callback = + [&goal_response_received]( + typename rclcpp_action::GenericClientGoalHandle::SharedPtr goal_handle) + { + if (goal_handle) { + goal_response_received = true; + } + }; + + { + ActionGoal bad_goal; + bad_goal.order = -1; + auto future_goal_handle = + action_generic_client->async_send_goal(&bad_goal, sizeof(bad_goal), send_goal_ops); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + EXPECT_FALSE(goal_response_received); + EXPECT_EQ(nullptr, goal_handle); + } + + { + ActionGoal goal; + goal.order = 4; + auto future_goal_handle = + action_generic_client->async_send_goal(&goal, sizeof(goal), send_goal_ops); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + EXPECT_TRUE(goal_response_received); + 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()); + auto future_result = action_generic_client->async_get_result(goal_handle); + EXPECT_TRUE(goal_handle->is_result_aware()); + dual_spin_until_future_complete(future_result); + auto wrapped_result = future_result.get(); + const ActionResult * result = (const ActionResult *)(wrapped_result.result); + ASSERT_EQ(5u, result->sequence.size()); + EXPECT_EQ(3, result->sequence.back()); + } +} + +TEST_F(TestGenericClientAgainstServer, async_send_goal_with_feedback_callback_wait_for_result) +{ + auto action_generic_client = rclcpp_action::create_generic_client( + client_node, + action_name, + "test_msgs/action/Fibonacci"); + ASSERT_TRUE(action_generic_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + goal.order = 4; + int feedback_count = 0; + auto send_goal_ops = rclcpp_action::GenericClient::SendGoalOptions(); + send_goal_ops.feedback_callback = [&feedback_count]( + typename rclcpp_action::GenericClientGoalHandle::SharedPtr goal_handle, + const void * feedback) + { + (void)goal_handle; + (void)feedback; + feedback_count++; + }; + auto future_goal_handle = + action_generic_client->async_send_goal(&goal, sizeof(goal), send_goal_ops); + 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_FALSE(goal_handle->is_result_aware()); + auto future_result = action_generic_client->async_get_result(goal_handle); + EXPECT_TRUE(goal_handle->is_result_aware()); + dual_spin_until_future_complete(future_result); + auto wrapped_result = future_result.get(); + const ActionResult * result = (const ActionResult *)(wrapped_result.result); + ASSERT_EQ(5u, result->sequence.size()); + EXPECT_EQ(3, result->sequence.back()); + EXPECT_EQ(5, feedback_count); +} + +TEST_F(TestGenericClientAgainstServer, async_send_goal_with_result_callback_wait_for_result) +{ + auto action_generic_client = rclcpp_action::create_generic_client( + client_node, + action_name, + "test_msgs/action/Fibonacci"); + ASSERT_TRUE(action_generic_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + goal.order = 4; + bool result_callback_received = false; + auto send_goal_ops = rclcpp_action::GenericClient::SendGoalOptions(); + send_goal_ops.result_callback = + [&result_callback_received]( + const rclcpp_action::GenericClientGoalHandle::WrappedResult & wrapped_result) + { + const ActionResult * result = (const ActionResult *)(wrapped_result.result); + if (rclcpp_action::GenericClientGoalHandle::ResultCode::SUCCEEDED == wrapped_result.code && + result->sequence.size() == 5u) + { + result_callback_received = true; + } + }; + auto future_goal_handle = + action_generic_client->async_send_goal(&goal, sizeof(goal), send_goal_ops); + 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 = action_generic_client->async_get_result(goal_handle); + dual_spin_until_future_complete(future_result); + auto wrapped_result = future_result.get(); + const ActionResult * result = (const ActionResult *)(wrapped_result.result); + EXPECT_TRUE(result_callback_received); + ASSERT_EQ(5u, result->sequence.size()); + EXPECT_EQ(3, result->sequence.back()); +} + +TEST_F(TestGenericClientAgainstServer, async_get_result_with_callback) +{ + auto action_generic_client = rclcpp_action::create_generic_client( + client_node, + action_name, + "test_msgs/action/Fibonacci"); + ASSERT_TRUE(action_generic_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + goal.order = 4; + auto future_goal_handle = action_generic_client->async_send_goal(&goal, sizeof(goal)); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + EXPECT_NE(goal_handle, nullptr); + 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()); + bool result_callback_received = false; + auto future_result = action_generic_client->async_get_result( + goal_handle, + [&result_callback_received]( + const rclcpp_action::GenericClientGoalHandle::WrappedResult & wrapped_result) + { + const ActionResult * result = (const ActionResult *)(wrapped_result.result); + if (rclcpp_action::GenericClientGoalHandle::ResultCode::SUCCEEDED == wrapped_result.code && + result->sequence.size() == 5u) + { + result_callback_received = true; + } + }); + dual_spin_until_future_complete(future_result); + auto wrapped_result = future_result.get(); + const ActionResult * result = (const ActionResult *)(wrapped_result.result); + EXPECT_TRUE(result_callback_received); + ASSERT_EQ(5u, result->sequence.size()); + EXPECT_EQ(3, result->sequence.back()); +} + +TEST_F(TestGenericClientAgainstServer, async_cancel_one_goal) +{ + auto action_generic_client = rclcpp_action::create_generic_client( + client_node, + action_name, + "test_msgs/action/Fibonacci"); + ASSERT_TRUE(action_generic_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + goal.order = 5; + auto future_goal_handle = action_generic_client->async_send_goal(&goal, sizeof(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()); + + auto future_cancel = action_generic_client->async_cancel_goal(goal_handle); + dual_spin_until_future_complete(future_cancel); + ActionCancelGoalResponse::SharedPtr cancel_response = future_cancel.get(); + EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code); +} + +TEST_F(TestGenericClientAgainstServer, async_cancel_one_goal_with_callback) +{ + auto action_generic_client = rclcpp_action::create_generic_client( + client_node, + action_name, + "test_msgs/action/Fibonacci"); + ASSERT_TRUE(action_generic_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + goal.order = 5; + auto future_goal_handle = action_generic_client->async_send_goal(&goal, sizeof(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()); + + bool cancel_response_received = false; + auto future_cancel = action_generic_client->async_cancel_goal( + goal_handle, + [&cancel_response_received, goal_handle]( + ActionCancelGoalResponse::SharedPtr response) + { + if ( + ActionCancelGoalResponse::ERROR_NONE == response->return_code && + 1ul == response->goals_canceling.size() && + goal_handle->get_goal_id() == response->goals_canceling[0].goal_id.uuid) + { + cancel_response_received = true; + } + }); + dual_spin_until_future_complete(future_cancel); + auto cancel_response = future_cancel.get(); + EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code); + ASSERT_EQ(1ul, cancel_response->goals_canceling.size()); + EXPECT_EQ(goal_handle->get_goal_id(), cancel_response->goals_canceling[0].goal_id.uuid); + EXPECT_TRUE(cancel_response_received); +} + +TEST_F(TestGenericClientAgainstServer, async_cancel_all_goals) +{ + auto action_generic_client = rclcpp_action::create_generic_client( + client_node, + action_name, + "test_msgs/action/Fibonacci"); + ASSERT_TRUE(action_generic_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + goal.order = 6; + auto future_goal_handle0 = action_generic_client->async_send_goal(&goal, sizeof(goal)); + 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_generic_client->async_send_goal(&goal, sizeof(goal)); + 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_generic_client->async_cancel_all_goals(); + dual_spin_until_future_complete(future_cancel_all); + auto cancel_response = future_cancel_all.get(); + + EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code); + 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(TestGenericClientAgainstServer, async_cancel_all_goals_with_callback) +{ + auto action_generic_client = rclcpp_action::create_generic_client( + client_node, + action_name, + "test_msgs/action/Fibonacci"); + ASSERT_TRUE(action_generic_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + goal.order = 6; + auto future_goal_handle0 = action_generic_client->async_send_goal(&goal, sizeof(goal)); + 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_generic_client->async_send_goal(&goal, sizeof(goal)); + 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))); + + bool cancel_callback_received = false; + auto future_cancel_all = action_generic_client->async_cancel_all_goals( + [&cancel_callback_received, goal_handle0, goal_handle1]( + ActionCancelGoalResponse::SharedPtr response) + { + if ( + response && + 2ul == response->goals_canceling.size() && + goal_handle0->get_goal_id() == response->goals_canceling[0].goal_id.uuid && + goal_handle1->get_goal_id() == response->goals_canceling[1].goal_id.uuid) + { + cancel_callback_received = true; + } + }); + dual_spin_until_future_complete(future_cancel_all); + auto cancel_response = future_cancel_all.get(); + + EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code); + EXPECT_TRUE(cancel_callback_received); + 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(TestGenericClientAgainstServer, async_cancel_some_goals) +{ + auto action_generic_client = rclcpp_action::create_generic_client( + client_node, + action_name, + "test_msgs/action/Fibonacci"); + ASSERT_TRUE(action_generic_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + goal.order = 6; + auto future_goal_handle0 = action_generic_client->async_send_goal(&goal, sizeof(goal)); + 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_generic_client->async_send_goal(&goal, sizeof(goal)); + 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_generic_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(); + + EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code); + 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()); +} + +TEST_F(TestGenericClientAgainstServer, async_cancel_some_goals_with_callback) +{ + auto action_generic_client = rclcpp_action::create_generic_client( + client_node, + action_name, + "test_msgs/action/Fibonacci"); + ASSERT_TRUE(action_generic_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + goal.order = 6; + auto future_goal_handle0 = action_generic_client->async_send_goal(&goal, sizeof(goal)); + 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_generic_client->async_send_goal(&goal, sizeof(goal)); + 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))); + + bool cancel_callback_received = false; + auto future_cancel_some = action_generic_client->async_cancel_goals_before( + goal_handle1->get_goal_stamp(), + [&cancel_callback_received, goal_handle0](ActionCancelGoalResponse::SharedPtr response) + { + if ( + response && + 1ul == response->goals_canceling.size() && + goal_handle0->get_goal_id() == response->goals_canceling[0].goal_id.uuid) + { + cancel_callback_received = true; + } + }); + dual_spin_until_future_complete(future_cancel_some); + auto cancel_response = future_cancel_some.get(); + + EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code); + EXPECT_TRUE(cancel_callback_received); + 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()); +} + +TEST_F(TestGenericClientAgainstServer, deadlock_in_callbacks) +{ + std::atomic feedback_callback_called = false; + std::atomic response_callback_called = false; + std::atomic result_callback_called = false; + std::atomic no_deadlock = false; + + std::thread tr = std::thread( + [&]() { + auto action_generic_client = rclcpp_action::create_generic_client( + client_node, + action_name, + "test_msgs/action/Fibonacci"); + ASSERT_TRUE(action_generic_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + + rclcpp_action::GenericClient::SendGoalOptions ops; + ops.feedback_callback = [&feedback_callback_called]( + typename rclcpp_action::GenericClientGoalHandle::SharedPtr handle, const void *) + { + // call functions on the handle that acquire the lock + handle->get_status(); + handle->is_feedback_aware(); + handle->is_result_aware(); + + feedback_callback_called = true; + }; + ops.goal_response_callback = [&response_callback_called]( + const rclcpp_action::GenericClientGoalHandle::SharedPtr & handle) { + // call functions on the handle that acquire the lock + handle->get_status(); + handle->is_feedback_aware(); + handle->is_result_aware(); + + response_callback_called = true; + }; + ops.result_callback = [&result_callback_called]( + const rclcpp_action::GenericClientGoalHandle::WrappedResult & ) { + result_callback_called = true; + }; + + goal.order = 6; + auto future_goal_handle = action_generic_client->async_send_goal(&goal, sizeof(goal), ops); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + + ASSERT_TRUE(goal_handle); + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(2))); + + auto result_future = action_generic_client->async_get_result(goal_handle); + dual_spin_until_future_complete(result_future); + + EXPECT_TRUE(result_future.valid()); + auto result = result_future.get(); + + no_deadlock = true; + }); + + auto start_time = std::chrono::system_clock::now(); + + while (std::chrono::system_clock::now() - start_time < std::chrono::milliseconds(2000) && + !no_deadlock) + { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + if (no_deadlock) { + tr.join(); + } else { + // In case of a failure, the thread is assumed to be in a deadlock. + // We detach the thread so we don't block further tests. + tr.detach(); + } + + EXPECT_TRUE(no_deadlock); + EXPECT_TRUE(response_callback_called); + EXPECT_TRUE(result_callback_called); + EXPECT_TRUE(feedback_callback_called); +} + +TEST_F(TestGenericClientAgainstServer, send_rcl_errors) +{ + auto action_generic_client = rclcpp_action::create_generic_client( + client_node, + action_name, + "test_msgs/action/Fibonacci"); + ASSERT_TRUE(action_generic_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + auto send_goal_ops = rclcpp_action::GenericClient::SendGoalOptions(); + send_goal_ops.result_callback = + [](const rclcpp_action::GenericClientGoalHandle::WrappedResult &) {}; + send_goal_ops.feedback_callback = []( + typename rclcpp_action::GenericClientGoalHandle::SharedPtr, const void *) {}; + + { + ActionGoal goal; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_send_goal_request, RCL_RET_ERROR); + EXPECT_THROW( + action_generic_client->async_send_goal(&goal, sizeof(goal), send_goal_ops), + rclcpp::exceptions::RCLError); + } + { + ActionGoal goal; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_send_result_request, RCL_RET_ERROR); + auto future_goal_handle = + action_generic_client->async_send_goal(&goal, sizeof(goal), send_goal_ops); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_UNKNOWN, goal_handle->get_status()); + } + { + ActionGoal goal; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_send_cancel_request, RCL_RET_ERROR); + auto future_goal_handle = + action_generic_client->async_send_goal(&goal, sizeof(goal), send_goal_ops); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + EXPECT_THROW( + action_generic_client->async_cancel_goals_before(goal_handle->get_goal_stamp()), + rclcpp::exceptions::RCLError); + } +} + +TEST_F(TestGenericClientAgainstServer, execute_rcl_errors) +{ + auto action_generic_client = rclcpp_action::create_generic_client( + client_node, + action_name, + "test_msgs/action/Fibonacci"); + ASSERT_TRUE(action_generic_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + auto send_goal_ops = rclcpp_action::GenericClient::SendGoalOptions(); + send_goal_ops.result_callback = + [](const rclcpp_action::GenericClientGoalHandle::WrappedResult &) {}; + send_goal_ops.feedback_callback = []( + typename rclcpp_action::GenericClientGoalHandle::SharedPtr, const void *) {}; + + { + ActionGoal goal; + goal.order = 5; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_take_feedback, RCL_RET_ERROR); + + auto future_goal_handle = + action_generic_client->async_send_goal(&goal, sizeof(goal), + send_goal_ops); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + auto future_result = action_generic_client->async_get_result(goal_handle); + EXPECT_THROW( + dual_spin_until_future_complete(future_result), + rclcpp::exceptions::RCLError); + } + { + ActionGoal goal; + goal.order = 5; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_take_goal_response, RCL_RET_ERROR); + + auto future_goal_handle = + action_generic_client->async_send_goal(&goal, sizeof(goal), send_goal_ops); + EXPECT_THROW( + dual_spin_until_future_complete(future_goal_handle), + rclcpp::exceptions::RCLError); + } + { + ActionGoal goal; + goal.order = 5; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_take_result_response, RCL_RET_ERROR); + + auto future_goal_handle = + action_generic_client->async_send_goal(&goal, sizeof(goal), send_goal_ops); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + auto future_result = action_generic_client->async_get_result(goal_handle); + EXPECT_THROW( + dual_spin_until_future_complete(future_result), + rclcpp::exceptions::RCLError); + } + { + ActionGoal goal; + goal.order = 5; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_take_cancel_response, RCL_RET_ERROR); + + auto future_goal_handle = + action_generic_client->async_send_goal(&goal, sizeof(goal), send_goal_ops); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + auto future_cancel_some = + action_generic_client->async_cancel_goals_before(goal_handle->get_goal_stamp()); + EXPECT_THROW( + dual_spin_until_future_complete(future_cancel_some), + rclcpp::exceptions::RCLError); + } +} From 9f366f0131c155db9beb1eee6af0e9a6ef134066 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Tue, 4 Mar 2025 15:17:04 +0800 Subject: [PATCH 2/3] Add the missing header files and address other review comments Signed-off-by: Barry Xu --- rclcpp_action/include/rclcpp_action/client_base.hpp | 4 ++++ .../include/rclcpp_action/create_generic_client.hpp | 2 ++ rclcpp_action/include/rclcpp_action/generic_client.hpp | 6 +++++- .../include/rclcpp_action/generic_client_goal_handle.hpp | 2 ++ rclcpp_action/src/create_generic_client.cpp | 1 + rclcpp_action/src/generic_client.cpp | 7 +++++++ rclcpp_action/src/generic_client_goal_handle.cpp | 4 ++++ rclcpp_action/test/test_generic_client.cpp | 5 +++++ 8 files changed, 30 insertions(+), 1 deletion(-) diff --git a/rclcpp_action/include/rclcpp_action/client_base.hpp b/rclcpp_action/include/rclcpp_action/client_base.hpp index 9f8783d88d..9d10d66c7e 100644 --- a/rclcpp_action/include/rclcpp_action/client_base.hpp +++ b/rclcpp_action/include/rclcpp_action/client_base.hpp @@ -15,8 +15,12 @@ #ifndef RCLCPP_ACTION__CLIENT_BASE_HPP_ #define RCLCPP_ACTION__CLIENT_BASE_HPP_ +#include +#include #include +#include #include +#include #include #include #include diff --git a/rclcpp_action/include/rclcpp_action/create_generic_client.hpp b/rclcpp_action/include/rclcpp_action/create_generic_client.hpp index b256ab13bd..8dcf68295f 100644 --- a/rclcpp_action/include/rclcpp_action/create_generic_client.hpp +++ b/rclcpp_action/include/rclcpp_action/create_generic_client.hpp @@ -35,6 +35,7 @@ namespace rclcpp_action * \param[in] group The action client will be added to this callback group. * If `nullptr`, then the action client is added to the default callback group. * \param[in] options Options to pass to the underlying `rcl_action_client_t`. + * \return newly created generic client. */ typename GenericClient::SharedPtr create_generic_client( @@ -55,6 +56,7 @@ create_generic_client( * \param[in] group The action client will be added to this callback group. * If `nullptr`, then the action client is added to the default callback group. * \param[in] options Options to pass to the underlying `rcl_action_client_t`. + * \return newly created generic client. */ template typename GenericClient::SharedPtr diff --git a/rclcpp_action/include/rclcpp_action/generic_client.hpp b/rclcpp_action/include/rclcpp_action/generic_client.hpp index 0e701099a4..bbd79b6b12 100644 --- a/rclcpp_action/include/rclcpp_action/generic_client.hpp +++ b/rclcpp_action/include/rclcpp_action/generic_client.hpp @@ -15,8 +15,12 @@ #ifndef RCLCPP_ACTION__GENERIC_CLIENT_HPP_ #define RCLCPP_ACTION__GENERIC_CLIENT_HPP_ +#include +#include +#include #include #include +#include #include #include "action_msgs/srv/cancel_goal.hpp" @@ -143,7 +147,7 @@ class GenericClient : public ClientBase /// Send an action goal request and asynchronously get the result. /** - * \param[in] goal The goal request (uuid+goal). + * \param[in] goal_request The goal request (uuid+goal). * \param[in] options Options for sending the goal request. Contains references to callbacks for * the goal response (accepted/rejected), feedback, and the final result. * \return A future that completes when the goal has been accepted or rejected. diff --git a/rclcpp_action/include/rclcpp_action/generic_client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/generic_client_goal_handle.hpp index cbe8b884cf..f8a4c0b33f 100644 --- a/rclcpp_action/include/rclcpp_action/generic_client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/generic_client_goal_handle.hpp @@ -15,6 +15,8 @@ #ifndef RCLCPP_ACTION__GENERIC_CLIENT_GOAL_HANDLE_HPP_ #define RCLCPP_ACTION__GENERIC_CLIENT_GOAL_HANDLE_HPP_ +#include +#include #include #include #include diff --git a/rclcpp_action/src/create_generic_client.cpp b/rclcpp_action/src/create_generic_client.cpp index d83d879864..7699a736ef 100644 --- a/rclcpp_action/src/create_generic_client.cpp +++ b/rclcpp_action/src/create_generic_client.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include "rclcpp/node.hpp" diff --git a/rclcpp_action/src/generic_client.cpp b/rclcpp_action/src/generic_client.cpp index fbeb3bb7bd..a3f77206dd 100644 --- a/rclcpp_action/src/generic_client.cpp +++ b/rclcpp_action/src/generic_client.cpp @@ -12,7 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include +#include +#include +#include #include +#include +#include #include "rclcpp_action/generic_client.hpp" diff --git a/rclcpp_action/src/generic_client_goal_handle.cpp b/rclcpp_action/src/generic_client_goal_handle.cpp index 1f614f1177..b0fad95903 100644 --- a/rclcpp_action/src/generic_client_goal_handle.cpp +++ b/rclcpp_action/src/generic_client_goal_handle.cpp @@ -12,6 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include +#include + #include "rclcpp_action/generic_client_goal_handle.hpp" #include "rclcpp/logging.hpp" diff --git a/rclcpp_action/test/test_generic_client.cpp b/rclcpp_action/test/test_generic_client.cpp index 4c70ef6c79..571c51a923 100644 --- a/rclcpp_action/test/test_generic_client.cpp +++ b/rclcpp_action/test/test_generic_client.cpp @@ -12,8 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include #include +#include +#include #include +#include #include "gtest/gtest.h" From 3f155e8597c7760fb44033224dc07992a7f7ad18 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Fri, 7 Mar 2025 11:27:14 +0800 Subject: [PATCH 3/3] Add the missing visibility control macros Signed-off-by: Barry Xu --- .../include/rclcpp_action/create_generic_client.hpp | 1 + rclcpp_action/include/rclcpp_action/generic_client.hpp | 10 ++++++++++ .../rclcpp_action/generic_client_goal_handle.hpp | 9 ++++++++- 3 files changed, 19 insertions(+), 1 deletion(-) diff --git a/rclcpp_action/include/rclcpp_action/create_generic_client.hpp b/rclcpp_action/include/rclcpp_action/create_generic_client.hpp index 8dcf68295f..c57f0bf004 100644 --- a/rclcpp_action/include/rclcpp_action/create_generic_client.hpp +++ b/rclcpp_action/include/rclcpp_action/create_generic_client.hpp @@ -37,6 +37,7 @@ namespace rclcpp_action * \param[in] options Options to pass to the underlying `rcl_action_client_t`. * \return newly created generic client. */ +RCLCPP_ACTION_PUBLIC typename GenericClient::SharedPtr create_generic_client( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, diff --git a/rclcpp_action/include/rclcpp_action/generic_client.hpp b/rclcpp_action/include/rclcpp_action/generic_client.hpp index bbd79b6b12..a8e19cfc52 100644 --- a/rclcpp_action/include/rclcpp_action/generic_client.hpp +++ b/rclcpp_action/include/rclcpp_action/generic_client.hpp @@ -104,6 +104,7 @@ class GenericClient : public ClientBase * \param[in] action_typesupport_handle the action type support handle * \param[in] client_options Options to pass to the underlying `rcl_action::rcl_action_client_t`. */ + RCLCPP_ACTION_PUBLIC GenericClient( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, @@ -139,6 +140,7 @@ class GenericClient : public ClientBase * \return A future that completes when the goal has been accepted or rejected. * If the goal is rejected, then the result will be a `nullptr`. */ + RCLCPP_ACTION_PUBLIC std::shared_future async_send_goal( const Goal goal, @@ -153,6 +155,7 @@ class GenericClient : public ClientBase * \return A future that completes when the goal has been accepted or rejected. * If the goal is rejected, then the result will be a `nullptr`. */ + RCLCPP_ACTION_PUBLIC std::shared_future async_send_goal( const GoalRequest goal_request, @@ -166,6 +169,7 @@ class GenericClient : public ClientBase * \param[in] result_callback Optional callback that is called when the result is received. * \return A future that is set to the goal result when the goal is finished. */ + RCLCPP_ACTION_PUBLIC std::shared_future async_get_result( typename GenericClientGoalHandle::SharedPtr goal_handle, @@ -184,6 +188,7 @@ class GenericClient : public ClientBase * * action_msgs/CancelGoal.srv. */ + RCLCPP_ACTION_PUBLIC std::shared_future async_cancel_goal( typename GenericClientGoalHandle::SharedPtr goal_handle, @@ -199,6 +204,7 @@ class GenericClient : public ClientBase * * action_msgs/CancelGoal.srv. */ + RCLCPP_ACTION_PUBLIC std::shared_future async_cancel_all_goals(CancelCallback cancel_callback = nullptr); @@ -213,6 +219,7 @@ class GenericClient : public ClientBase * * \param[in] goal_handle The goal were the callbacks shall be stopped */ + RCLCPP_ACTION_PUBLIC void stop_callbacks(typename GenericClientGoalHandle::SharedPtr goal_handle); @@ -220,6 +227,7 @@ class GenericClient : public ClientBase /** * For further information see stop_callbacks(typename GenericGoalHandle::SharedPtr goal_handle) */ + RCLCPP_ACTION_PUBLIC void stop_callbacks(const GoalUUID & goal_id); @@ -234,11 +242,13 @@ class GenericClient : public ClientBase * * action_msgs/CancelGoal.srv. */ + RCLCPP_ACTION_PUBLIC std::shared_future async_cancel_goals_before( const rclcpp::Time & stamp, CancelCallback cancel_callback = nullptr); + RCLCPP_ACTION_PUBLIC virtual ~GenericClient(); diff --git a/rclcpp_action/include/rclcpp_action/generic_client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/generic_client_goal_handle.hpp index f8a4c0b33f..1d7fa34884 100644 --- a/rclcpp_action/include/rclcpp_action/generic_client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/generic_client_goal_handle.hpp @@ -67,25 +67,32 @@ class GenericClientGoalHandle const void *)>; using ResultCallback = std::function; - virtual ~GenericClientGoalHandle(); + RCLCPP_ACTION_PUBLIC + virtual + ~GenericClientGoalHandle(); /// Get the unique ID for the goal. + RCLCPP_ACTION_PUBLIC const GoalUUID & get_goal_id() const; /// Get the time when the goal was accepted. + RCLCPP_ACTION_PUBLIC rclcpp::Time get_goal_stamp() const; /// Get the goal status code. + RCLCPP_ACTION_PUBLIC int8_t get_status(); /// Check if an action client has subscribed to feedback for the goal. + RCLCPP_ACTION_PUBLIC bool is_feedback_aware(); /// Check if an action client has requested the result for the goal. + RCLCPP_ACTION_PUBLIC bool is_result_aware();