diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py index 9595924f9..bfd0b2fe0 100644 --- a/rclpy/rclpy/executors.py +++ b/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) diff --git a/rclpy/rclpy/guard_condition.py b/rclpy/rclpy/guard_condition.py index 319b55fb8..b8ba32664 100644 --- a/rclpy/rclpy/guard_condition.py +++ b/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() diff --git a/rclpy/rclpy/signals.py b/rclpy/rclpy/signals.py index 0dc425af7..df47ffc18 100644 --- a/rclpy/rclpy/signals.py +++ b/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() diff --git a/rclpy/src/rclpy/_rclpy_pybind11.cpp b/rclpy/src/rclpy/_rclpy_pybind11.cpp index 1f0ad401e..19512db24 100644 --- a/rclpy/src/rclpy/_rclpy_pybind11.cpp +++ b/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); diff --git a/rclpy/src/rclpy/guard_condition.cpp b/rclpy/src/rclpy/guard_condition.cpp index d5f6e43f6..5c0d4a302 100644 --- a/rclpy/src/rclpy/guard_condition.cpp +++ b/rclpy/src/rclpy/guard_condition.cpp @@ -31,32 +31,7 @@ namespace rclpy { -/// Handle destructor for guard condition -static void -_rclpy_destroy_guard_condition(void * p) -{ - auto gc = static_cast(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( rclpy_handle_get_pointer_from_capsule(pycontext.ptr(), "rcl_context_t")); @@ -64,56 +39,61 @@ guard_condition_create(py::capsule pycontext) 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( - static_cast(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( + 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(pygc_c); - // pygc now owns the rcl_guard_contition_t - gc.release(); - - auto gc_handle = static_cast(pygc); - auto context_handle = static_cast(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( - 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_>(module, "GuardCondition") + .def(py::init()) + .def_property_readonly( + "pointer", [](const GuardCondition & guard_condition) { + return reinterpret_cast(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 diff --git a/rclpy/src/rclpy/guard_condition.hpp b/rclpy/src/rclpy/guard_condition.hpp index 4a61c9be9..f23c5e266 100644 --- a/rclpy/src/rclpy/guard_condition.hpp +++ b/rclpy/src/rclpy/guard_condition.hpp @@ -17,31 +17,73 @@ #include +#include +#include + +#include + +#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 +{ +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(capsule); + } + + /// Force an early destruction of this object + void destroy() override; + +private: + std::shared_ptr 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_ diff --git a/rclpy/src/rclpy/wait_set.cpp b/rclpy/src/rclpy/wait_set.cpp index 273856427..e901b8ec4 100644 --- a/rclpy/src/rclpy/wait_set.cpp +++ b/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") diff --git a/rclpy/src/rclpy/wait_set.hpp b/rclpy/src/rclpy/wait_set.hpp index bf5923999..6e10cc16e 100644 --- a/rclpy/src/rclpy/wait_set.hpp +++ b/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 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 diff --git a/rclpy/test/test_waitable.py b/rclpy/test/test_waitable.py index 18603fdd0..c7f422de0 100644 --- a/rclpy/test/test_waitable.py +++ b/rclpy/test/test_waitable.py @@ -220,7 +220,7 @@ def __init__(self, node): super().__init__(ReentrantCallbackGroup()) with node.context.handle as context_capsule: - self.guard_condition = _rclpy.rclpy_create_guard_condition(context_capsule) + self.guard_condition = _rclpy.GuardCondition(context_capsule) self.guard_condition_index = None self.guard_is_ready = False @@ -253,8 +253,7 @@ def get_num_entities(self): def add_to_wait_set(self, wait_set): """Add entities to wait set.""" - self.guard_condition_index = wait_set.add_entity( - 'guard_condition', self.guard_condition) + self.guard_condition_index = wait_set.add_guard_condition(self.guard_condition) class MutuallyExclusiveWaitable(Waitable): @@ -369,7 +368,7 @@ def test_waitable_with_guard_condition(self): self.node.add_waitable(self.waitable) thr = self.start_spin_thread(self.waitable) - _rclpy.rclpy_trigger_guard_condition(self.waitable.guard_condition) + self.waitable.guard_condition.trigger_guard_condition() thr.join() assert self.waitable.future.done()