diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index f29b63c5e8..a52d43f58b 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -111,24 +111,31 @@ class Client : public ClientBase * This constructs an action client, but it will not work until it has been added to a node. * Use `rclcpp_action::create_client()` to both construct and add to a node. * + * If enable_feedback_msg_optimization is set to true, an action client can handle up to 6 goals + * simultaneously. If the number of goals exceeds the limit, optimization is automatically + * disabled. + * * \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] client_options Options to pass to the underlying `rcl_action::rcl_action_client_t`. + * \param[in] enable_feedback_msg_optimization Enable feedback subscription content filter to + * optimize the handling of feedback messages. */ Client( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, const std::string & action_name, - const rcl_action_client_options_t & client_options = rcl_action_client_get_default_options() + const rcl_action_client_options_t & client_options = rcl_action_client_get_default_options(), + bool enable_feedback_msg_optimization = false ) : ClientBase( node_base, node_graph, node_logging, action_name, rosidl_typesupport_cpp::get_action_type_support_handle(), - client_options) + client_options, enable_feedback_msg_optimization) { } @@ -180,6 +187,20 @@ class Client : public ClientBase } return; } + + // If feedback message optimization is enabled, add goal id to feedback subscription + // content filter + if (enable_feedback_msg_optimization_) { + std::lock_guard lock(configure_feedback_sub_content_filter_mutex_); + if (!configure_feedback_subscription_filter_add_goal_id(goal_request->goal_id.uuid)) { + RCLCPP_ERROR( + this->get_logger(), + "Failed to add goal id to feedback subscription content filter for action client." + " Disable feedback message optimization."); + enable_feedback_msg_optimization_ = false; + } + } + GoalInfo goal_info; goal_info.goal_id.uuid = goal_request->goal_id.uuid; goal_info.stamp = goal_response->stamp; @@ -519,6 +540,22 @@ class Client : public ClientBase wrapped_result.goal_id = goal_handle->get_goal_id(); wrapped_result.code = static_cast(result_response->status); goal_handle->set_result(wrapped_result); + + // If feedback message optimization is enabled, remove goal id from feedback subscription + // content filter + if (this->enable_feedback_msg_optimization_) { + std::lock_guard lock( + this->configure_feedback_sub_content_filter_mutex_); + if (!this->configure_feedback_subscription_filter_remove_goal_id( + goal_handle->get_goal_id())) + { + RCLCPP_ERROR( + this->get_logger(), + "Failed to remove goal id from feedback subscription content filter for action " + "client. Disable feedback message optimization."); + this->enable_feedback_msg_optimization_ = false; + } + } std::lock_guard lock(goal_handles_mutex_); goal_handles_.erase(goal_handle->get_goal_id()); }); @@ -539,9 +576,30 @@ class Client : public ClientBase 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 + [this, cancel_callback, promise](std::shared_ptr response) mutable { auto cancel_response = std::static_pointer_cast(response); + + // If feedback message optimization is enabled, remove goal id from feedback subscription + // content filter + if (this->enable_feedback_msg_optimization_) { + for (const auto & goal_info : cancel_response->goals_canceling) { + std::lock_guard lock(this->configure_feedback_sub_content_filter_mutex_); + if (!this->configure_feedback_subscription_filter_remove_goal_id( + goal_info.goal_id.uuid)) + { + RCLCPP_ERROR( + this->get_logger(), + "Failed to remove goal id from feedback subscription content filter for action " + "client. Disable feedback message optimization."); + this->enable_feedback_msg_optimization_ = false; + // When an error occurs, the rcl layer will disable the content filter, so there is + // no need to continue removing the goal id. + break; + } + } + } + promise->set_value(cancel_response); if (cancel_callback) { cancel_callback(cancel_response); diff --git a/rclcpp_action/include/rclcpp_action/client_base.hpp b/rclcpp_action/include/rclcpp_action/client_base.hpp index 73cabfb9a0..10e51de6e8 100644 --- a/rclcpp_action/include/rclcpp_action/client_base.hpp +++ b/rclcpp_action/include/rclcpp_action/client_base.hpp @@ -201,7 +201,8 @@ class ClientBase : public rclcpp::Waitable 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); + const rcl_action_client_options_t & options, + bool enable_feedback_msg_optimization); /// Wait for action_server_is_ready() to become true, or until the given timeout is reached. RCLCPP_ACTION_PUBLIC @@ -295,6 +296,22 @@ class ClientBase : public rclcpp::Waitable void handle_feedback_message(std::shared_ptr message) = 0; + /// \internal + /// \return true if goal id was added to feedback subscription content filter successfully + /// \return false if an error occurred during calling rcl function or + /// if the used rmw middleware doesn't support Content Filtering feature. + bool + configure_feedback_subscription_filter_add_goal_id(const GoalUUID & goal_id); + + + /// \internal + /// \return true if goal id was removed from feedback subscription content filter successfully or + /// goal id was not found in feedback subscription content filter or + /// if the used rmw middleware doesn't support Content Filtering feature. + /// \return false if an error occurred during calling rcl function. + bool + configure_feedback_subscription_filter_remove_goal_id(const GoalUUID & goal_id); + /// \internal virtual std::shared_ptr @@ -322,6 +339,15 @@ class ClientBase : public rclcpp::Waitable // Storage for std::function callbacks to keep them in scope std::unordered_map> entity_type_to_on_ready_callback_; + // Enable feedback subscription content filter + // Initialization is configured by the user. + // When an error occurs while adding or removing a goal ID from the content filter, it will + // automatically be set to false. + std::atomic_bool enable_feedback_msg_optimization_; + // Mutex to protect feedback subscription content filter configuration because the related rcl + // function is not thread-safe. + std::mutex configure_feedback_sub_content_filter_mutex_; + private: std::unique_ptr pimpl_; diff --git a/rclcpp_action/include/rclcpp_action/create_client.hpp b/rclcpp_action/include/rclcpp_action/create_client.hpp index f594bca78d..b3cea43b00 100644 --- a/rclcpp_action/include/rclcpp_action/create_client.hpp +++ b/rclcpp_action/include/rclcpp_action/create_client.hpp @@ -30,6 +30,9 @@ namespace rclcpp_action * This function is equivalent to \sa create_client()` however is using the individual * node interfaces to create the client. * + * If enable_feedback_msg_optimization is set to true, an action client can handle up to 6 goals + * simultaneously. If the number of goals exceeds the limit, optimization is automatically disabled. + * * \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. @@ -38,6 +41,8 @@ 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`. + * \param[in] enable_feedback_msg_optimization Enable feedback subscription content filter to + * optimize the handling of feedback messages. */ template typename Client::SharedPtr @@ -48,7 +53,8 @@ create_client( rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_interface, const std::string & name, rclcpp::CallbackGroup::SharedPtr group = nullptr, - const rcl_action_client_options_t & options = rcl_action_client_get_default_options()) + const rcl_action_client_options_t & options = rcl_action_client_get_default_options(), + bool enable_feedback_msg_optimization = false) { std::weak_ptr weak_node = node_waitables_interface; @@ -85,7 +91,8 @@ create_client( node_graph_interface, node_logging_interface, name, - options), + options, + enable_feedback_msg_optimization), deleter); node_waitables_interface->add_waitable(action_client, group); @@ -94,11 +101,17 @@ create_client( /// Create an action client. /** + * + * If enable_feedback_msg_optimization is set to true, an action client can handle up to 6 goals + * simultaneously. If the number of goals exceeds the limit, optimization is automatically disabled. + * * \param[in] node The action client will be added to this node. * \param[in] name The action name. * \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`. + * \param[in] enable_feedback_msg_optimization Enable feedback subscription content filter to + * optimize the handling of feedback messages. */ template typename Client::SharedPtr @@ -106,7 +119,8 @@ create_client( NodeT node, const std::string & name, rclcpp::CallbackGroup::SharedPtr group = nullptr, - const rcl_action_client_options_t & options = rcl_action_client_get_default_options()) + const rcl_action_client_options_t & options = rcl_action_client_get_default_options(), + bool enable_feedback_msg_optimization = false) { return rclcpp_action::create_client( node->get_node_base_interface(), @@ -115,7 +129,8 @@ create_client( node->get_node_waitables_interface(), name, group, - options); + options, + enable_feedback_msg_optimization); } } // namespace rclcpp_action diff --git a/rclcpp_action/include/rclcpp_action/create_generic_client.hpp b/rclcpp_action/include/rclcpp_action/create_generic_client.hpp index c57f0bf004..058461c7ee 100644 --- a/rclcpp_action/include/rclcpp_action/create_generic_client.hpp +++ b/rclcpp_action/include/rclcpp_action/create_generic_client.hpp @@ -26,6 +26,9 @@ namespace rclcpp_action * This function is equivalent to \sa create_generic_client()` however is using the individual * node interfaces to create the client. * + * If enable_feedback_msg_optimization is set to true, an action client can handle up to 6 goals + * simultaneously. If the number of goals exceeds the limit, optimization is automatically disabled. + * * \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. @@ -35,6 +38,8 @@ 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`. + * \param[in] enable_feedback_msg_optimization Enable feedback subscription content filter to + * optimize the handling of feedback messages. * \return newly created generic client. */ RCLCPP_ACTION_PUBLIC @@ -47,16 +52,23 @@ create_generic_client( 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()); + const rcl_action_client_options_t & options = rcl_action_client_get_default_options(), + bool enable_feedback_msg_optimization = false); /// Create an action generic client. /** + * + * If enable_feedback_msg_optimization is set to true, an action client can handle up to 6 goals + * simultaneously. If the number of goals exceeds the limit, optimization is automatically disabled. + * * \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`. + * \param[in] enable_feedback_msg_optimization Enable feedback subscription content filter to + * optimize the handling of feedback messages. * \return newly created generic client. */ template @@ -66,7 +78,8 @@ create_generic_client( 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()) + const rcl_action_client_options_t & options = rcl_action_client_get_default_options(), + bool enable_feedback_msg_optimization = false) { return rclcpp_action::create_generic_client( node->get_node_base_interface(), @@ -76,7 +89,8 @@ create_generic_client( name, type, group, - options); + options, + enable_feedback_msg_optimization); } } // namespace rclcpp_action diff --git a/rclcpp_action/include/rclcpp_action/generic_client.hpp b/rclcpp_action/include/rclcpp_action/generic_client.hpp index a8e19cfc52..e4eafdf307 100644 --- a/rclcpp_action/include/rclcpp_action/generic_client.hpp +++ b/rclcpp_action/include/rclcpp_action/generic_client.hpp @@ -95,6 +95,10 @@ class GenericClient : public ClientBase * node. * Use `rclcpp_action::create_generic_client()` to both construct and add to a node. * + * If enable_feedback_msg_optimization is set to true, an action client can handle up to 6 goals + * simultaneously. If the number of goals exceeds the limit, optimization is automatically + * disabled. + * * \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. @@ -103,6 +107,8 @@ class GenericClient : public ClientBase * \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`. + * \param[in] enable_feedback_msg_optimization Enable feedback subscription content filter to + * optimize the handling of feedback messages. */ RCLCPP_ACTION_PUBLIC GenericClient( @@ -112,7 +118,8 @@ class GenericClient : public ClientBase 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()); + const rcl_action_client_options_t & client_options = rcl_action_client_get_default_options(), + bool enable_feedback_msg_optimization = false); /// Send an action goal and asynchronously get the result. /** diff --git a/rclcpp_action/src/client_base.cpp b/rclcpp_action/src/client_base.cpp index 5b29dac91a..c0195e0913 100644 --- a/rclcpp_action/src/client_base.cpp +++ b/rclcpp_action/src/client_base.cpp @@ -190,8 +190,10 @@ ClientBase::ClientBase( rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, const std::string & action_name, const rosidl_action_type_support_t * type_support, - const rcl_action_client_options_t & client_options) -: pimpl_(new ClientBaseImpl( + const rcl_action_client_options_t & client_options, + bool enable_feedback_msg_optimization) +: enable_feedback_msg_optimization_(enable_feedback_msg_optimization), + pimpl_(new ClientBaseImpl( node_base, node_graph, node_logging, action_name, type_support, client_options)) { } @@ -830,4 +832,23 @@ ClientBase::configure_introspection( } } +bool +ClientBase::configure_feedback_subscription_filter_add_goal_id( + const GoalUUID & goal_id) +{ + const rcl_ret_t ret = rcl_action_client_configure_feedback_subscription_filter_add_goal_id( + pimpl_->client_handle.get(), goal_id.data(), goal_id.size()); + + return ret == RCL_RET_OK; +} + +bool +ClientBase::configure_feedback_subscription_filter_remove_goal_id( + const GoalUUID & goal_id) +{ + const rcl_ret_t ret = rcl_action_client_configure_feedback_subscription_filter_remove_goal_id( + pimpl_->client_handle.get(), goal_id.data(), goal_id.size()); + + return ret == RCL_RET_OK; +} } // namespace rclcpp_action diff --git a/rclcpp_action/src/create_generic_client.cpp b/rclcpp_action/src/create_generic_client.cpp index 7699a736ef..eb42fc5445 100644 --- a/rclcpp_action/src/create_generic_client.cpp +++ b/rclcpp_action/src/create_generic_client.cpp @@ -32,7 +32,8 @@ create_generic_client( const std::string & name, const std::string & type, rclcpp::CallbackGroup::SharedPtr group, - const rcl_action_client_options_t & options) + const rcl_action_client_options_t & options, + bool enable_feedback_msg_optimization) { std::weak_ptr weak_node = node_waitables_interface; @@ -75,7 +76,8 @@ create_generic_client( name, typesupport_lib, action_typesupport_handle, - options), + options, + enable_feedback_msg_optimization), deleter); node_waitables_interface->add_waitable(action_client, group); diff --git a/rclcpp_action/src/generic_client.cpp b/rclcpp_action/src/generic_client.cpp index e3a5a3adc2..6400365ca3 100644 --- a/rclcpp_action/src/generic_client.cpp +++ b/rclcpp_action/src/generic_client.cpp @@ -32,11 +32,13 @@ GenericClient::GenericClient( 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) + const rcl_action_client_options_t & client_options, + bool enable_feedback_msg_optimization) : ClientBase( node_base, node_graph, node_logging, action_name, action_typesupport_handle, - client_options), + client_options, + enable_feedback_msg_optimization), ts_lib_(std::move(typesupport_lib)) { auto goal_service_request_type_support_intro = get_message_typesupport_handle( @@ -143,6 +145,19 @@ GenericClient::async_send_goal( return; } + // If feedback message optimization is enabled, add goal id to feedback subscription + // content filter + if (enable_feedback_msg_optimization_) { + std::lock_guard lock(configure_feedback_sub_content_filter_mutex_); + if (!configure_feedback_subscription_filter_add_goal_id(uuid)) { + RCLCPP_ERROR( + this->get_logger(), + "Failed to add goal id to feedback subscription content filter for action " + "generic client. Disable feedback message optimization."); + enable_feedback_msg_optimization_ = false; + } + } + action_msgs::msg::GoalInfo goal_info; goal_info.goal_id.uuid = uuid; std::memcpy( @@ -450,6 +465,22 @@ GenericClient::make_result_aware(typename GenericClientGoalHandle::SharedPtr goa goal_handle->set_result(wrapped_result); + // If feedback message optimization is enabled, remove goal id from feedback subscription + // content filter + if (this->enable_feedback_msg_optimization_) { + std::lock_guard lock( + this->configure_feedback_sub_content_filter_mutex_); + if (!this->configure_feedback_subscription_filter_remove_goal_id( + goal_handle->get_goal_id())) + { + RCLCPP_ERROR( + this->get_logger(), + "Failed to remove goal id from feedback subscription content filter for action " + "generic client. Disable feedback message optimization."); + this->enable_feedback_msg_optimization_ = false; + } + } + std::lock_guard lock(goal_handles_mutex_); goal_handles_.erase(goal_handle->get_goal_id()); }); @@ -469,9 +500,30 @@ GenericClient::async_cancel( 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 + [this, cancel_callback, promise](std::shared_ptr response) mutable { auto cancel_response = std::static_pointer_cast(response); + + // If feedback message optimization is enabled, remove goal id from feedback subscription + // content filter + if (this->enable_feedback_msg_optimization_) { + for (const auto & goal_info : cancel_response->goals_canceling) { + std::lock_guard lock(this->configure_feedback_sub_content_filter_mutex_); + if (!this->configure_feedback_subscription_filter_remove_goal_id( + goal_info.goal_id.uuid)) + { + RCLCPP_ERROR( + this->get_logger(), + "Failed to remove goal id from feedback subscription content filter for action " + "generic client. Disable feedback message optimization."); + this->enable_feedback_msg_optimization_ = false; + // When an error occurs, the rcl layer will disable the content filter, so there is + // no need to continue removing the goal id. + break; + } + } + } + promise->set_value(cancel_response); if (cancel_callback) { cancel_callback(cancel_response); diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index c3a505ed5d..fedca44342 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include @@ -28,6 +29,7 @@ #include "rcl_action/names.h" #include "rcl_action/default_qos.h" +#include "rcl_action/action_client.h" #include "rcl_action/wait.h" #include "rclcpp/clock.hpp" @@ -123,6 +125,10 @@ class TestClient : public ::testing::Test status_message.status_list.push_back(goal_status); status_publisher->publish(status_message); client_executor.spin_once(); + while (pending_handle_goal_) { + // wait until allowed to proceed + std::this_thread::sleep_for(200ms); + } ActionFeedbackMessage feedback_message; feedback_message.goal_id.uuid = goal_request->goal_id.uuid; feedback_message.feedback.sequence.push_back(0); @@ -246,6 +252,40 @@ class TestClient : public ::testing::Test client_node.reset(); } + void enable_pending_handling_goal() + { + pending_handle_goal_ = true; + } + + void disable_pending_handling_goal() + { + pending_handle_goal_ = false; + } + + class GoalFilteringClient : public rclcpp_action::Client + { +public: + using rclcpp_action::Client::Client; + + bool add_goal_id_to_configure_filter(const rclcpp_action::GoalUUID & uuid) + { + return this->configure_feedback_subscription_filter_add_goal_id(uuid); + } + bool remove_goal_id_from_configure_filter(const rclcpp_action::GoalUUID & uuid) + { + return this->configure_feedback_subscription_filter_remove_goal_id(uuid); + } + }; + + std::shared_ptr make_goal_filtering_client() + { + return std::make_shared( + client_node->get_node_base_interface(), + client_node->get_node_graph_interface(), + client_node->get_node_logging_interface(), + action_name); + } + template void dual_spin_until_future_complete(std::shared_future & future) { @@ -279,6 +319,7 @@ class TestClient : public ::testing::Test typename rclcpp::Service::SharedPtr cancel_service; typename rclcpp::Publisher::SharedPtr feedback_publisher; typename rclcpp::Publisher::SharedPtr status_publisher; + std::atomic_bool pending_handle_goal_{false}; }; class TestClientAgainstServer : public TestClient @@ -286,6 +327,7 @@ class TestClientAgainstServer : public TestClient protected: void SetUp() override { + disable_pending_handling_goal(); SetUpServer(); TestClient::SetUp(); } @@ -325,7 +367,8 @@ TEST_F(TestClient, construction_and_destruction_callback_group) client_node->get_node_waitables_interface(), action_name, group, - options + options, + false ).reset()); } @@ -389,6 +432,119 @@ TEST_F(TestClient, wait_for_action_server_rcl_errors) TearDownServer(); } +TEST_F(TestClient, check_configure_feedback_subscription_filter_add_goal_id) +{ + auto action_client = make_goal_filtering_client(); + rclcpp_action::GoalUUID uuid; + uuid.fill(0u); + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", + rcl_action_client_configure_feedback_subscription_filter_add_goal_id, + RCL_RET_OK); + + EXPECT_TRUE(action_client->add_goal_id_to_configure_filter(uuid)); + } + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", + rcl_action_client_configure_feedback_subscription_filter_add_goal_id, + RCL_RET_UNSUPPORTED); + + EXPECT_FALSE(action_client->add_goal_id_to_configure_filter(uuid)); + } + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", + rcl_action_client_configure_feedback_subscription_filter_add_goal_id, + RCL_RET_ERROR); + + EXPECT_FALSE(action_client->add_goal_id_to_configure_filter(uuid)); + } + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", + rcl_action_client_configure_feedback_subscription_filter_add_goal_id, + RCL_RET_ACTION_CLIENT_INVALID); + + EXPECT_FALSE(action_client->add_goal_id_to_configure_filter(uuid)); + } + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", + rcl_action_client_configure_feedback_subscription_filter_add_goal_id, + RCL_RET_BAD_ALLOC); + + EXPECT_FALSE(action_client->add_goal_id_to_configure_filter(uuid)); + } + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", + rcl_action_client_configure_feedback_subscription_filter_add_goal_id, + RCL_RET_INVALID_ARGUMENT); + + EXPECT_FALSE(action_client->add_goal_id_to_configure_filter(uuid)); + } +} + +TEST_F(TestClient, check_configure_feedback_subscription_filter_remove_goal_id) +{ + auto action_client = make_goal_filtering_client(); + rclcpp_action::GoalUUID uuid; + uuid.fill(0u); + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", + rcl_action_client_configure_feedback_subscription_filter_remove_goal_id, + RCL_RET_OK); + + EXPECT_TRUE(action_client->remove_goal_id_from_configure_filter(uuid)); + } + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", + rcl_action_client_configure_feedback_subscription_filter_remove_goal_id, + RCL_RET_ERROR); + + EXPECT_FALSE(action_client->remove_goal_id_from_configure_filter(uuid)); + } + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", + rcl_action_client_configure_feedback_subscription_filter_remove_goal_id, + RCL_RET_ACTION_CLIENT_INVALID); + + EXPECT_FALSE(action_client->remove_goal_id_from_configure_filter(uuid)); + } + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", + rcl_action_client_configure_feedback_subscription_filter_remove_goal_id, + RCL_RET_BAD_ALLOC); + + EXPECT_FALSE(action_client->remove_goal_id_from_configure_filter(uuid)); + } + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", + rcl_action_client_configure_feedback_subscription_filter_remove_goal_id, + RCL_RET_INVALID_ARGUMENT); + + EXPECT_FALSE(action_client->remove_goal_id_from_configure_filter(uuid)); + } +} + TEST_F(TestClient, is_ready) { auto action_client = rclcpp_action::create_client(client_node, action_name); rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); @@ -1052,3 +1208,111 @@ TEST_F(TestClientAgainstServer, test_configure_introspection) // No method was found to make rcl_action_client_configure_action_introspection return // a value other than RCL_RET_OK. mocking_utils::patch_and_return does not work for this function. } + +TEST_F(TestClientAgainstServer, + enable_feedback_msg_optimization_does_not_affect_normal_feedback_reception) +{ + // Test that when the feedback message optimization is enabled, the goal's feedback messages + // are received without any issues. + const rcl_action_client_options_t options = rcl_action_client_get_default_options(); + auto action_client = + rclcpp_action::create_client(client_node, action_name, nullptr, options, true); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + enable_pending_handling_goal(); + + std::array orders{4, 5}; + std::array feedback_counts{}; + std::array, 2> goal_handle_futures{}; + + for (size_t i = 0; i < orders.size(); ++i) { + ActionGoal goal; + goal.order = orders[i]; + auto send_goal_ops = rclcpp_action::Client::SendGoalOptions(); + send_goal_ops.feedback_callback = [&count = feedback_counts[i]]( + [[maybe_unused]] typename ActionGoalHandle::SharedPtr goal_handle, + [[maybe_unused]] const std::shared_ptr feedback) + { + count++; + }; + goal_handle_futures[i] = action_client->async_send_goal(goal, send_goal_ops); + } + + server_executor.spin_some(500ms); + disable_pending_handling_goal(); + + for (auto & future_goal_handle : goal_handle_futures) { + dual_spin_until_future_complete(future_goal_handle); + } + + for (size_t i = 0; i < orders.size(); ++i) { + auto goal_handle = goal_handle_futures[i].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_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(); + + EXPECT_EQ(orders[i] + 1, feedback_counts[i]); + EXPECT_EQ(orders[i] + 1, wrapped_result.result->sequence.size()); + } +} + +TEST_F(TestClientAgainstServer, + enable_feedback_msg_optimization_handles_multiple_goals) +{ + // When testing with more than 6 goals running at the same time, the internal feedback + // subscription content filter will automatically turn off, but this does not affect the + // reception of feedback messages for all goals. + const rcl_action_client_options_t options = rcl_action_client_get_default_options(); + auto action_client = + rclcpp_action::create_client(client_node, action_name, nullptr, options, true); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + enable_pending_handling_goal(); + + constexpr size_t goal_count = 7u; + std::array goal_msgs; + std::array feedback_counts{}; + std::array, goal_count> + goal_handle_futures{}; + + for (size_t i = 0; i < goal_count; ++i) { + goal_msgs[i].order = static_cast(i + 1); + auto send_goal_ops = rclcpp_action::Client::SendGoalOptions(); + send_goal_ops.feedback_callback = [&count = feedback_counts[i]]( + [[maybe_unused]] typename ActionGoalHandle::SharedPtr goal_handle, + [[maybe_unused]] const std::shared_ptr feedback) + { + count++; + }; + goal_handle_futures[i] = action_client->async_send_goal(goal_msgs[i], send_goal_ops); + } + + server_executor.spin_some(500ms); + disable_pending_handling_goal(); + + std::array, goal_count> + result_futures{}; + + for (size_t i = 0; i < goal_count; ++i) { + dual_spin_until_future_complete(goal_handle_futures[i]); + auto goal_handle = goal_handle_futures[i].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()); + result_futures[i] = action_client->async_get_result(goal_handle); + EXPECT_TRUE(goal_handle->is_result_aware()); + } + + for (size_t i = 0; i < goal_count; ++i) { + dual_spin_until_future_complete(result_futures[i]); + auto wrapped_result = result_futures[i].get(); + + EXPECT_EQ(goal_msgs[i].order + 1, feedback_counts[i]); + EXPECT_EQ(goal_msgs[i].order + 1, wrapped_result.result->sequence.size()); + } +} diff --git a/rclcpp_action/test/test_generic_client.cpp b/rclcpp_action/test/test_generic_client.cpp index 571c51a923..a4d3d61c67 100644 --- a/rclcpp_action/test/test_generic_client.cpp +++ b/rclcpp_action/test/test_generic_client.cpp @@ -13,12 +13,14 @@ // limitations under the License. #include +#include #include #include #include #include #include #include +#include #include "gtest/gtest.h" @@ -120,6 +122,10 @@ class TestGenericClient : public ::testing::Test status_message.status_list.push_back(goal_status); status_publisher->publish(status_message); client_executor.spin_once(); + while (pending_handle_goal_) { + // wait until allowed to proceed + std::this_thread::sleep_for(200ms); + } ActionFeedbackMessage feedback_message; feedback_message.goal_id.uuid = goal_request->goal_id.uuid; feedback_message.feedback.sequence.push_back(0); @@ -254,6 +260,16 @@ class TestGenericClient : public ::testing::Test } while (std::future_status::ready != status); } + void enable_pending_handling_goal() + { + pending_handle_goal_ = true; + } + + void disable_pending_handling_goal() + { + pending_handle_goal_ = false; + } + rclcpp::Clock clock{RCL_ROS_TIME}; const rclcpp::Time zero_stamp{0, 0, RCL_ROS_TIME}; @@ -276,6 +292,7 @@ class TestGenericClient : public ::testing::Test typename rclcpp::Service::SharedPtr cancel_service; typename rclcpp::Publisher::SharedPtr feedback_publisher; typename rclcpp::Publisher::SharedPtr status_publisher; + std::atomic_bool pending_handle_goal_{false}; }; TEST_F(TestGenericClient, construction_with_free_function) { @@ -431,6 +448,7 @@ class TestGenericClientAgainstServer : public TestGenericClient protected: void SetUp() override { + disable_pending_handling_goal(); SetUpServer(); TestGenericClient::SetUp(); } @@ -1095,6 +1113,8 @@ TEST_F(TestGenericClientAgainstServer, send_rcl_errors) TEST_F(TestGenericClientAgainstServer, execute_rcl_errors) { + // Test that when the feedback message optimization is enabled, the goal's feedback messages + // are received without any issues. auto action_generic_client = rclcpp_action::create_generic_client( client_node, action_name, @@ -1167,3 +1187,148 @@ TEST_F(TestGenericClientAgainstServer, execute_rcl_errors) rclcpp::exceptions::RCLError); } } + +TEST_F(TestGenericClientAgainstServer, + enable_feedback_msg_optimization_does_not_affect_normal_feedback_reception) +{ + const rcl_action_client_options_t options = rcl_action_client_get_default_options(); + auto action_generic_client = + rclcpp_action::create_generic_client( + client_node, + action_name, + "test_msgs/action/Fibonacci", + nullptr, + options, + true); + ASSERT_TRUE(action_generic_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + enable_pending_handling_goal(); + + constexpr size_t goal_count = 2u; + std::array goal_msgs; + std::array feedback_counts{}; + + using FutureGoalHandle = + std::shared_future; + std::vector goal_handle_futures; + goal_handle_futures.reserve(goal_count); + + for (size_t i = 0; i < goal_count; ++i) { + goal_msgs[i].order = 4; + auto send_goal_ops = rclcpp_action::GenericClient::SendGoalOptions(); + send_goal_ops.feedback_callback = [&count = feedback_counts[i]]( + typename rclcpp_action::GenericClientGoalHandle::SharedPtr, const void *) + { + count++; + }; + goal_handle_futures.emplace_back( + action_generic_client->async_send_goal(&goal_msgs[i], sizeof(goal_msgs[i]), send_goal_ops)); + } + + server_executor.spin_some(500ms); + disable_pending_handling_goal(); + + std::vector goal_handles; + goal_handles.reserve(goal_count); + + for (auto & future_goal_handle : goal_handle_futures) { + 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()); + goal_handles.push_back(goal_handle); + } + + using FutureResult = std::shared_future; + std::vector result_futures; + result_futures.reserve(goal_count); + + for (auto & goal_handle : goal_handles) { + result_futures.emplace_back(action_generic_client->async_get_result(goal_handle)); + EXPECT_TRUE(goal_handle->is_result_aware()); + } + + for (size_t i = 0; i < goal_count; ++i) { + dual_spin_until_future_complete(result_futures[i]); + auto wrapped_result = result_futures[i].get(); + const ActionResult * result = (const ActionResult *)(wrapped_result.result); + + EXPECT_EQ(goal_msgs[i].order + 1, feedback_counts[i]); + EXPECT_EQ(goal_msgs[i].order + 1, result->sequence.size()); + } +} + +TEST_F(TestGenericClientAgainstServer, + enable_feedback_msg_optimization_handles_multiple_goals) +{ + // When testing with more than 6 goals running at the same time, the internal feedback + // subscription content filter will automatically turn off, but this does not affect the + // reception of feedback messages for all goals. + const rcl_action_client_options_t options = rcl_action_client_get_default_options(); + auto action_generic_client = + rclcpp_action::create_generic_client( + client_node, + action_name, + "test_msgs/action/Fibonacci", + nullptr, + options, + true); + ASSERT_TRUE(action_generic_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + enable_pending_handling_goal(); + + constexpr size_t goal_count = 7u; + std::array goal_msgs; + std::array feedback_counts{}; + + using FutureGoalHandle = + std::shared_future; + std::vector goal_handle_futures; + goal_handle_futures.reserve(goal_count); + + for (size_t i = 0; i < goal_count; ++i) { + goal_msgs[i].order = static_cast(i + 1); + auto send_goal_ops = rclcpp_action::GenericClient::SendGoalOptions(); + send_goal_ops.feedback_callback = [&count = feedback_counts[i]]( + typename rclcpp_action::GenericClientGoalHandle::SharedPtr, const void *) + { + count++; + }; + goal_handle_futures.emplace_back( + action_generic_client->async_send_goal(&goal_msgs[i], sizeof(goal_msgs[i]), send_goal_ops)); + } + + server_executor.spin_some(500ms); + disable_pending_handling_goal(); + + std::vector goal_handles; + goal_handles.reserve(goal_count); + + for (auto & future_goal_handle : goal_handle_futures) { + 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()); + goal_handles.push_back(goal_handle); + } + + using FutureResult = std::shared_future; + std::vector result_futures; + result_futures.reserve(goal_count); + + for (auto & goal_handle : goal_handles) { + result_futures.emplace_back(action_generic_client->async_get_result(goal_handle)); + EXPECT_TRUE(goal_handle->is_result_aware()); + } + + for (size_t i = 0; i < goal_count; ++i) { + dual_spin_until_future_complete(result_futures[i]); + auto wrapped_result = result_futures[i].get(); + const ActionResult * result = (const ActionResult *)(wrapped_result.result); + + EXPECT_EQ(goal_msgs[i].order + 1, feedback_counts[i]); + EXPECT_EQ(goal_msgs[i].order + 1, result->sequence.size()); + } +}