Convert WaitSet to use C++ Classes

rclpy/rclpy/executors.py

@@ -507,7 +507,7 @@ def _wait_for_ready_callbacks(
                 len(subscriptions), len(guards), len(timers), len(clients), len(services))
 
             # Construct a wait set
-            wait_set = _rclpy.rclpy_get_zero_initialized_wait_set()
+            wait_set = None
             with ExitStack() as context_stack:
                 sub_handles = []
                 for sub in subscriptions:
@@ -556,8 +556,7 @@ def _wait_for_ready_callbacks(
                         pass
 
                 context_capsule = context_stack.enter_context(self._context.handle)
-                _rclpy.rclpy_wait_set_init(
-                    wait_set,
+                wait_set = _rclpy.WaitSet(
                     entity_count.num_subscriptions,
                     entity_count.num_guard_conditions,
                     entity_count.num_timers,
@@ -566,33 +565,33 @@ def _wait_for_ready_callbacks(
                     entity_count.num_events,
                     context_capsule)
 
-                _rclpy.rclpy_wait_set_clear_entities(wait_set)
+                wait_set.clear_entities()
                 for sub_handle in sub_handles:
-                    _rclpy.rclpy_wait_set_add_subscription(wait_set, sub_handle)
+                    wait_set.add_subscription(sub_handle)
                 for cli_handle in client_handles:
-                    _rclpy.rclpy_wait_set_add_client(wait_set, cli_handle)
+                    wait_set.add_client(cli_handle)
                 for srv_capsule in service_handles:
-                    _rclpy.rclpy_wait_set_add_service(wait_set, srv_capsule)
+                    wait_set.add_service(srv_capsule)
                 for tmr_handle in timer_handles:
-                    _rclpy.rclpy_wait_set_add_timer(wait_set, tmr_handle)
+                    wait_set.add_timer(tmr_handle)
                 for gc_capsule in guard_capsules:
-                    _rclpy.rclpy_wait_set_add_entity('guard_condition', wait_set, gc_capsule)
+                    wait_set.add_entity('guard_condition', gc_capsule)
                 for waitable in waitables:
                     waitable.add_to_wait_set(wait_set)
 
                 # Wait for something to become ready
-                _rclpy.rclpy_wait(wait_set, timeout_nsec)
+                wait_set.wait(timeout_nsec)
                 if self._is_shutdown:
                     raise ShutdownException()
                 if not self._context.ok():
                     raise ExternalShutdownException()
 
                 # get ready entities
-                subs_ready = _rclpy.rclpy_get_ready_entities('subscription', wait_set)
-                guards_ready = _rclpy.rclpy_get_ready_entities('guard_condition', wait_set)
-                timers_ready = _rclpy.rclpy_get_ready_entities('timer', wait_set)
-                clients_ready = _rclpy.rclpy_get_ready_entities('client', wait_set)
-                services_ready = _rclpy.rclpy_get_ready_entities('service', wait_set)
+                subs_ready = wait_set.get_ready_entities('subscription')
+                guards_ready = wait_set.get_ready_entities('guard_condition')
+                timers_ready = wait_set.get_ready_entities('timer')
+                clients_ready = wait_set.get_ready_entities('client')
+                services_ready = wait_set.get_ready_entities('service')
 
                 # Mark all guards as triggered before yielding since they're auto-taken
                 for gc in guards:

rclpy/rclpy/qos_event.py

@@ -87,7 +87,7 @@ def is_ready(self, wait_set):
         """Return True if entities are ready in the wait set."""
         if self._event_index is None:
             return False
-        if _rclpy.rclpy_wait_set_is_ready('event', wait_set, self._event_index):
+        if wait_set.is_ready('event', self._event_index):
             self._ready_to_take_data = True
         return self._ready_to_take_data
 
@@ -112,7 +112,7 @@ def get_num_entities(self):
     def add_to_wait_set(self, wait_set):
         """Add entites to wait set."""
         with self.__event:
-            self._event_index = _rclpy.rclpy_wait_set_add_event(wait_set, self.__event)
+            self._event_index = wait_set.add_event(self.__event)
 
     def __enter__(self):
         """Mark event as in-use to prevent destruction while waiting on it."""

rclpy/src/rclpy/_rclpy_pybind11.cpp

@@ -151,43 +151,7 @@ PYBIND11_MODULE(_rclpy_pybind11, m) {
   rclpy::define_subscription(m);
   rclpy::define_time_point(m);
   rclpy::define_clock(m);
-
-  m.def(
-    "rclpy_get_zero_initialized_wait_set", &rclpy::get_zero_initialized_wait_set,
-    "rclpy_get_zero_initialized_wait_set.");
-  m.def(
-    "rclpy_wait_set_init", &rclpy::wait_set_init,
-    "rclpy_wait_set_init.");
-  m.def(
-    "rclpy_wait_set_clear_entities", &rclpy::wait_set_clear_entities,
-    "rclpy_wait_set_clear_entities.");
-  m.def(
-    "rclpy_wait_set_add_entity", &rclpy::wait_set_add_entity,
-    "rclpy_wait_set_add_entity.");
-  m.def(
-    "rclpy_wait_set_add_client", &rclpy::wait_set_add_client,
-    "Add a client to the wait set.");
-  m.def(
-    "rclpy_wait_set_add_service", &rclpy::wait_set_add_service,
-    "Add a service to the wait set.");
-  m.def(
-    "rclpy_wait_set_add_subscription", &rclpy::wait_set_add_subscription,
-    "Add a subscription to the wait set.");
-  m.def(
-    "rclpy_wait_set_add_timer", &rclpy::wait_set_add_timer,
-    "Add a timer to the wait set.");
-  m.def(
-    "rclpy_wait_set_add_event", &rclpy::wait_set_add_event,
-    "Add an event to the wait set.");
-  m.def(
-    "rclpy_wait_set_is_ready", &rclpy::wait_set_is_ready,
-    "rclpy_wait_set_is_ready.");
-  m.def(
-    "rclpy_get_ready_entities", &rclpy::get_ready_entities,
-    "List non null entities in wait set.");
-  m.def(
-    "rclpy_wait", &rclpy::wait,
-    "rclpy_wait.");
+  rclpy::define_waitset(m);
 
   m.def(
     "rclpy_expand_topic_name", &rclpy::expand_topic_name,

rclpy/src/rclpy/action_client.cpp

@@ -234,30 +234,26 @@ ActionClient::is_action_server_available()
 }
 
 void
-ActionClient::add_to_waitset(py::capsule pywait_set)
+ActionClient::add_to_waitset(WaitSet & wait_set)
 {
-  auto wait_set = static_cast<rcl_wait_set_t *>(pywait_set);
-
   rcl_ret_t ret = rcl_action_wait_set_add_action_client(
-    wait_set, rcl_action_client_.get(), NULL, NULL);
+    wait_set.rcl_ptr(), rcl_action_client_.get(), NULL, NULL);
   if (RCL_RET_OK != ret) {
     std::string error_text{"Failed to add 'rcl_action_client_t' to wait set"};
     throw rclpy::RCLError(error_text);
   }
 }
 
 py::tuple
-ActionClient::is_ready(py::capsule pywait_set)
+ActionClient::is_ready(WaitSet & wait_set)
 {
-  auto wait_set = static_cast<rcl_wait_set_t *>(pywait_set);
-
   bool is_feedback_ready = false;
   bool is_status_ready = false;
   bool is_goal_response_ready = false;
   bool is_cancel_response_ready = false;
   bool is_result_response_ready = false;
   rcl_ret_t ret = rcl_action_client_wait_set_get_entities_ready(
-    wait_set,
+    wait_set.rcl_ptr(),
     rcl_action_client_.get(),
     &is_feedback_ready,
     &is_status_ready,

rclpy/src/rclpy/action_client.hpp

@@ -23,6 +23,7 @@
 
 #include "destroyable.hpp"
 #include "handle.hpp"
+#include "wait_set.hpp"
 
 namespace py = pybind11;
 
@@ -194,7 +195,7 @@ class ActionClient : public Destroyable, public std::enable_shared_from_this<Act
    * This must be called after waiting on the wait set.
    * Raises RuntimeError on failure.
    *
-   * \param[in] pywait_set Capsule pointing to the wait set structure.
+   * \param[in] wait_set Capsule pointing to the wait set structure.
    * \return A tuple of booleans representing the sub-entities ready:
    *       (is_feedback_ready,
    *        is_status_ready,
@@ -203,15 +204,15 @@ class ActionClient : public Destroyable, public std::enable_shared_from_this<Act
    *        is_result_response_ready)
    */
   py::tuple
-  is_ready(py::capsule pywait_set);
+  is_ready(WaitSet & wait_set);
 
   /// Add an action entitiy to a wait set.
   /**
    * Raises RuntimeError on failure.
-   * \param[in] pywait_set Capsule pointer to an rcl_wait_set_t.
+   * \param[in] wait_set Capsule pointer to an rcl_wait_set_t.
    */
   void
-  add_to_waitset(py::capsule pywait_set);
+  add_to_waitset(WaitSet & wait_set);
 
   /// Get rcl_client_t pointer
   rcl_action_client_t *

rclpy/src/rclpy/action_server.cpp

@@ -248,15 +248,14 @@ ActionServer::get_num_entities()
 }
 
 py::tuple
-ActionServer::is_ready(py::capsule pywait_set)
+ActionServer::is_ready(WaitSet & wait_set)
 {
-  auto wait_set = static_cast<rcl_wait_set_t *>(pywait_set);
   bool is_goal_request_ready = false;
   bool is_cancel_request_ready = false;
   bool is_result_request_ready = false;
   bool is_goal_expired = false;
   rcl_ret_t ret = rcl_action_server_wait_set_get_entities_ready(
-    wait_set,
+    wait_set.rcl_ptr(),
     rcl_action_server_.get(),
     &is_goal_request_ready,
     &is_cancel_request_ready,
@@ -276,10 +275,10 @@ ActionServer::is_ready(py::capsule pywait_set)
 }
 
 void
-ActionServer::add_to_waitset(py::capsule pywait_set)
+ActionServer::add_to_waitset(WaitSet & wait_set)
 {
-  auto wait_set = static_cast<rcl_wait_set_t *>(pywait_set);
-  rcl_ret_t ret = rcl_action_wait_set_add_action_server(wait_set, rcl_action_server_.get(), NULL);
+  rcl_ret_t ret = rcl_action_wait_set_add_action_server(
+    wait_set.rcl_ptr(), rcl_action_server_.get(), NULL);
   if (RCL_RET_OK != ret) {
     throw rclpy::RCLError("Failed to add 'rcl_action_server_t' to wait set");
   }

rclpy/src/rclpy/action_server.hpp

@@ -24,6 +24,7 @@
 #include "clock.hpp"
 #include "destroyable.hpp"
 #include "handle.hpp"
+#include "wait_set.hpp"
 
 namespace py = pybind11;
 
@@ -220,7 +221,7 @@ class ActionServer : public Destroyable, public std::enable_shared_from_this<Act
    *        is_goal_expired)
    */
   py::tuple
-  is_ready(py::capsule pywait_set);
+  is_ready(WaitSet & wait_set);
 
   /// Add an action entitiy to a wait set.
   /**
@@ -230,7 +231,7 @@ class ActionServer : public Destroyable, public std::enable_shared_from_this<Act
    * \param[in] pywait_set Capsule pointer to an rcl_wait_set_t.
    */
   void
-  add_to_waitset(py::capsule pywait_set);
+  add_to_waitset(WaitSet & wait_set);
 
   /// Get rcl_client_t pointer
   rcl_action_server_t *