diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py index c721428a2..9595924f9 100644 --- a/rclpy/rclpy/executors.py +++ b/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: diff --git a/rclpy/rclpy/qos_event.py b/rclpy/rclpy/qos_event.py index a7b878c49..4969816da 100644 --- a/rclpy/rclpy/qos_event.py +++ b/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.""" diff --git a/rclpy/src/rclpy/_rclpy_pybind11.cpp b/rclpy/src/rclpy/_rclpy_pybind11.cpp index 478fe44e1..1f0ad401e 100644 --- a/rclpy/src/rclpy/_rclpy_pybind11.cpp +++ b/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, diff --git a/rclpy/src/rclpy/action_client.cpp b/rclpy/src/rclpy/action_client.cpp index 68f78d421..3fa78cc76 100644 --- a/rclpy/src/rclpy/action_client.cpp +++ b/rclpy/src/rclpy/action_client.cpp @@ -234,12 +234,10 @@ 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(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); @@ -247,17 +245,15 @@ ActionClient::add_to_waitset(py::capsule pywait_set) } py::tuple -ActionClient::is_ready(py::capsule pywait_set) +ActionClient::is_ready(WaitSet & wait_set) { - auto wait_set = static_cast(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, diff --git a/rclpy/src/rclpy/action_client.hpp b/rclpy/src/rclpy/action_client.hpp index 7ce59659f..360e92343 100644 --- a/rclpy/src/rclpy/action_client.hpp +++ b/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(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(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"); } diff --git a/rclpy/src/rclpy/action_server.hpp b/rclpy/src/rclpy/action_server.hpp index f5008a9dd..c58279f9f 100644 --- a/rclpy/src/rclpy/action_server.hpp +++ b/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(PyCapsule_GetPointer(pycapsule, "rcl_wait_set_t")); - - rcl_ret_t ret = rcl_wait_set_fini(wait_set); - if (ret != RCL_RET_OK) { - // Warning should use line number of the current stack frame - int stack_level = 1; - PyErr_WarnFormat( - PyExc_RuntimeWarning, stack_level, - "Failed to fini wait set: %s", rcl_get_error_string().str); - rcl_reset_error(); - } - - PyMem_Free(wait_set); -} - -py::capsule -get_zero_initialized_wait_set() +WaitSet::WaitSet() { - auto deleter = [](rcl_wait_set_t * ptr) {PyMem_FREE(ptr);}; - auto wait_set = std::unique_ptr( - static_cast(PyMem_Malloc(sizeof(rcl_wait_set_t))), - deleter); - if (!wait_set) { - throw std::bad_alloc(); - } - - *wait_set = rcl_get_zero_initialized_wait_set(); - return py::capsule(wait_set.release(), "rcl_wait_set_t", _rclpy_destroy_wait_set); + // Create a client + rcl_wait_set_ = std::shared_ptr( + new rcl_wait_set_t, + [](rcl_wait_set_t * waitset) + { + rcl_ret_t ret = rcl_wait_set_fini(waitset); + 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 wait set: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + delete waitset; + }); + *rcl_wait_set_ = rcl_get_zero_initialized_wait_set(); } -void -wait_set_init( - py::capsule pywait_set, +WaitSet::WaitSet( size_t number_of_subscriptions, size_t number_of_guard_conditions, size_t number_of_timers, @@ -77,10 +62,23 @@ wait_set_init( size_t number_of_events, py::capsule pycontext) { - if (0 != std::strcmp("rcl_wait_set_t", pywait_set.name())) { - throw py::value_error("capsule is not an rcl_wait_set_t"); - } - auto wait_set = static_cast(pywait_set); + // Create a client + rcl_wait_set_ = std::shared_ptr( + new rcl_wait_set_t, + [](rcl_wait_set_t * waitset) + { + rcl_ret_t ret = rcl_wait_set_fini(waitset); + 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 wait set: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + delete waitset; + }); + *rcl_wait_set_ = rcl_get_zero_initialized_wait_set(); auto context = static_cast( rclpy_handle_get_pointer_from_capsule(pycontext.ptr(), "rcl_context_t")); @@ -89,7 +87,7 @@ wait_set_init( } rcl_ret_t ret = rcl_wait_set_init( - wait_set, + rcl_wait_set_.get(), number_of_subscriptions, number_of_guard_conditions, number_of_timers, @@ -98,33 +96,29 @@ wait_set_init( number_of_events, context, rcl_get_default_allocator()); - if (ret != RCL_RET_OK) { + if (RCL_RET_OK != ret) { throw RCLError("failed to initialize wait set"); } } void -wait_set_clear_entities(py::capsule pywait_set) +WaitSet::destroy() { - if (0 != std::strcmp("rcl_wait_set_t", pywait_set.name())) { - throw py::value_error("capsule is not an rcl_wait_set_t"); - } - auto wait_set = static_cast(pywait_set); + rcl_wait_set_.reset(); +} - rcl_ret_t ret = rcl_wait_set_clear(wait_set); +void +WaitSet::clear_entities() +{ + rcl_ret_t ret = rcl_wait_set_clear(rcl_wait_set_.get()); if (ret != RCL_RET_OK) { throw RCLError("failed to clear wait set"); } } size_t -wait_set_add_entity(const std::string & entity_type, py::capsule pywait_set, py::capsule pyentity) +WaitSet::add_entity(const std::string & entity_type, py::capsule pyentity) { - if (0 != std::strcmp("rcl_wait_set_t", pywait_set.name())) { - throw py::value_error("capsule is not an rcl_wait_set_t"); - } - auto wait_set = static_cast(pywait_set); - rcl_ret_t ret = RCL_RET_ERROR; size_t index; @@ -134,7 +128,7 @@ wait_set_add_entity(const std::string & entity_type, py::capsule pywait_set, py: if (!guard_condition) { throw py::error_already_set(); } - ret = rcl_wait_set_add_guard_condition(wait_set, guard_condition, &index); + ret = rcl_wait_set_add_guard_condition(rcl_wait_set_.get(), guard_condition, &index); } else { std::string error_text{"'"}; error_text += entity_type; @@ -151,15 +145,10 @@ wait_set_add_entity(const std::string & entity_type, py::capsule pywait_set, py: } size_t -wait_set_add_service(const py::capsule pywait_set, const Service & service) +WaitSet::add_service(const Service & service) { - if (0 != std::strcmp("rcl_wait_set_t", pywait_set.name())) { - throw py::value_error("capsule is not an rcl_wait_set_t"); - } - auto wait_set = static_cast(pywait_set); - size_t index; - rcl_ret_t ret = rcl_wait_set_add_service(wait_set, service.rcl_ptr(), &index); + rcl_ret_t ret = rcl_wait_set_add_service(rcl_wait_set_.get(), service.rcl_ptr(), &index); if (RCL_RET_OK != ret) { throw RCLError("failed to add service to wait set"); } @@ -167,15 +156,11 @@ wait_set_add_service(const py::capsule pywait_set, const Service & service) } size_t -wait_set_add_subscription(const py::capsule pywait_set, const Subscription & subscription) +WaitSet::add_subscription(const Subscription & subscription) { - if (0 != std::strcmp("rcl_wait_set_t", pywait_set.name())) { - throw py::value_error("capsule is not an rcl_wait_set_t"); - } - auto wait_set = static_cast(pywait_set); - size_t index; - rcl_ret_t ret = rcl_wait_set_add_subscription(wait_set, subscription.rcl_ptr(), &index); + rcl_ret_t ret = rcl_wait_set_add_subscription( + rcl_wait_set_.get(), subscription.rcl_ptr(), &index); if (RCL_RET_OK != ret) { throw RCLError("failed to add subscription to wait set"); } @@ -183,15 +168,10 @@ wait_set_add_subscription(const py::capsule pywait_set, const Subscription & sub } size_t -wait_set_add_timer(const py::capsule pywait_set, const Timer & timer) +WaitSet::add_timer(const Timer & timer) { - if (0 != std::strcmp("rcl_wait_set_t", pywait_set.name())) { - throw py::value_error("capsule is not an rcl_wait_set_t"); - } - auto wait_set = static_cast(pywait_set); - size_t index; - rcl_ret_t ret = rcl_wait_set_add_timer(wait_set, timer.rcl_ptr(), &index); + rcl_ret_t ret = rcl_wait_set_add_timer(rcl_wait_set_.get(), timer.rcl_ptr(), &index); if (RCL_RET_OK != ret) { throw RCLError("failed to add client to wait set"); } @@ -199,15 +179,10 @@ wait_set_add_timer(const py::capsule pywait_set, const Timer & timer) } size_t -wait_set_add_client(const py::capsule pywait_set, const Client & client) +WaitSet::add_client(const Client & client) { - if (0 != std::strcmp("rcl_wait_set_t", pywait_set.name())) { - throw py::value_error("capsule is not an rcl_wait_set_t"); - } - auto wait_set = static_cast(pywait_set); - size_t index; - rcl_ret_t ret = rcl_wait_set_add_client(wait_set, client.rcl_ptr(), &index); + rcl_ret_t ret = rcl_wait_set_add_client(rcl_wait_set_.get(), client.rcl_ptr(), &index); if (RCL_RET_OK != ret) { throw RCLError("failed to add client to wait set"); } @@ -215,15 +190,10 @@ wait_set_add_client(const py::capsule pywait_set, const Client & client) } size_t -wait_set_add_event(const py::capsule pywait_set, const QoSEvent & event) +WaitSet::add_event(const QoSEvent & event) { - if (0 != std::strcmp("rcl_wait_set_t", pywait_set.name())) { - throw py::value_error("capsule is not an rcl_wait_set_t"); - } - auto wait_set = static_cast(pywait_set); - size_t index; - rcl_ret_t ret = rcl_wait_set_add_event(wait_set, event.rcl_ptr(), &index); + rcl_ret_t ret = rcl_wait_set_add_event(rcl_wait_set_.get(), event.rcl_ptr(), &index); if (RCL_RET_OK != ret) { throw RCLError("failed to add event to wait set"); } @@ -231,33 +201,28 @@ wait_set_add_event(const py::capsule pywait_set, const QoSEvent & event) } bool -wait_set_is_ready(const std::string & entity_type, py::capsule pywait_set, size_t index) +WaitSet::is_ready(const std::string & entity_type, size_t index) { - if (0 != std::strcmp("rcl_wait_set_t", pywait_set.name())) { - throw py::value_error("capsule is not an rcl_wait_set_t"); - } - auto wait_set = static_cast(pywait_set); - const void ** entities = NULL; size_t num_entities = 0; if ("subscription" == entity_type) { - entities = reinterpret_cast(wait_set->subscriptions); - num_entities = wait_set->size_of_subscriptions; + entities = reinterpret_cast(rcl_wait_set_->subscriptions); + num_entities = rcl_wait_set_->size_of_subscriptions; } else if ("client" == entity_type) { - entities = reinterpret_cast(wait_set->clients); - num_entities = wait_set->size_of_clients; + entities = reinterpret_cast(rcl_wait_set_->clients); + num_entities = rcl_wait_set_->size_of_clients; } else if ("service" == entity_type) { - entities = reinterpret_cast(wait_set->services); - num_entities = wait_set->size_of_services; + entities = reinterpret_cast(rcl_wait_set_->services); + num_entities = rcl_wait_set_->size_of_services; } else if ("timer" == entity_type) { - entities = reinterpret_cast(wait_set->timers); - num_entities = wait_set->size_of_timers; + entities = reinterpret_cast(rcl_wait_set_->timers); + num_entities = rcl_wait_set_->size_of_timers; } else if ("guard_condition" == entity_type) { - entities = reinterpret_cast(wait_set->guard_conditions); - num_entities = wait_set->size_of_guard_conditions; + entities = reinterpret_cast(rcl_wait_set_->guard_conditions); + num_entities = rcl_wait_set_->size_of_guard_conditions; } else if ("event" == entity_type) { - entities = reinterpret_cast(wait_set->events); - num_entities = wait_set->size_of_events; + entities = reinterpret_cast(rcl_wait_set_->events); + num_entities = rcl_wait_set_->size_of_events; } else { std::string error_text{"'"}; error_text += entity_type; @@ -292,23 +257,23 @@ _get_ready_entities(const EntityArray ** entities, const size_t num_entities) } py::list -get_ready_entities(const std::string & entity_type, py::capsule pywait_set) +WaitSet::get_ready_entities(const std::string & entity_type) { - if (0 != std::strcmp("rcl_wait_set_t", pywait_set.name())) { - throw py::value_error("capsule is not an rcl_wait_set_t"); - } - auto wait_set = static_cast(pywait_set); - if ("subscription" == entity_type) { - return _get_ready_entities(wait_set->subscriptions, wait_set->size_of_subscriptions); + return _get_ready_entities( + rcl_wait_set_->subscriptions, rcl_wait_set_->size_of_subscriptions); } else if ("client" == entity_type) { - return _get_ready_entities(wait_set->clients, wait_set->size_of_clients); + return _get_ready_entities( + rcl_wait_set_->clients, rcl_wait_set_->size_of_clients); } else if ("service" == entity_type) { - return _get_ready_entities(wait_set->services, wait_set->size_of_services); + return _get_ready_entities( + rcl_wait_set_->services, rcl_wait_set_->size_of_services); } else if ("timer" == entity_type) { - return _get_ready_entities(wait_set->timers, wait_set->size_of_timers); + return _get_ready_entities( + rcl_wait_set_->timers, rcl_wait_set_->size_of_timers); } else if ("guard_condition" == entity_type) { - return _get_ready_entities(wait_set->guard_conditions, wait_set->size_of_guard_conditions); + return _get_ready_entities( + rcl_wait_set_->guard_conditions, rcl_wait_set_->size_of_guard_conditions); } std::string error_text{"'"}; @@ -318,23 +283,60 @@ get_ready_entities(const std::string & entity_type, py::capsule pywait_set) } void -wait(py::capsule pywait_set, int64_t timeout) +WaitSet::wait(int64_t timeout) { - if (0 != std::strcmp("rcl_wait_set_t", pywait_set.name())) { - throw py::value_error("capsule is not an rcl_wait_set_t"); - } - auto wait_set = static_cast(pywait_set); - rcl_ret_t ret; // Could be a long wait, release the GIL { py::gil_scoped_release gil_release; - ret = rcl_wait(wait_set, timeout); + ret = rcl_wait(rcl_wait_set_.get(), timeout); } - if (ret != RCL_RET_OK && ret != RCL_RET_TIMEOUT) { + if (RCL_RET_OK != ret && RCL_RET_TIMEOUT != ret) { throw RCLError("failed to wait on wait set"); } } + +void define_waitset(py::object module) +{ + py::class_>(module, "WaitSet") + .def(py::init()) + .def(py::init<>()) + .def_property_readonly( + "pointer", [](const WaitSet & waitset) { + return reinterpret_cast(waitset.rcl_ptr()); + }, + "Get the address of the entity as an integer") + .def( + "clear_entities", &WaitSet::clear_entities, + "Clear all the pointers in the wait set") + .def( + "add_entity", &WaitSet::add_entity, + "Add an entity to the wait set structure") + .def( + "add_service", &WaitSet::add_service, + "Add a service to the wait set structure") + .def( + "add_subscription", &WaitSet::add_subscription, + "Add a subcription to the wait set structure") + .def( + "add_client", &WaitSet::add_client, + "Add a client to the wait set structure") + .def( + "add_timer", &WaitSet::add_timer, + "Add a timer to the wait set structure") + .def( + "add_event", &WaitSet::add_event, + "Add an event to the wait set structure") + .def( + "is_ready", &WaitSet::is_ready, + "Check if an entity in the wait set is ready by its index") + .def( + "get_ready_entities", &WaitSet::get_ready_entities, + "Get list of entities ready by entity type") + .def( + "wait", &WaitSet::wait, + "Wait until timeout is reached or event happened"); +} } // namespace rclpy diff --git a/rclpy/src/rclpy/wait_set.hpp b/rclpy/src/rclpy/wait_set.hpp index 5cd1b5f84..bf5923999 100644 --- a/rclpy/src/rclpy/wait_set.hpp +++ b/rclpy/src/rclpy/wait_set.hpp @@ -21,6 +21,7 @@ #include #include "client.hpp" +#include "destroyable.hpp" #include "qos_event.hpp" #include "service.hpp" #include "subscription.hpp" @@ -30,149 +31,151 @@ namespace py = pybind11; namespace rclpy { -/// Return a Capsule pointing to a zero initialized rcl_wait_set_t structure -py::capsule -get_zero_initialized_wait_set(); - -/// Initialize a wait set -/** - * Raises RCLError if the wait set could not be initialized - * - * \param[in] pywait_set Capsule pointing to the wait set structure - * \param[in] node_name string name of the node to be created - * \param[in] number_of_subscriptions int - * \param[in] number_of_guard_conditions int - * \param[in] number_of_timers int - * \param[in] number_of_clients int - * \param[in] number_of_services int - * \param[in] pycontext Capsule pointing to an rcl_context_t - */ -void -wait_set_init( - py::capsule pywait_set, - size_t number_of_subscriptions, - size_t number_of_guard_conditions, - size_t number_of_timers, - size_t number_of_clients, - size_t number_of_services, - size_t number_of_events, - py::capsule pycontext); - -/// Clear all the pointers in the wait set -/** - * Raises RCLError if any rcl error occurs - * - * \param[in] pywait_set Capsule pointing to the wait set structure - */ -void -wait_set_clear_entities(py::capsule pywait_set); - -/// Add an entity to the wait set structure -/** - * Raises RuntimeError if the entity type is unknown - * Raises RCLError if any lower level error occurs - * - * \param[in] entity_type string defining the entity ["subscription, client, service"] - * \param[in] pywait_set Capsule pointing to the wait set structure - * \param[in] pyentity Capsule pointing to the entity to add - * \return Index in waitset entity was added at - */ -size_t -wait_set_add_entity(const std::string & entity_type, py::capsule pywait_set, py::capsule pyentity); - -/// Add a service to the wait set structure -/** - * Raises RCLError if any lower level error occurs - * - * \param[in] pywait_set Capsule pointing to the wait set structure - * \param[in] service a service to add to the wait set - * \return Index in waitset entity was added at - */ -size_t -wait_set_add_service(const py::capsule pywait_set, const Service & service); - -/// Add a subcription to the wait set structure -/** - * Raises RCLError if any lower level error occurs - * - * \param[in] pywait_set Capsule pointing to the wait set structure - * \param[in] service a service to add to the wait set - * \return Index in waitset entity was added at - */ -size_t -wait_set_add_subscription(const py::capsule pywait_set, const Subscription & subscription); - -/// Add a client to the wait set structure -/** - * Raises RCLError if any lower level error occurs - * - * \param[in] pywait_set Capsule pointing to the wait set structure - * \param[in] client a client to add to the wait set - * \return Index in waitset entity was added at - */ -size_t -wait_set_add_client(const py::capsule pywait_set, const Client & client); - -/// Add a timer to the wait set structure -/** - * Raises RCLError if any lower level error occurs - * - * \param[in] pywait_set Capsule pointing to the wait set structure - * \param[in] timer a timer to add to the wait set - * \return Index in waitset entity was added at - */ -size_t -wait_set_add_timer(const py::capsule pywait_set, const Timer & timer); - -/// Add an event to the wait set structure -/** - * Raises RCLError if any lower level error occurs - * - * \param[in] pywait_set Capsule pointing to the wait set structure - * \param[in] event a QoSEvent to add to the wait set - * \return Index in waitset entity was added at - */ -size_t -wait_set_add_event(const py::capsule pywait_set, const QoSEvent & event); - -/// Check if an entity in the wait set is ready by its index -/** - * This must be called after waiting on the wait set. - * Raises RuntimeError if the entity type is unknown - * Raises IndexError if the given index is beyond the number of entities in the set - * - * \param[in] entity_type string defining the entity ["subscription, client, service"] - * \param[in] pywait_set Capsule pointing to the wait set structure - * \param[in] index location in the wait set of the entity to check - * \return True if the entity at the index in the wait set is not NULL - */ -bool -wait_set_is_ready(const std::string & entity_type, py::capsule pywait_set, size_t index); - -/// Get list of non-null entities in wait set -/** - * Raises ValueError if pywait_set is not a wait set capsule - * Raises RuntimeError if the entity type is not known - * - * \param[in] entity_type string defining the entity ["subscription, client, service"] - * \param[in] pywait_set Capsule pointing to the wait set structure - * \return List of wait set entities pointers ready for take - */ -py::list -get_ready_entities(const std::string & entity_type, py::capsule pywait_set); - -/// Wait until timeout is reached or event happened -/** - * Raises ValueError if pywait_set is not a wait set capsule - * Raises RCLError if there was an error while waiting - * - * This function will wait for an event to happen or for the timeout to expire. - * A negative timeout means wait forever, a timeout of 0 means no wait - * \param[in] pywait_set Capsule pointing to the wait set structure - * \param[in] timeout optional time to wait before waking up (in nanoseconds) - */ -void -wait(py::capsule pywait_set, int64_t timeout); +class WaitSet : public Destroyable, public std::enable_shared_from_this +{ +public: + /// Initialize a wait set + WaitSet(); + + /// Initialize a wait set + /** + * Raises RCLError if the wait set could not be initialized + * + * \param[in] node_name string name of the node to be created + * \param[in] number_of_subscriptions a positive number or zero + * \param[in] number_of_guard_conditions int + * \param[in] number_of_timers int + * \param[in] number_of_clients int + * \param[in] number_of_services int + * \param[in] pycontext Capsule pointing to an rcl_context_t + */ + WaitSet( + size_t number_of_subscriptions, + size_t number_of_guard_conditions, + size_t number_of_timers, + size_t number_of_clients, + size_t number_of_services, + size_t number_of_events, + py::capsule pycontext); + + /// Clear all the pointers in the wait set + /** + * Raises RCLError if any rcl error occurs + */ + void + clear_entities(); + + /// Add an entity to the wait set structure + /** + * Raises RuntimeError if the entity type is unknown + * Raises RCLError if any lower level error occurs + * + * \param[in] entity_type string defining the entity ["subscription, client, service"] + * \param[in] pyentity Capsule pointing to the entity to add + * \return Index in waitset entity was added at + */ + size_t + add_entity(const std::string & entity_type, py::capsule pyentity); + + /// Add a service to the wait set structure + /** + * Raises RCLError if any lower level error occurs + * + * \param[in] service a service to add to the wait set + * \return Index in waitset entity was added at + */ + size_t + add_service(const Service & service); + + /// Add a subcription to the wait set structure + /** + * Raises RCLError if any lower level error occurs + * + * \param[in] service a service to add to the wait set + * \return Index in waitset entity was added at + */ + size_t + add_subscription(const Subscription & subscription); + + /// Add a client to the wait set structure + /** + * Raises RCLError if any lower level error occurs + * + * \param[in] client a client to add to the wait set + * \return Index in waitset entity was added at + */ + size_t + add_client(const Client & client); + + /// Add a timer to the wait set structure + /** + * Raises RCLError if any lower level error occurs + * + * \param[in] timer a timer to add to the wait set + * \return Index in waitset entity was added at + */ + size_t + add_timer(const Timer & timer); + + /// Add an event to the wait set structure + /** + * Raises RCLError if any lower level error occurs + * + * \param[in] event a QoSEvent to add to the wait set + * \return Index in waitset entity was added at + */ + size_t + add_event(const QoSEvent & event); + + /// Check if an entity in the wait set is ready by its index + /** + * This must be called after waiting on the wait set. + * Raises RuntimeError if the entity type is unknown + * Raises IndexError if the given index is beyond the number of entities in the set + * + * \param[in] entity_type string defining the entity ["subscription, client, service"] + * \param[in] index location in the wait set of the entity to check + * \return True if the entity at the index in the wait set is not NULL + */ + bool + is_ready(const std::string & entity_type, size_t index); + + /// Get list of entities ready by entity type + /** + * Raises RuntimeError if the entity type is not known + * + * \param[in] entity_type string defining the entity ["subscription, client, service"] + * \return List of wait set entities pointers ready for take + */ + py::list + get_ready_entities(const std::string & entity_type); + + /// Wait until timeout is reached or event happened + /** + * Raises RCLError if there was an error while waiting + * + * This function will wait for an event to happen or for the timeout to expire. + * A negative timeout means wait forever, a timeout of 0 means no wait + * \param[in] timeout optional time to wait before waking up (in nanoseconds) + */ + void + wait(int64_t timeout); + + /// Get rcl_client_t pointer + rcl_wait_set_t * rcl_ptr() const + { + return rcl_wait_set_.get(); + } + + /// Force an early destruction of this object + void destroy() override; + +private: + std::shared_ptr rcl_wait_set_; +}; + +/// Define a pybind11 wrapper for an rclpy::Service +void define_waitset(py::object module); } // namespace rclpy #endif // RCLPY__WAIT_SET_HPP_ diff --git a/rclpy/test/test_qos_event.py b/rclpy/test/test_qos_event.py index 8435f141b..34a810468 100644 --- a/rclpy/test/test_qos_event.py +++ b/rclpy/test/test_qos_event.py @@ -237,41 +237,40 @@ def test_call_publisher_rclpy_event_apis(self): # Go through the exposed apis and ensure that things don't explode when called # Make no assumptions about being able to actually receive the events publisher = self.node.create_publisher(EmptyMsg, self.topic_name, 10) - wait_set = _rclpy.rclpy_get_zero_initialized_wait_set() with self.context.handle as context_handle: - _rclpy.rclpy_wait_set_init(wait_set, 0, 0, 0, 0, 0, 3, context_handle) + wait_set = _rclpy.WaitSet(0, 0, 0, 0, 0, 3, context_handle) deadline_event_handle = self._create_event_handle( publisher, QoSPublisherEventType.RCL_PUBLISHER_OFFERED_DEADLINE_MISSED) with deadline_event_handle: - deadline_event_index = _rclpy.rclpy_wait_set_add_event(wait_set, deadline_event_handle) + deadline_event_index = wait_set.add_event(deadline_event_handle) self.assertIsNotNone(deadline_event_index) liveliness_event_handle = self._create_event_handle( publisher, QoSPublisherEventType.RCL_PUBLISHER_LIVELINESS_LOST) with liveliness_event_handle: - liveliness_event_index = _rclpy.rclpy_wait_set_add_event( - wait_set, liveliness_event_handle) + liveliness_event_index = wait_set.add_event( + liveliness_event_handle) self.assertIsNotNone(liveliness_event_index) try: incompatible_qos_event_handle = self._create_event_handle( publisher, QoSPublisherEventType.RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS) with incompatible_qos_event_handle: - incompatible_qos_event_index = _rclpy.rclpy_wait_set_add_event( - wait_set, incompatible_qos_event_handle) + incompatible_qos_event_index = wait_set.add_event( + incompatible_qos_event_handle) self.assertIsNotNone(incompatible_qos_event_index) except UnsupportedEventTypeError: self.assertTrue(self.is_fastrtps) # We live in our own namespace and have created no other participants, so # there can't be any of these events. - _rclpy.rclpy_wait(wait_set, 0) - self.assertFalse(_rclpy.rclpy_wait_set_is_ready('event', wait_set, deadline_event_index)) - self.assertFalse(_rclpy.rclpy_wait_set_is_ready('event', wait_set, liveliness_event_index)) + wait_set.wait(0) + self.assertFalse(wait_set.is_ready('event', deadline_event_index)) + self.assertFalse(wait_set.is_ready('event', liveliness_event_index)) if not self.is_fastrtps: - self.assertFalse(_rclpy.rclpy_wait_set_is_ready( - 'event', wait_set, incompatible_qos_event_index)) + self.assertFalse(wait_set.is_ready( + 'event', incompatible_qos_event_index)) # Calling take data even though not ready should provide me an empty initialized message # Tests data conversion utilities in C side @@ -310,42 +309,39 @@ def test_call_subscription_rclpy_event_apis(self): # Go through the exposed apis and ensure that things don't explode when called # Make no assumptions about being able to actually receive the events subscription = self.node.create_subscription(EmptyMsg, self.topic_name, Mock(), 10) - wait_set = _rclpy.rclpy_get_zero_initialized_wait_set() with self.context.handle as context_handle: - _rclpy.rclpy_wait_set_init(wait_set, 0, 0, 0, 0, 0, 3, context_handle) + wait_set = _rclpy.WaitSet(0, 0, 0, 0, 0, 3, context_handle) deadline_event_handle = self._create_event_handle( subscription, QoSSubscriptionEventType.RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED) with deadline_event_handle: - deadline_event_index = _rclpy.rclpy_wait_set_add_event( - wait_set, deadline_event_handle) + deadline_event_index = wait_set.add_event(deadline_event_handle) self.assertIsNotNone(deadline_event_index) liveliness_event_handle = self._create_event_handle( subscription, QoSSubscriptionEventType.RCL_SUBSCRIPTION_LIVELINESS_CHANGED) with liveliness_event_handle: - liveliness_event_index = _rclpy.rclpy_wait_set_add_event( - wait_set, liveliness_event_handle) + liveliness_event_index = wait_set.add_event(liveliness_event_handle) self.assertIsNotNone(liveliness_event_index) try: incompatible_qos_event_handle = self._create_event_handle( subscription, QoSSubscriptionEventType.RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS) with incompatible_qos_event_handle: - incompatible_qos_event_index = _rclpy.rclpy_wait_set_add_event( - wait_set, incompatible_qos_event_handle) + incompatible_qos_event_index = wait_set.add_event( + incompatible_qos_event_handle) self.assertIsNotNone(incompatible_qos_event_index) except UnsupportedEventTypeError: self.assertTrue(self.is_fastrtps) # We live in our own namespace and have created no other participants, so # there can't be any of these events. - _rclpy.rclpy_wait(wait_set, 0) - self.assertFalse(_rclpy.rclpy_wait_set_is_ready('event', wait_set, deadline_event_index)) - self.assertFalse(_rclpy.rclpy_wait_set_is_ready('event', wait_set, liveliness_event_index)) + wait_set.wait(0) + self.assertFalse(wait_set.is_ready('event', deadline_event_index)) + self.assertFalse(wait_set.is_ready('event', liveliness_event_index)) if not self.is_fastrtps: - self.assertFalse(_rclpy.rclpy_wait_set_is_ready( - 'event', wait_set, incompatible_qos_event_index)) + self.assertFalse(wait_set.is_ready( + 'event', incompatible_qos_event_index)) # Calling take data even though not ready should provide me an empty initialized message # Tests data conversion utilities in C side diff --git a/rclpy/test/test_waitable.py b/rclpy/test/test_waitable.py index 135d36dda..18603fdd0 100644 --- a/rclpy/test/test_waitable.py +++ b/rclpy/test/test_waitable.py @@ -52,7 +52,7 @@ def __init__(self, node): def is_ready(self, wait_set): """Return True if entities are ready in the wait set.""" - if _rclpy.rclpy_wait_set_is_ready('client', wait_set, self.client_index): + if wait_set.is_ready('client', self.client_index): self.client_is_ready = True return self.client_is_ready @@ -76,7 +76,7 @@ def get_num_entities(self): def add_to_wait_set(self, wait_set): """Add entities to wait set.""" - self.client_index = _rclpy.rclpy_wait_set_add_client(wait_set, self.client) + self.client_index = wait_set.add_client(self.client) class ServerWaitable(Waitable): @@ -95,7 +95,7 @@ def __init__(self, node): def is_ready(self, wait_set): """Return True if entities are ready in the wait set.""" - if _rclpy.rclpy_wait_set_is_ready('service', wait_set, self.server_index): + if wait_set.is_ready('service', self.server_index): self.server_is_ready = True return self.server_is_ready @@ -119,7 +119,7 @@ def get_num_entities(self): def add_to_wait_set(self, wait_set): """Add entities to wait set.""" - self.server_index = _rclpy.rclpy_wait_set_add_service(wait_set, self.server) + self.server_index = wait_set.add_service(self.server) class TimerWaitable(Waitable): @@ -140,7 +140,7 @@ def __init__(self, node): def is_ready(self, wait_set): """Return True if entities are ready in the wait set.""" - if _rclpy.rclpy_wait_set_is_ready('timer', wait_set, self.timer_index): + if wait_set.is_ready('timer', self.timer_index): self.timer_is_ready = True return self.timer_is_ready @@ -165,7 +165,7 @@ def get_num_entities(self): def add_to_wait_set(self, wait_set): """Add entities to wait set.""" - self.timer_index = _rclpy.rclpy_wait_set_add_timer(wait_set, self.timer) + self.timer_index = wait_set.add_timer(self.timer) class SubscriptionWaitable(Waitable): @@ -184,7 +184,7 @@ def __init__(self, node): def is_ready(self, wait_set): """Return True if entities are ready in the wait set.""" - if _rclpy.rclpy_wait_set_is_ready('subscription', wait_set, self.subscription_index): + if wait_set.is_ready('subscription', self.subscription_index): self.subscription_is_ready = True return self.subscription_is_ready @@ -210,8 +210,8 @@ def get_num_entities(self): def add_to_wait_set(self, wait_set): """Add entities to wait set.""" - self.subscription_index = _rclpy.rclpy_wait_set_add_subscription( - wait_set, self.subscription) + self.subscription_index = wait_set.add_subscription( + self.subscription) class GuardConditionWaitable(Waitable): @@ -229,7 +229,7 @@ def __init__(self, node): def is_ready(self, wait_set): """Return True if entities are ready in the wait set.""" - if _rclpy.rclpy_wait_set_is_ready('guard_condition', wait_set, self.guard_condition_index): + if wait_set.is_ready('guard_condition', self.guard_condition_index): self.guard_is_ready = True return self.guard_is_ready @@ -253,8 +253,8 @@ def get_num_entities(self): def add_to_wait_set(self, wait_set): """Add entities to wait set.""" - self.guard_condition_index = _rclpy.rclpy_wait_set_add_entity( - 'guard_condition', wait_set, self.guard_condition) + self.guard_condition_index = wait_set.add_entity( + 'guard_condition', self.guard_condition) class MutuallyExclusiveWaitable(Waitable):