diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index 331c1b6da5..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, }; @@ -68,6 +68,13 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable std::shared_ptr take_data() override = 0; + std::shared_ptr + take_data_by_entity_id(size_t 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..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, }; @@ -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(size_t 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..1a0d8b61f1 100644 --- a/rclcpp/include/rclcpp/waitable.hpp +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -160,6 +160,24 @@ 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. + * + * \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(size_t 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..6c1284cb22 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(size_t 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..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, @@ -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(size_t 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..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, @@ -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(size_t 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..f9144ecf49 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(size_t 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..b0afb9aa50 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(size_t 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();