Convert Guardcondition to use C++ classes

rclpy/rclpy/executors.py

@@ -541,10 +541,11 @@ def _wait_for_ready_callbacks(
                     except InvalidHandle:
                         entity_count.num_timers -= 1
 
-                guard_capsules = []
+                guard_handles = []
                 for gc in guards:
                     try:
-                        guard_capsules.append(context_stack.enter_context(gc.handle))
+                        context_stack.enter_context(gc.handle)
+                        guard_handles.append(gc.handle)
                     except InvalidHandle:
                         entity_count.num_guard_conditions -= 1
 
@@ -574,8 +575,8 @@ def _wait_for_ready_callbacks(
                     wait_set.add_service(srv_capsule)
                 for tmr_handle in timer_handles:
                     wait_set.add_timer(tmr_handle)
-                for gc_capsule in guard_capsules:
-                    wait_set.add_entity('guard_condition', gc_capsule)
+                for gc_handle in guard_handles:
+                    wait_set.add_guard_condition(gc_handle)
                 for waitable in waitables:
                     waitable.add_to_wait_set(wait_set)
 

rclpy/rclpy/guard_condition.py

@@ -12,7 +12,6 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-from rclpy.handle import Handle
 from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
 from rclpy.utilities import get_default_context
 
@@ -22,7 +21,7 @@ class GuardCondition:
     def __init__(self, callback, callback_group, context=None):
         self._context = get_default_context() if context is None else context
         with self._context.handle as capsule:
-            self.__handle = Handle(_rclpy.rclpy_create_guard_condition(capsule))
+            self.__gc = _rclpy.GuardCondition(capsule)
         self.callback = callback
         self.callback_group = callback_group
         # True when the callback is ready to fire but has not been "taken" by an executor
@@ -31,12 +30,12 @@ def __init__(self, callback, callback_group, context=None):
         self._executor_triggered = False
 
     def trigger(self):
-        with self.handle as capsule:
-            _rclpy.rclpy_trigger_guard_condition(capsule)
+        with self.__gc:
+            self.__gc.trigger_guard_condition()
 
     @property
     def handle(self):
-        return self.__handle
+        return self.__gc
 
     def destroy(self):
-        self.handle.destroy()
+        self.handle.destroy_when_not_in_use()

rclpy/rclpy/signals.py

@@ -21,8 +21,9 @@ class SignalHandlerGuardCondition(GuardCondition):
 
     def __init__(self, context=None):
         super().__init__(callback=None, callback_group=None, context=context)
-        with self.handle as capsule:
-            _signals.rclpy_register_sigint_guard_condition(capsule)
+        with self.handle:
+            # TODO(ahcorde): Remove the pycapsule method when #728 is in
+            _signals.rclpy_register_sigint_guard_condition(self.handle.pycapsule())
 
     def __del__(self):
         try:
@@ -35,6 +36,7 @@ def __del__(self):
             pass
 
     def destroy(self):
-        with self.handle as capsule:
-            _signals.rclpy_unregister_sigint_guard_condition(capsule)
+        with self.handle:
+            # TODO(ahcorde): Remove the pycapsule method when #728 is in
+            _signals.rclpy_unregister_sigint_guard_condition(self.handle.pycapsule())
         super().destroy()

rclpy/src/rclpy/_rclpy_pybind11.cpp

@@ -139,15 +139,8 @@ PYBIND11_MODULE(_rclpy_pybind11, m) {
     "rclpy_qos_check_compatible", &rclpy::qos_check_compatible,
     "Check if two QoS profiles are compatible.");
 
-  m.def(
-    "rclpy_create_guard_condition", &rclpy::guard_condition_create,
-    "Create a general purpose guard condition");
-  m.def(
-    "rclpy_trigger_guard_condition", &rclpy::guard_condition_trigger,
-    "Trigger a general purpose guard condition");
-
+  rclpy::define_guard_condition(m);
   rclpy::define_timer(m);
-
   rclpy::define_subscription(m);
   rclpy::define_time_point(m);
   rclpy::define_clock(m);

rclpy/src/rclpy/guard_condition.cpp

@@ -31,89 +31,69 @@
 
 namespace rclpy
 {
-/// Handle destructor for guard condition
-static void
-_rclpy_destroy_guard_condition(void * p)
-{
-  auto gc = static_cast<rcl_guard_condition_t *>(p);
-  if (!gc) {
-    // Warning should use line number of the current stack frame
-    int stack_level = 1;
-    PyErr_WarnFormat(
-      PyExc_RuntimeWarning, stack_level, "_rclpy_destroy_guard_condition got NULL pointer");
-    return;
-  }
-
-  rcl_ret_t ret = rcl_guard_condition_fini(gc);
-  if (RCL_RET_OK != ret) {
-    // Warning should use line number of the current stack frame
-    int stack_level = 1;
-    PyErr_WarnFormat(
-      PyExc_RuntimeWarning, stack_level, "Failed to fini guard condition: %s",
-      rcl_get_error_string().str);
-  }
-  PyMem_Free(gc);
-}
-
-py::capsule
-guard_condition_create(py::capsule pycontext)
+GuardCondition::GuardCondition(py::capsule pycontext)
 {
   auto context = static_cast<rcl_context_t *>(
     rclpy_handle_get_pointer_from_capsule(pycontext.ptr(), "rcl_context_t"));
   if (!context) {
     throw py::error_already_set();
   }
 
-  // Use smart pointer to make sure memory is free'd on error
-  auto deleter = [](rcl_guard_condition_t * ptr) {_rclpy_destroy_guard_condition(ptr);};
-  auto gc = std::unique_ptr<rcl_guard_condition_t, decltype(deleter)>(
-    static_cast<rcl_guard_condition_t *>(PyMem_Malloc(sizeof(rcl_guard_condition_t))),
-    deleter);
-  if (!gc) {
-    throw std::bad_alloc();
-  }
-
-  *gc = rcl_get_zero_initialized_guard_condition();
+  rcl_guard_condition_ = std::shared_ptr<rcl_guard_condition_t>(
+    new rcl_guard_condition_t,
+    [](rcl_guard_condition_t * guard_condition)
+    {
+      rcl_ret_t ret = rcl_guard_condition_fini(guard_condition);
+      if (RCL_RET_OK != ret) {
+        // Warning should use line number of the current stack frame
+        int stack_level = 1;
+        PyErr_WarnFormat(
+          PyExc_RuntimeWarning, stack_level, "Failed to fini guard condition: %s",
+          rcl_get_error_string().str);
+        rcl_reset_error();
+      }
+      delete guard_condition;
+    });
+
+  *rcl_guard_condition_ = rcl_get_zero_initialized_guard_condition();
   rcl_guard_condition_options_t gc_options = rcl_guard_condition_get_default_options();
 
-  rcl_ret_t ret = rcl_guard_condition_init(gc.get(), context, gc_options);
+  rcl_ret_t ret = rcl_guard_condition_init(rcl_guard_condition_.get(), context, gc_options);
   if (ret != RCL_RET_OK) {
     throw RCLError("failed to create guard_condition");
   }
-
-  PyObject * pygc_c =
-    rclpy_create_handle_capsule(gc.get(), "rcl_guard_condition_t", _rclpy_destroy_guard_condition);
-  if (!pygc_c) {
-    throw py::error_already_set();
-  }
-  auto pygc = py::reinterpret_steal<py::capsule>(pygc_c);
-  // pygc now owns the rcl_guard_contition_t
-  gc.release();
-
-  auto gc_handle = static_cast<rclpy_handle_t *>(pygc);
-  auto context_handle = static_cast<rclpy_handle_t *>(pycontext);
-  _rclpy_handle_add_dependency(gc_handle, context_handle);
-  if (PyErr_Occurred()) {
-    _rclpy_handle_dec_ref(gc_handle);
-    throw py::error_already_set();
-  }
-
-  return pygc;
 }
 
 void
-guard_condition_trigger(py::capsule pygc)
+GuardCondition::destroy()
 {
-  auto gc = static_cast<rcl_guard_condition_t *>(
-    rclpy_handle_get_pointer_from_capsule(pygc.ptr(), "rcl_guard_condition_t"));
-  if (!gc) {
-    throw py::error_already_set();
-  }
+  rcl_guard_condition_.reset();
+}
 
-  rcl_ret_t ret = rcl_trigger_guard_condition(gc);
+void
+GuardCondition::trigger_guard_condition()
+{
+  rcl_ret_t ret = rcl_trigger_guard_condition(rcl_guard_condition_.get());
 
-  if (ret != RCL_RET_OK) {
+  if (RCL_RET_OK != ret) {
     throw RCLError("failed to trigger guard condition");
   }
 }
+
+void define_guard_condition(py::object module)
+{
+  py::class_<GuardCondition, Destroyable, std::shared_ptr<GuardCondition>>(module, "GuardCondition")
+  .def(py::init<py::capsule>())
+  .def_property_readonly(
+    "pointer", [](const GuardCondition & guard_condition) {
+      return reinterpret_cast<size_t>(guard_condition.rcl_ptr());
+    },
+    "Get the address of the entity as an integer")
+  .def(
+    "trigger_guard_condition", &GuardCondition::trigger_guard_condition,
+    "Trigger a general purpose guard condition")
+  .def(
+    "pycapsule", &GuardCondition::pycapsule,
+    "Return a pycapsule object with the guardcondition pointer");
+}
 }  // namespace rclpy

rclpy/src/rclpy/guard_condition.hpp

@@ -17,31 +17,73 @@
 
 #include <pybind11/pybind11.h>
 
+#include <rcl/error_handling.h>
+#include <rcl/guard_condition.h>
+
+#include <memory>
+
+#include "destroyable.hpp"
+#include "rclpy_common/exceptions.hpp"
+#include "rclpy_common/handle.h"
+#include "utils.hpp"
+
 namespace py = pybind11;
 
 namespace rclpy
 {
 /// Create a general purpose guard condition
-/**
- * A successful call will return a Capsule with the pointer of the created
- * rcl_guard_condition_t * structure
- *
- * Raises RuntimeError if initializing the guard condition fails
- *
- * \return a guard condition capsule
- */
-py::capsule
-guard_condition_create(py::capsule pycontext);
-
-/// Trigger a general purpose guard condition
-/**
- * Raises ValueError if pygc is not a guard condition capsule
- * Raises RCLError if the guard condition could not be triggered
- *
- * \param[in] pygc Capsule pointing to guard condtition
- */
-void
-guard_condition_trigger(py::capsule pygc);
+class GuardCondition : public Destroyable, public std::enable_shared_from_this<GuardCondition>
+{
+public:
+  /**
+   * Raises RuntimeError if initializing the guard condition fails
+   */
+  explicit GuardCondition(py::capsule pycontext);
+
+  /// Trigger a general purpose guard condition
+  /**
+   * Raises ValueError if pygc is not a guard condition capsule
+   * Raises RCLError if the guard condition could not be triggered
+   */
+  void
+  trigger_guard_condition();
+
+  /// Get rcl_client_t pointer
+  rcl_guard_condition_t * rcl_ptr() const
+  {
+    return rcl_guard_condition_.get();
+  }
+
+  // TODO(ahcorde): Remove the pycapsule method when #728 is in
+  /// Return a pycapsule object to be able to handle the signal in C.
+  py::capsule
+  pycapsule() const
+  {
+    PyObject * capsule = rclpy_create_handle_capsule(
+      rcl_guard_condition_.get(), "rcl_guard_condition_t", _rclpy_destroy_guard_condition);
+    if (!capsule) {
+      throw py::error_already_set();
+    }
+    return py::reinterpret_steal<py::capsule>(capsule);
+  }
+
+  /// Force an early destruction of this object
+  void destroy() override;
+
+private:
+  std::shared_ptr<rcl_guard_condition_t> rcl_guard_condition_;
+
+  /// Handle destructor for guard condition
+  static void
+  _rclpy_destroy_guard_condition(void * p)
+  {
+    (void)p;
+    // Empty destructor, the class should take care of the lifecycle.
+  }
+};
+
+/// Define a pybind11 wrapper for an rclpy::Service
+void define_guard_condition(py::object module);
 }  // namespace rclpy
 
 #endif  // RCLPY__GUARD_CONDITION_HPP_

rclpy/src/rclpy/wait_set.cpp

@@ -178,6 +178,17 @@ WaitSet::add_timer(const Timer & timer)
   return index;
 }
 
+size_t
+WaitSet::add_guard_condition(const GuardCondition & gc)
+{
+  size_t index;
+  rcl_ret_t ret = rcl_wait_set_add_guard_condition(rcl_wait_set_.get(), gc.rcl_ptr(), &index);
+  if (RCL_RET_OK != ret) {
+    throw RCLError("failed to add guard condition to wait set");
+  }
+  return index;
+}
+
 size_t
 WaitSet::add_client(const Client & client)
 {
@@ -323,6 +334,9 @@ void define_waitset(py::object module)
   .def(
     "add_client", &WaitSet::add_client,
     "Add a client to the wait set structure")
+  .def(
+    "add_guard_condition", &WaitSet::add_guard_condition,
+    "Add a guard condition to the wait set structure")
   .def(
     "add_timer", &WaitSet::add_timer,
     "Add a timer to the wait set structure")

rclpy/src/rclpy/wait_set.hpp

@@ -22,6 +22,7 @@
 
 #include "client.hpp"
 #include "destroyable.hpp"
+#include "guard_condition.hpp"
 #include "qos_event.hpp"
 #include "service.hpp"
 #include "subscription.hpp"
@@ -107,6 +108,16 @@ class WaitSet : public Destroyable, public std::enable_shared_from_this<WaitSet>
   size_t
   add_client(const Client & client);
 
+  /// Add a guard condition to the wait set structure
+  /**
+   * Raises RCLError if any lower level error occurs
+   *
+   * \param[in] timer a guard condition to add to the wait set
+   * \return Index in waitset entity was added at
+   */
+  size_t
+  add_guard_condition(const GuardCondition & gc);
+
   /// Add a timer to the wait set structure
   /**
    * Raises RCLError if any lower level error occurs