From d9bbb1b7024b35d742cc3f2610e539e248883939 Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Thu, 24 Feb 2022 16:45:24 +0000 Subject: [PATCH 1/2] add take_data_by_entity_id API to waitable Signed-off-by: Alberto Soragna --- .../subscription_intra_process_base.hpp | 7 ++++++ rclcpp/include/rclcpp/qos_event.hpp | 7 ++++++ rclcpp/include/rclcpp/waitable.hpp | 16 ++++++++++++ rclcpp/src/rclcpp/waitable.cpp | 9 +++++++ .../include/rclcpp_action/client.hpp | 5 ++++ .../include/rclcpp_action/server.hpp | 4 +++ rclcpp_action/src/client.cpp | 25 +++++++++++++++++++ rclcpp_action/src/server.cpp | 20 +++++++++++++++ 8 files changed, 93 insertions(+) diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index 331c1b6da5..427e270d17 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -68,6 +68,13 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable std::shared_ptr take_data() override = 0; + std::shared_ptr + take_data_by_entity_id(int id) override + { + (void)id; + return take_data(); + } + void execute(std::shared_ptr & data) override = 0; diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index 59c99dda42..04948a0d29 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -259,6 +259,13 @@ class QOSEventHandler : public QOSEventHandlerBase return std::static_pointer_cast(std::make_shared(callback_info)); } + std::shared_ptr + take_data_by_entity_id(int id) override + { + (void)id; + return take_data(); + } + /// Execute any entities of the Waitable that are ready. void execute(std::shared_ptr & data) override diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp index db06f70678..02e53e1a7f 100644 --- a/rclcpp/include/rclcpp/waitable.hpp +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -160,6 +160,22 @@ class Waitable std::shared_ptr take_data() = 0; + /// Take the data so that it can be consumed with `execute`. + /** + * This function allows to specify an entity ID to take the data from. + * Entity IDs are identifiers that can be defined by waitable-derived + * classes that are composed of several distinct entities. + * The main use-case is in conjunction with the listener APIs. + * + * \sa rclcpp::Waitable::take_data + * \sa rclcpp::Waitable::set_on_ready_callback + * + */ + RCLCPP_PUBLIC + virtual + std::shared_ptr + take_data_by_entity_id(int id); + /// Execute data that is passed in. /** * Before calling this method, the Waitable should be added to a wait set, diff --git a/rclcpp/src/rclcpp/waitable.cpp b/rclcpp/src/rclcpp/waitable.cpp index 16ebb1b546..d9f9833378 100644 --- a/rclcpp/src/rclcpp/waitable.cpp +++ b/rclcpp/src/rclcpp/waitable.cpp @@ -54,6 +54,15 @@ Waitable::get_number_of_ready_guard_conditions() return 0u; } +std::shared_ptr +Waitable::take_data_by_entity_id(int id) +{ + (void)id; + throw std::runtime_error( + "Custom waitables should override take_data_by_entity_id " + "if they want to use it."); +} + bool Waitable::exchange_in_use_by_wait_set_state(bool in_use_state) { diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 9bcddadec0..9a10c79f60 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -134,6 +134,11 @@ class ClientBase : public rclcpp::Waitable std::shared_ptr take_data() override; + /// \internal + RCLCPP_ACTION_PUBLIC + std::shared_ptr + take_data_by_entity_id(int id) override; + /// \internal RCLCPP_ACTION_PUBLIC void diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 554cb1cf56..18535b13d4 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -131,6 +131,10 @@ class ServerBase : public rclcpp::Waitable std::shared_ptr take_data() override; + RCLCPP_ACTION_PUBLIC + std::shared_ptr + take_data_by_entity_id(int id) override; + /// Act on entities in the wait set which are ready to be acted upon. /// \internal RCLCPP_ACTION_PUBLIC diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index 07982092d5..ff17d996a1 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -589,6 +589,31 @@ ClientBase::take_data() } } +std::shared_ptr +ClientBase::take_data_by_entity_id(int id) +{ + // Mark as ready the entity from which we want to take data + switch (static_cast(id)) { + case EntityType::GoalClient: + pimpl_->is_goal_response_ready = true; + break; + case EntityType::ResultClient: + pimpl_->is_result_response_ready = true; + break; + case EntityType::CancelClient: + pimpl_->is_cancel_response_ready = true; + break; + case EntityType::FeedbackSubscription: + pimpl_->is_feedback_ready = true; + break; + case EntityType::StatusSubscription: + pimpl_->is_status_ready = true; + break; + } + + return take_data(); +} + void ClientBase::execute(std::shared_ptr & data) { diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index a07e203e28..d1b7097911 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -268,6 +268,25 @@ ServerBase::take_data() } } +std::shared_ptr +ServerBase::take_data_by_entity_id(int id) +{ + // Mark as ready the entity from which we want to take data + switch (static_cast(id)) { + case EntityType::GoalService: + pimpl_->goal_request_ready_ = true; + break; + case EntityType::ResultService: + pimpl_->result_request_ready_ = true; + break; + case EntityType::CancelService: + pimpl_->cancel_request_ready_ = true; + break; + } + + return take_data(); +} + void ServerBase::execute(std::shared_ptr & data) { @@ -398,6 +417,7 @@ ServerBase::execute_cancel_request_received(std::shared_ptr & data) } auto request = std::get<1>(*shared_ptr); auto request_header = std::get<2>(*shared_ptr); + pimpl_->cancel_request_ready_ = false; // Convert c++ message to C message rcl_action_cancel_request_t cancel_request = rcl_action_get_zero_initialized_cancel_request(); From 2f3695febe5580b9a58219d8f15468a04d585e4c Mon Sep 17 00:00:00 2001 From: William Woodall Date: Mon, 4 Apr 2022 16:34:45 -0700 Subject: [PATCH 2/2] use size_t for entity id Signed-off-by: William Woodall --- .../rclcpp/experimental/subscription_intra_process_base.hpp | 4 ++-- rclcpp/include/rclcpp/qos_event.hpp | 4 ++-- rclcpp/include/rclcpp/waitable.hpp | 6 ++++-- rclcpp/src/rclcpp/waitable.cpp | 2 +- rclcpp_action/include/rclcpp_action/client.hpp | 4 ++-- rclcpp_action/include/rclcpp_action/server.hpp | 4 ++-- rclcpp_action/src/client.cpp | 2 +- rclcpp_action/src/server.cpp | 2 +- 8 files changed, 15 insertions(+), 13 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index 427e270d17..037d1aaa2a 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -38,7 +38,7 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable public: RCLCPP_SMART_PTR_ALIASES_ONLY(SubscriptionIntraProcessBase) - enum class EntityType + enum class EntityType : std::size_t { Subscription, }; @@ -69,7 +69,7 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable take_data() override = 0; std::shared_ptr - take_data_by_entity_id(int id) override + take_data_by_entity_id(size_t id) override { (void)id; return take_data(); diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index 04948a0d29..f258c5d5ba 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -89,7 +89,7 @@ class UnsupportedEventTypeException : public exceptions::RCLErrorBase, public st class QOSEventHandlerBase : public Waitable { public: - enum class EntityType + enum class EntityType : std::size_t { Event, }; @@ -260,7 +260,7 @@ class QOSEventHandler : public QOSEventHandlerBase } std::shared_ptr - take_data_by_entity_id(int id) override + take_data_by_entity_id(size_t id) override { (void)id; return take_data(); diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp index 02e53e1a7f..1a0d8b61f1 100644 --- a/rclcpp/include/rclcpp/waitable.hpp +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -167,14 +167,16 @@ class Waitable * classes that are composed of several distinct entities. * The main use-case is in conjunction with the listener APIs. * + * \param[in] id the id of the entity from which to take + * \returns the type-erased data taken from entity specified + * * \sa rclcpp::Waitable::take_data * \sa rclcpp::Waitable::set_on_ready_callback - * */ RCLCPP_PUBLIC virtual std::shared_ptr - take_data_by_entity_id(int id); + take_data_by_entity_id(size_t id); /// Execute data that is passed in. /** diff --git a/rclcpp/src/rclcpp/waitable.cpp b/rclcpp/src/rclcpp/waitable.cpp index d9f9833378..6c1284cb22 100644 --- a/rclcpp/src/rclcpp/waitable.cpp +++ b/rclcpp/src/rclcpp/waitable.cpp @@ -55,7 +55,7 @@ Waitable::get_number_of_ready_guard_conditions() } std::shared_ptr -Waitable::take_data_by_entity_id(int id) +Waitable::take_data_by_entity_id(size_t id) { (void)id; throw std::runtime_error( diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 9a10c79f60..5eac5419f1 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -66,7 +66,7 @@ class ClientBase : public rclcpp::Waitable virtual ~ClientBase(); /// Enum to identify entities belonging to the action client - enum class EntityType + enum class EntityType : std::size_t { GoalClient, ResultClient, @@ -137,7 +137,7 @@ class ClientBase : public rclcpp::Waitable /// \internal RCLCPP_ACTION_PUBLIC std::shared_ptr - take_data_by_entity_id(int id) override; + take_data_by_entity_id(size_t id) override; /// \internal RCLCPP_ACTION_PUBLIC diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 18535b13d4..892de5743b 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -72,7 +72,7 @@ class ServerBase : public rclcpp::Waitable { public: /// Enum to identify entities belonging to the action server - enum class EntityType + enum class EntityType : std::size_t { GoalService, ResultService, @@ -133,7 +133,7 @@ class ServerBase : public rclcpp::Waitable RCLCPP_ACTION_PUBLIC std::shared_ptr - take_data_by_entity_id(int id) override; + take_data_by_entity_id(size_t id) override; /// Act on entities in the wait set which are ready to be acted upon. /// \internal diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index ff17d996a1..f9144ecf49 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -590,7 +590,7 @@ ClientBase::take_data() } std::shared_ptr -ClientBase::take_data_by_entity_id(int id) +ClientBase::take_data_by_entity_id(size_t id) { // Mark as ready the entity from which we want to take data switch (static_cast(id)) { diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index d1b7097911..b0afb9aa50 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -269,7 +269,7 @@ ServerBase::take_data() } std::shared_ptr -ServerBase::take_data_by_entity_id(int id) +ServerBase::take_data_by_entity_id(size_t id) { // Mark as ready the entity from which we want to take data switch (static_cast(id)) {