add take_data_by_entity_id API to waitable (#1892)

* add take_data_by_entity_id API to waitable

Signed-off-by: Alberto Soragna <alberto.soragna@gmail.com>

* use size_t for entity id

Signed-off-by: William Woodall <william@osrfoundation.org>

Co-authored-by: William Woodall <william@osrfoundation.org>

Author: alsora

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<void>
   take_data() override = 0;
 
+  std::shared_ptr<void>
+  take_data_by_entity_id(size_t id) override
+  {
+    (void)id;
+    return take_data();
+  }
+
   void
   execute(std::shared_ptr<void> & data) override = 0;
 

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<void>(std::make_shared<EventCallbackInfoT>(callback_info));
   }
 
+  std::shared_ptr<void>
+  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<void> & data) override

rclcpp/include/rclcpp/waitable.hpp

@@ -160,6 +160,24 @@ class Waitable
   std::shared_ptr<void>
   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<void>
+  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,

rclcpp/src/rclcpp/waitable.cpp

@@ -54,6 +54,15 @@ Waitable::get_number_of_ready_guard_conditions()
   return 0u;
 }
 
+std::shared_ptr<void>
+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)
 {

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<void>
   take_data() override;
 
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  std::shared_ptr<void>
+  take_data_by_entity_id(size_t id) override;
+
   /// \internal
   RCLCPP_ACTION_PUBLIC
   void

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<void>
   take_data() override;
 
+  RCLCPP_ACTION_PUBLIC
+  std::shared_ptr<void>
+  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

rclcpp_action/src/client.cpp

@@ -589,6 +589,31 @@ ClientBase::take_data()
   }
 }
 
+std::shared_ptr<void>
+ClientBase::take_data_by_entity_id(size_t id)
+{
+  // Mark as ready the entity from which we want to take data
+  switch (static_cast<EntityType>(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<void> & data)
 {

rclcpp_action/src/server.cpp

@@ -268,6 +268,25 @@ ServerBase::take_data()
   }
 }
 
+std::shared_ptr<void>
+ServerBase::take_data_by_entity_id(size_t id)
+{
+  // Mark as ready the entity from which we want to take data
+  switch (static_cast<EntityType>(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<void> & data)
 {
@@ -398,6 +417,7 @@ ServerBase::execute_cancel_request_received(std::shared_ptr<void> & 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();