diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py index d0c1d02f8..80a580cfb 100644 --- a/rclpy/rclpy/executors.py +++ b/rclpy/rclpy/executors.py @@ -329,8 +329,8 @@ async def _execute_timer(self, tmr, _): await await_or_execute(tmr.callback) def _take_subscription(self, sub): - with sub.handle as capsule: - msg_info = _rclpy.rclpy_take(capsule, sub.msg_type, sub.raw) + with sub.handle: + msg_info = sub.handle.take_message(sub.msg_type, sub.raw) if msg_info is not None: return msg_info[0] return None @@ -512,10 +512,11 @@ def _wait_for_ready_callbacks( # Construct a wait set wait_set = _rclpy.rclpy_get_zero_initialized_wait_set() with ExitStack() as context_stack: - sub_capsules = [] + sub_handles = [] for sub in subscriptions: try: - sub_capsules.append(context_stack.enter_context(sub.handle)) + context_stack.enter_context(sub.handle) + sub_handles.append(sub.handle) except InvalidHandle: entity_count.num_subscriptions -= 1 @@ -562,8 +563,8 @@ def _wait_for_ready_callbacks( context_capsule) _rclpy.rclpy_wait_set_clear_entities(wait_set) - for sub_capsule in sub_capsules: - _rclpy.rclpy_wait_set_add_entity('subscription', wait_set, sub_capsule) + for sub_handle in sub_handles: + _rclpy.rclpy_wait_set_add_subscription(wait_set, sub_handle) for cli_handle in client_handles: _rclpy.rclpy_wait_set_add_client(wait_set, cli_handle) for srv_capsule in service_handles: diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index a4ad1560b..b8f36137b 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -1257,21 +1257,20 @@ def create_publisher( check_is_valid_msg_type(msg_type) try: with self.handle as node_capsule: - publisher_capsule = _rclpy.rclpy_create_publisher( + publisher_object = _rclpy.Publisher( node_capsule, msg_type, topic, qos_profile.get_c_qos_profile()) except ValueError: failed = True if failed: self._validate_topic_or_service_name(topic) - publisher_handle = Handle(publisher_capsule) try: publisher = Publisher( - publisher_handle, msg_type, topic, qos_profile, + publisher_object, msg_type, topic, qos_profile, event_callbacks=event_callbacks or PublisherEventCallbacks(), callback_group=callback_group) except Exception: - publisher_handle.destroy() + publisher_object.destroy_when_not_in_use() raise self.__publishers.append(publisher) self._wake_executor() @@ -1335,21 +1334,20 @@ def create_subscription( check_is_valid_msg_type(msg_type) try: with self.handle as capsule: - subscription_capsule = _rclpy.rclpy_create_subscription( + subscription_object = _rclpy.Subscription( capsule, msg_type, topic, qos_profile.get_c_qos_profile()) except ValueError: failed = True if failed: self._validate_topic_or_service_name(topic) - subscription_handle = Handle(subscription_capsule) try: subscription = Subscription( - subscription_handle, msg_type, + subscription_object, msg_type, topic, callback, callback_group, qos_profile, raw, event_callbacks=event_callbacks or SubscriptionEventCallbacks()) except Exception: - subscription_handle.destroy() + subscription_object.destroy_when_not_in_use() raise self.__subscriptions.append(subscription) callback_group.add_entity(subscription) diff --git a/rclpy/rclpy/publisher.py b/rclpy/rclpy/publisher.py index eb9c31d7c..ccd41e58f 100644 --- a/rclpy/rclpy/publisher.py +++ b/rclpy/rclpy/publisher.py @@ -15,7 +15,6 @@ from typing import TypeVar, Union from rclpy.callback_groups import CallbackGroup -from rclpy.handle import Handle from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.qos import QoSProfile from rclpy.qos_event import PublisherEventCallbacks @@ -28,7 +27,7 @@ class Publisher: def __init__( self, - publisher_handle: Handle, + publisher_impl: _rclpy.Publisher, msg_type: MsgType, topic: str, qos_profile: QoSProfile, @@ -44,18 +43,18 @@ def __init__( A publisher is used as a primary means of communication in a ROS system by publishing messages on a ROS topic. - :param publisher_handle: Capsule pointing to the underlying ``rcl_publisher_t`` object. + :param publisher_impl: Publisher wrapping the underlying ``rcl_publisher_t`` object. :param msg_type: The type of ROS messages the publisher will publish. :param topic: The name of the topic the publisher will publish to. :param qos_profile: The quality of service profile to apply to the publisher. """ - self.__handle = publisher_handle + self.__publisher = publisher_impl self.msg_type = msg_type self.topic = topic self.qos_profile = qos_profile self.event_handlers: QoSEventHandler = event_callbacks.create_event_handlers( - callback_group, publisher_handle, topic) + callback_group, publisher_impl, topic) def publish(self, msg: Union[MsgType, bytes]) -> None: """ @@ -65,32 +64,32 @@ def publish(self, msg: Union[MsgType, bytes]) -> None: :raises: TypeError if the type of the passed message isn't an instance of the provided type when the publisher was constructed. """ - with self.handle as capsule: + with self.handle: if isinstance(msg, self.msg_type): - _rclpy.rclpy_publish(capsule, msg) + self.__publisher.publish(msg) elif isinstance(msg, bytes): - _rclpy.rclpy_publish_raw(capsule, msg) + self.__publisher.publish_raw(msg) else: raise TypeError('Expected {}, got {}'.format(self.msg_type, type(msg))) def get_subscription_count(self) -> int: """Get the amount of subscribers that this publisher has.""" - with self.handle as capsule: - return _rclpy.rclpy_publisher_get_subscription_count(capsule) + with self.handle: + return self.__publisher.get_subscription_count() @property def topic_name(self) -> str: - with self.handle as capsule: - return _rclpy.rclpy_publisher_get_topic_name(capsule) + with self.handle: + return self.__publisher.get_topic_name() @property def handle(self): - return self.__handle + return self.__publisher def destroy(self): for handler in self.event_handlers: handler.destroy() - self.handle.destroy() + self.__publisher.destroy_when_not_in_use() def assert_liveliness(self) -> None: """ @@ -99,5 +98,5 @@ def assert_liveliness(self) -> None: If the QoS Liveliness policy is set to MANUAL_BY_TOPIC, the application must call this at least as often as ``QoSProfile.liveliness_lease_duration``. """ - with self.handle as capsule: - _rclpy.rclpy_assert_liveliness(capsule) + with self.handle: + _rclpy.rclpy_assert_liveliness(self.handle) diff --git a/rclpy/rclpy/qos_event.py b/rclpy/rclpy/qos_event.py index f44550423..21002b72d 100644 --- a/rclpy/rclpy/qos_event.py +++ b/rclpy/rclpy/qos_event.py @@ -19,7 +19,6 @@ import rclpy from rclpy.callback_groups import CallbackGroup -from rclpy.handle import Handle from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.logging import get_logger from rclpy.qos import qos_policy_name_from_kind @@ -70,15 +69,16 @@ def __init__( callback_group: CallbackGroup, callback: Callable, event_type: IntEnum, - parent_handle: Handle, + parent_impl, ): # Waitable init adds self to callback_group super().__init__(callback_group) self.event_type = event_type self.callback = callback - with parent_handle as parent_capsule: - self.__event = _rclpy.QoSEvent(parent_capsule, event_type) + with parent_impl: + self.__event = _rclpy.QoSEvent(parent_impl, event_type) + self._ready_to_take_data = False self._event_index = None @@ -150,10 +150,10 @@ def __init__( self.use_default_callbacks = use_default_callbacks def create_event_handlers( - self, callback_group: CallbackGroup, subscription_handle: Handle, topic_name: str, + self, callback_group: CallbackGroup, subscription: _rclpy.Subscription, topic_name: str, ) -> List[QoSEventHandler]: - with subscription_handle as subscription_capsule: - logger = get_logger(_rclpy.rclpy_get_subscription_logger_name(subscription_capsule)) + with subscription: + logger = get_logger(subscription.get_logger_name()) event_handlers = [] if self.deadline: @@ -161,14 +161,14 @@ def create_event_handlers( callback_group=callback_group, callback=self.deadline, event_type=QoSSubscriptionEventType.RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED, - parent_handle=subscription_handle)) + parent_impl=subscription)) if self.incompatible_qos: event_handlers.append(QoSEventHandler( callback_group=callback_group, callback=self.incompatible_qos, event_type=QoSSubscriptionEventType.RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS, - parent_handle=subscription_handle)) + parent_impl=subscription)) elif self.use_default_callbacks: # Register default callback when not specified try: @@ -184,7 +184,7 @@ def _default_incompatible_qos_callback(event): callback_group=callback_group, callback=_default_incompatible_qos_callback, event_type=event_type, - parent_handle=subscription_handle)) + parent_impl=subscription)) except UnsupportedEventTypeError: pass @@ -194,14 +194,14 @@ def _default_incompatible_qos_callback(event): callback_group=callback_group, callback=self.liveliness, event_type=QoSSubscriptionEventType.RCL_SUBSCRIPTION_LIVELINESS_CHANGED, - parent_handle=subscription_handle)) + parent_impl=subscription)) if self.message_lost: event_handlers.append(QoSEventHandler( callback_group=callback_group, callback=self.message_lost, event_type=QoSSubscriptionEventType.RCL_SUBSCRIPTION_MESSAGE_LOST, - parent_handle=subscription_handle)) + parent_impl=subscription)) return event_handlers @@ -235,10 +235,10 @@ def __init__( self.use_default_callbacks = use_default_callbacks def create_event_handlers( - self, callback_group: CallbackGroup, publisher_handle: Handle, topic_name: str, + self, callback_group: CallbackGroup, publisher: _rclpy.Publisher, topic_name: str, ) -> List[QoSEventHandler]: - with publisher_handle as publisher_capsule: - logger = get_logger(_rclpy.rclpy_get_publisher_logger_name(publisher_capsule)) + with publisher: + logger = get_logger(publisher.get_logger_name()) event_handlers = [] if self.deadline: @@ -246,21 +246,21 @@ def create_event_handlers( callback_group=callback_group, callback=self.deadline, event_type=QoSPublisherEventType.RCL_PUBLISHER_OFFERED_DEADLINE_MISSED, - parent_handle=publisher_handle)) + parent_impl=publisher)) if self.liveliness: event_handlers.append(QoSEventHandler( callback_group=callback_group, callback=self.liveliness, event_type=QoSPublisherEventType.RCL_PUBLISHER_LIVELINESS_LOST, - parent_handle=publisher_handle)) + parent_impl=publisher)) if self.incompatible_qos: event_handlers.append(QoSEventHandler( callback_group=callback_group, callback=self.incompatible_qos, event_type=QoSPublisherEventType.RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS, - parent_handle=publisher_handle)) + parent_impl=publisher)) elif self.use_default_callbacks: # Register default callback when not specified try: @@ -275,7 +275,7 @@ def _default_incompatible_qos_callback(event): callback_group=callback_group, callback=_default_incompatible_qos_callback, event_type=QoSPublisherEventType.RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS, - parent_handle=publisher_handle)) + parent_impl=publisher)) except UnsupportedEventTypeError: pass diff --git a/rclpy/rclpy/subscription.py b/rclpy/rclpy/subscription.py index 3ad22766c..ed16d5ecb 100644 --- a/rclpy/rclpy/subscription.py +++ b/rclpy/rclpy/subscription.py @@ -16,7 +16,6 @@ from typing import TypeVar from rclpy.callback_groups import CallbackGroup -from rclpy.handle import Handle from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.qos import QoSProfile from rclpy.qos_event import QoSEventHandler @@ -31,7 +30,7 @@ class Subscription: def __init__( self, - subscription_handle: Handle, + subscription_impl: _rclpy.Subscription, msg_type: MsgType, topic: str, callback: Callable, @@ -46,8 +45,8 @@ def __init__( .. warning:: Users should not create a subscription with this constructor, instead they should call :meth:`.Node.create_subscription`. - :param subscription_handle: :class:`Handle` wrapping the underlying ``rcl_subscription_t`` - object. + :param subscription_impl: :class:`Subscription` wrapping the underlying + ``rcl_subscription_t`` object. :param msg_type: The type of ROS messages the subscription will subscribe to. :param topic: The name of the topic the subscription will subscribe to. :param callback: A user-defined callback function that is called when a message is @@ -58,7 +57,7 @@ def __init__( :param raw: If ``True``, then received messages will be stored in raw binary representation. """ - self.__handle = subscription_handle + self.__subscription = subscription_impl self.msg_type = msg_type self.topic = topic self.callback = callback @@ -69,18 +68,18 @@ def __init__( self.raw = raw self.event_handlers: QoSEventHandler = event_callbacks.create_event_handlers( - callback_group, subscription_handle, topic) + callback_group, subscription_impl, topic) @property def handle(self): - return self.__handle + return self.__subscription def destroy(self): for handler in self.event_handlers: handler.destroy() - self.handle.destroy() + self.handle.destroy_when_not_in_use() @property def topic_name(self): - with self.handle as h: - return _rclpy.rclpy_get_subscription_topic_name(h) + with self.handle: + return self.__subscription.get_topic_name() diff --git a/rclpy/src/rclpy/_rclpy_pybind11.cpp b/rclpy/src/rclpy/_rclpy_pybind11.cpp index 2fbacb1ae..ef61174a6 100644 --- a/rclpy/src/rclpy/_rclpy_pybind11.cpp +++ b/rclpy/src/rclpy/_rclpy_pybind11.cpp @@ -121,24 +121,7 @@ PYBIND11_MODULE(_rclpy_pybind11, m) { rclpy::define_duration(m); - m.def( - "rclpy_create_publisher", &rclpy::publisher_create, - "Create a Publisher"); - m.def( - "rclpy_get_publisher_logger_name", &rclpy::publisher_get_logger_name, - "Get the logger name associated with the node of a publisher."); - m.def( - "rclpy_publisher_get_subscription_count", &rclpy::publisher_get_subscription_count, - "Count subscribers from a publisher"); - m.def( - "rclpy_publisher_get_topic_name", &rclpy::publisher_get_topic_name, - "Get the resolved name(topic) of publisher"); - m.def( - "rclpy_publish", &rclpy::publisher_publish_message, - "Publish a message"); - m.def( - "rclpy_publish_raw", &rclpy::publisher_publish_raw, - "Publish a serialized message"); + rclpy::define_publisher(m); rclpy::define_service(m); @@ -157,19 +140,7 @@ PYBIND11_MODULE(_rclpy_pybind11, m) { rclpy::define_timer(m); - m.def( - "rclpy_create_subscription", &rclpy::subscription_create, - "Create a Subscription"); - m.def( - "rclpy_get_subscription_logger_name", &rclpy::subscription_get_logger_name, - "Get the logger name associated with the node of a subscription"); - m.def( - "rclpy_get_subscription_topic_name", &rclpy::subscription_get_topic_name, - "Get the topic name of a subscription"); - m.def( - "rclpy_take", &rclpy::subscription_take_message, - "Take a message and its metadata from a subscription"); - + rclpy::define_subscription(m); rclpy::define_time_point(m); rclpy::define_clock(m); @@ -191,6 +162,9 @@ PYBIND11_MODULE(_rclpy_pybind11, m) { 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."); diff --git a/rclpy/src/rclpy/client.cpp b/rclpy/src/rclpy/client.cpp index 8587f527d..d277dee71 100644 --- a/rclpy/src/rclpy/client.cpp +++ b/rclpy/src/rclpy/client.cpp @@ -155,7 +155,7 @@ Client::take_response(py::object pyresponse_type) void define_client(py::object module) { - py::class_(module, "Client") + py::class_>(module, "Client") .def(py::init()) .def_property_readonly( "pointer", [](const Client & client) { diff --git a/rclpy/src/rclpy/client.hpp b/rclpy/src/rclpy/client.hpp index f9f5a9f34..8c7621a8d 100644 --- a/rclpy/src/rclpy/client.hpp +++ b/rclpy/src/rclpy/client.hpp @@ -28,7 +28,7 @@ namespace py = pybind11; namespace rclpy { -class Client : public Destroyable +class Client : public Destroyable, public std::enable_shared_from_this { public: /// Create a client diff --git a/rclpy/src/rclpy/clock.cpp b/rclpy/src/rclpy/clock.cpp index bf91c84c7..f74947997 100644 --- a/rclpy/src/rclpy/clock.cpp +++ b/rclpy/src/rclpy/clock.cpp @@ -181,7 +181,7 @@ void Clock::remove_clock_callback(py::object pyjump_handle) void define_clock(py::object module) { - py::class_(module, "Clock") + py::class_>(module, "Clock") .def(py::init()) .def_property_readonly( "pointer", [](const Clock & clock) { diff --git a/rclpy/src/rclpy/clock.hpp b/rclpy/src/rclpy/clock.hpp index 06bd5076b..c55682dff 100644 --- a/rclpy/src/rclpy/clock.hpp +++ b/rclpy/src/rclpy/clock.hpp @@ -31,7 +31,7 @@ namespace py = pybind11; namespace rclpy { -class Clock : public Destroyable +class Clock : public Destroyable, public std::enable_shared_from_this { public: /// Create a clock @@ -105,12 +105,6 @@ class Clock : public Destroyable void remove_clock_callback(py::object pyjump_handle); - /// Get rcl_client_t pointer - std::shared_ptr get_shared_ptr() - { - return rcl_clock_; - } - /// Get rcl_client_t pointer rcl_clock_t * rcl_ptr() const { diff --git a/rclpy/src/rclpy/destroyable.cpp b/rclpy/src/rclpy/destroyable.cpp index fa6ced3a1..fc8fda849 100644 --- a/rclpy/src/rclpy/destroyable.cpp +++ b/rclpy/src/rclpy/destroyable.cpp @@ -62,7 +62,7 @@ Destroyable::destroy_when_not_in_use() void define_destroyable(py::object module) { - py::class_(module, "Destroyable") + py::class_>(module, "Destroyable") .def("__enter__", &Destroyable::enter) .def("__exit__", &Destroyable::exit) .def( diff --git a/rclpy/src/rclpy/publisher.cpp b/rclpy/src/rclpy/publisher.cpp index 5a73882fd..c9caf916c 100644 --- a/rclpy/src/rclpy/publisher.cpp +++ b/rclpy/src/rclpy/publisher.cpp @@ -29,39 +29,12 @@ namespace rclpy { -static void -_rclpy_destroy_publisher(void * p) -{ - auto pub = static_cast(p); - if (!pub) { - // Warning should use line number of the current stack frame - int stack_level = 1; - PyErr_WarnFormat( - PyExc_RuntimeWarning, stack_level, "_rclpy_destroy_publisher got NULL pointer"); - return; - } - - rcl_ret_t ret = rcl_publisher_fini(&(pub->publisher), pub->node); - 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 publisher: %s", - rcl_get_error_string().str); - } - PyMem_Free(pub); -} - -py::capsule -publisher_create( +Publisher::Publisher( py::capsule pynode, py::object pymsg_type, std::string topic, py::object pyqos_profile) +: node_handle_(std::make_shared(pynode)) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } + auto node = node_handle_->cast("rcl_node_t"); auto msg_type = static_cast( rclpy_common_get_type_support(pymsg_type.ptr())); @@ -75,22 +48,31 @@ publisher_create( publisher_ops.qos = pyqos_profile.cast(); } - // Use smart pointer to make sure memory is free'd on error - auto deleter = [](rclpy_publisher_t * ptr) {_rclpy_destroy_publisher(ptr);}; - auto pub = std::unique_ptr( - static_cast(PyMem_Malloc(sizeof(rclpy_publisher_t))), - deleter); - if (!pub) { - throw std::bad_alloc(); - } - pub->publisher = rcl_get_zero_initialized_publisher(); - pub->node = node; + rcl_publisher_ = std::shared_ptr( + new rcl_publisher_t, + [this](rcl_publisher_t * publisher) + { + auto node = node_handle_->cast_or_warn("rcl_node_t"); + + rcl_ret_t ret = rcl_publisher_fini(publisher, node); + 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 publisher: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + delete publisher; + }); + + *rcl_publisher_ = rcl_get_zero_initialized_publisher(); rcl_ret_t ret = rcl_publisher_init( - &(pub->publisher), node, msg_type, + rcl_publisher_.get(), node, msg_type, topic.c_str(), &publisher_ops); - if (ret != RCL_RET_OK) { - if (ret == RCL_RET_TOPIC_NAME_INVALID) { + if (RCL_RET_OK != ret) { + if (RCL_RET_TOPIC_NAME_INVALID == ret) { std::string error_text{"Failed to create publisher due to invalid topic name '"}; error_text += topic; error_text += "'"; @@ -98,37 +80,20 @@ publisher_create( } throw RCLError("Failed to create publisher"); } +} - PyObject * pypub_c = - rclpy_create_handle_capsule(pub.get(), "rclpy_publisher_t", _rclpy_destroy_publisher); - if (!pypub_c) { - throw py::error_already_set(); - } - auto pypub = py::reinterpret_steal(pypub_c); - // pypub now owns the rclpy_publisher_t - pub.release(); - - auto pub_handle = static_cast(pypub); - auto node_handle = static_cast(pynode); - _rclpy_handle_add_dependency(pub_handle, node_handle); - if (PyErr_Occurred()) { - _rclpy_handle_dec_ref(pub_handle); - throw py::error_already_set(); - } - - return pypub; +void Publisher::destroy() +{ + rcl_publisher_.reset(); + node_handle_.reset(); } const char * -publisher_get_logger_name(py::capsule pypublisher) +Publisher::get_logger_name() { - auto pub = static_cast( - rclpy_handle_get_pointer_from_capsule(pypublisher.ptr(), "rclpy_publisher_t")); - if (!pub) { - throw py::error_already_set(); - } + auto node = node_handle_->cast("rcl_node_t"); - const char * node_logger_name = rcl_node_get_logger_name(pub->node); + const char * node_logger_name = rcl_node_get_logger_name(node); if (!node_logger_name) { throw RCLError("Node logger name not set"); } @@ -137,17 +102,11 @@ publisher_get_logger_name(py::capsule pypublisher) } size_t -publisher_get_subscription_count(py::capsule pypublisher) +Publisher::get_subscription_count() { - auto pub = static_cast( - rclpy_handle_get_pointer_from_capsule(pypublisher.ptr(), "rclpy_publisher_t")); - if (!pub) { - throw py::error_already_set(); - } - size_t count = 0; - rcl_ret_t ret = rcl_publisher_get_subscription_count(&pub->publisher, &count); - if (ret != RCL_RET_OK) { + rcl_ret_t ret = rcl_publisher_get_subscription_count(rcl_publisher_.get(), &count); + if (RCL_RET_OK != ret) { throw RCLError("failed to get subscription count"); } @@ -155,15 +114,9 @@ publisher_get_subscription_count(py::capsule pypublisher) } std::string -publisher_get_topic_name(py::capsule pypublisher) +Publisher::get_topic_name() { - auto pub = static_cast( - rclpy_handle_get_pointer_from_capsule(pypublisher.ptr(), "rclpy_publisher_t")); - if (!pub) { - throw py::error_already_set(); - } - - const char * topic_name = rcl_publisher_get_topic_name(&pub->publisher); + const char * topic_name = rcl_publisher_get_topic_name(rcl_publisher_.get()); if (!topic_name) { throw RCLError("failed to get topic name"); } @@ -172,44 +125,59 @@ publisher_get_topic_name(py::capsule pypublisher) } void -publisher_publish_message(py::capsule pypublisher, py::object pymsg) +Publisher::publish(py::object pymsg) { - auto pub = static_cast( - rclpy_handle_get_pointer_from_capsule(pypublisher.ptr(), "rclpy_publisher_t")); - if (!pub) { - throw py::error_already_set(); - } - destroy_ros_message_signature * destroy_ros_message = NULL; void * raw_ros_message = rclpy_convert_from_py(pymsg.ptr(), &destroy_ros_message); if (!raw_ros_message) { throw py::error_already_set(); } - rcl_ret_t ret = rcl_publish(&(pub->publisher), raw_ros_message, NULL); + rcl_ret_t ret = rcl_publish(rcl_publisher_.get(), raw_ros_message, NULL); destroy_ros_message(raw_ros_message); - if (ret != RCL_RET_OK) { + if (RCL_RET_OK != ret) { throw RCLError("Failed to publish"); } } void -publisher_publish_raw(py::capsule pypublisher, std::string msg) +Publisher::publish_raw(std::string msg) { - auto pub = static_cast( - rclpy_handle_get_pointer_from_capsule(pypublisher.ptr(), "rclpy_publisher_t")); - if (!pub) { - throw py::error_already_set(); - } - rcl_serialized_message_t serialized_msg = rmw_get_zero_initialized_serialized_message(); serialized_msg.buffer_capacity = msg.size(); serialized_msg.buffer_length = msg.size(); serialized_msg.buffer = reinterpret_cast(const_cast(msg.c_str())); - rcl_ret_t ret = rcl_publish_serialized_message(&(pub->publisher), &serialized_msg, NULL); - if (ret != RCL_RET_OK) { + rcl_ret_t ret = rcl_publish_serialized_message(rcl_publisher_.get(), &serialized_msg, NULL); + if (RCL_RET_OK != ret) { throw RCLError("Failed to publish"); } } + +void +define_publisher(py::object module) +{ + py::class_>(module, "Publisher") + .def(py::init()) + .def_property_readonly( + "pointer", [](const Publisher & publisher) { + return reinterpret_cast(publisher.rcl_ptr()); + }, + "Get the address of the entity as an integer") + .def( + "get_logger_name", &Publisher::get_logger_name, + "Get the name of the logger associated with the node of the publisher") + .def( + "get_subscription_count", &Publisher::get_subscription_count, + "Count subscribers from a publisher.") + .def( + "get_topic_name", &Publisher::get_topic_name, + "Retrieve the topic name from a Publisher.") + .def( + "publish", &Publisher::publish, + "Publish a message") + .def( + "publish_raw", &Publisher::publish_raw, + "Publish a serialized message."); +} } // namespace rclpy diff --git a/rclpy/src/rclpy/publisher.hpp b/rclpy/src/rclpy/publisher.hpp index a42bfb377..1c5949d43 100644 --- a/rclpy/src/rclpy/publisher.hpp +++ b/rclpy/src/rclpy/publisher.hpp @@ -17,87 +17,104 @@ #include +#include +#include + +#include #include +#include "destroyable.hpp" +#include "handle.hpp" + namespace py = pybind11; namespace rclpy { -/// Create a publisher /** - * This function will create a publisher and attach it to the provided topic name + * This class will create a publisher and attach it to the provided topic name * This publisher will use the typesupport defined in the message module - * provided as pymsg_type to send messages over the wire. - * - * Raises ValueError if the topic name is invalid - * Raises ValueError if the capsules are not the correct types - * Raises RCLError if the publisher cannot be created - * - * \param[in] pynode Capsule pointing to the node to add the publisher to - * \param[in] pymsg_type Message type associated with the publisher - * \param[in] topic The name of the topic to attach the publisher to - * \param[in] pyqos_profile rmw_qos_profile_t object for this publisher - * \return Capsule of the pointer to the created rcl_publisher_t * structure, or + * provided as pymsg_type to send messages. */ -py::capsule -publisher_create( - py::capsule pynode, py::object pymsg_type, std::string topic, - py::object pyqos_profile); +class Publisher : public Destroyable, public std::enable_shared_from_this +{ +public: + /// Create a publisher + /* + * Raises ValueError if the topic name is invalid + * Raises ValueError if the capsules are not the correct types + * Raises RCLError if the publisher cannot be created + * + * \param[in] pynode Capsule pointing to the node to add the publisher to + * \param[in] pymsg_type Message type associated with the publisher + * \param[in] topic The name of the topic to attach the publisher to + * \param[in] pyqos_profile rmw_qos_profile_t object for this publisher + */ + Publisher( + py::capsule pynode, py::object pymsg_type, std::string topic, + py::object pyqos_profile); -/// Get the name of the logger associated with the node of the publisher. -/** - * Raises ValueError if pypublisher is not a publisher capsule - * Raises RCLError if logger name not set - * - * \param[in] pypublisher Capsule pointing to the publisher - * \return logger_name of pypublisher - */ -const char * -publisher_get_logger_name(py::capsule pypublisher); + /// Get the name of the logger associated with the node of the publisher. + /** + * Raises RCLError if logger name not set + * + * \return the logger name + */ + const char * + get_logger_name(); -/// Count subscribers from a publisher. -/** - * Raise ValueError if capsule is not a publisher - * Raises RCLError if the subscriber count cannot be determined - * - * \param[in] pypublisher Capsule pointing to the publisher - * \return count of subscribers - */ -size_t -publisher_get_subscription_count(py::capsule pypublisher); + /// Count subscribers from a publisher. + /** + * Raises RCLError if the subscriber count cannot be determined + * + * \return number of subscribers + */ + size_t + get_subscription_count(); -/// Retrieve the topic name from a rclpy_publisher_t -/** - * Raise ValueError if capsule is not a publisher - * Raises RCLError if the name cannot be determined - * - * \param[in] pypublisher Capsule pointing to the publisher - * \return name of the publisher's topic - */ -std::string -publisher_get_topic_name(py::capsule pypublisher); + /// Retrieve the topic name from a rclpy_publisher_t + /** + * Raises RCLError if the name cannot be determined + * + * \return name of the publisher's topic + */ + std::string + get_topic_name(); -/// Publish a message -/** - * Raises ValueError if pypublisher is not a publisher capsule - * Raises RCLError if the message cannot be published - * - * \param[in] pypublisher Capsule pointing to the publisher - * \param[in] pymsg Message to send - */ -void -publisher_publish_message(py::capsule pypublisher, py::object pymsg); + /// Publish a message + /** + * Raises RCLError if the message cannot be published + * + * \param[in] pymsg Message to send + */ + void + publish(py::object pymsg); -/// Publish a serialized message -/** - * Raises ValueError if pypublisher is not a publisher capsule - * Raises RCLError if the message cannot be published - * - * \param[in] pypublisher Capsule pointing to the publisher - * \param[in] msg serialized message to send - */ -void -publisher_publish_raw(py::capsule pypublisher, std::string msg); + /// Publish a serialized message + /** + * Raises RCLError if the message cannot be published + * + * \param[in] msg serialized message to send + */ + void + publish_raw(std::string msg); + + /// Get rcl_client_t pointer + rcl_publisher_t * + rcl_ptr() const + { + return rcl_publisher_.get(); + } + + /// Force an early destruction of this object + void + destroy() override; + +private: + std::shared_ptr node_handle_; + std::shared_ptr rcl_publisher_; +}; +/// Define a pybind11 wrapper for an rclpy::Service +void define_publisher(py::object module); } // namespace rclpy #endif // RCLPY__PUBLISHER_HPP_ diff --git a/rclpy/src/rclpy/qos_event.cpp b/rclpy/src/rclpy/qos_event.cpp index ffd4c798c..d22d24e06 100644 --- a/rclpy/src/rclpy/qos_event.cpp +++ b/rclpy/src/rclpy/qos_event.cpp @@ -57,21 +57,22 @@ create_zero_initialized_event() void QoSEvent::destroy() { + grandparent_pub_handle_.reset(); + grandparent_sub_handle_.reset(); rcl_event_.reset(); - parent_handle_.reset(); } QoSEvent::QoSEvent( - py::capsule pysubscription, rcl_subscription_event_type_t event_type) -: parent_handle_(std::make_shared(pysubscription)), event_type_(event_type) + rclpy::Subscription & subscription, rcl_subscription_event_type_t event_type) +: event_type_(event_type) { - auto wrapper = parent_handle_->cast("rclpy_subscription_t"); + grandparent_sub_handle_ = subscription.shared_from_this(); // Create a subscription event rcl_event_ = create_zero_initialized_event(); rcl_ret_t ret = rcl_subscription_event_init( - rcl_event_.get(), &(wrapper->subscription), event_type); + rcl_event_.get(), grandparent_sub_handle_->rcl_ptr(), event_type); if (RCL_RET_BAD_ALLOC == ret) { rcl_reset_error(); throw std::bad_alloc(); @@ -85,16 +86,16 @@ QoSEvent::QoSEvent( } QoSEvent::QoSEvent( - py::capsule pypublisher, rcl_publisher_event_type_t event_type) -: parent_handle_(std::make_shared(pypublisher)), event_type_(event_type) + rclpy::Publisher & publisher, rcl_publisher_event_type_t event_type) +: event_type_(event_type) { - auto wrapper = parent_handle_->cast("rclpy_publisher_t"); + grandparent_pub_handle_ = publisher.shared_from_this(); // Create a publisher event rcl_event_ = create_zero_initialized_event(); rcl_ret_t ret = rcl_publisher_event_init( - rcl_event_.get(), &(wrapper->publisher), event_type); + rcl_event_.get(), grandparent_pub_handle_->rcl_ptr(), event_type); if (RCL_RET_BAD_ALLOC == ret) { rcl_reset_error(); throw std::bad_alloc(); @@ -168,9 +169,9 @@ QoSEvent::take_event() void define_qos_event(py::module module) { - py::class_(module, "QoSEvent") - .def(py::init()) - .def(py::init()) + py::class_>(module, "QoSEvent") + .def(py::init()) + .def(py::init()) .def_property_readonly( "pointer", [](const QoSEvent & event) { return reinterpret_cast(event.rcl_ptr()); diff --git a/rclpy/src/rclpy/qos_event.hpp b/rclpy/src/rclpy/qos_event.hpp index 7c886758c..1c218a220 100644 --- a/rclpy/src/rclpy/qos_event.hpp +++ b/rclpy/src/rclpy/qos_event.hpp @@ -24,27 +24,30 @@ #include "destroyable.hpp" #include "handle.hpp" +#include "publisher.hpp" +#include "subscription.hpp" namespace py = pybind11; namespace rclpy { -class QoSEvent : public Destroyable +/* + * This class will create an event handle for the given subscription. + */ +class QoSEvent : public Destroyable, public std::enable_shared_from_this { public: /// Create a subscription event /** - * This function will create an event handle for the given subscription. - * * Raises UnsupportedEventTypeError if the event type is not supported * Raises TypeError if arguments are not of the correct types i.e. a subscription capsule * Raises MemoryError if the event can't be allocated * Raises RCLError if event initialization failed in rcl * - * \param[in] pysubscription Capsule containing the subscription + * \param[in] ssubscription Subscription wrapping the underlying ``rcl_subscription_t`` object. * \param[in] event_type Type of event to create */ - QoSEvent(py::capsule pysubscription, rcl_subscription_event_type_t event_type); + QoSEvent(rclpy::Subscription & subscriber, rcl_subscription_event_type_t event_type); /// Create a publisher event /** @@ -55,10 +58,10 @@ class QoSEvent : public Destroyable * Raises MemoryError if the event can't be allocated * Raises RCLError if event initialization failed in rcl * - * \param[in] pypublisher Capsule containing the publisher + * \param[in] publisher Publisher wrapping the underlying ``rcl_publisher_t`` object. * \param[in] event_type Type of event to create */ - QoSEvent(py::capsule pypublisher, rcl_publisher_event_type_t event_type); + QoSEvent(rclpy::Publisher & publisher, rcl_publisher_event_type_t event_type); ~QoSEvent() = default; @@ -88,7 +91,8 @@ class QoSEvent : public Destroyable destroy() override; private: - std::shared_ptr parent_handle_; + std::shared_ptr grandparent_pub_handle_; + std::shared_ptr grandparent_sub_handle_; std::shared_ptr rcl_event_; std::variant event_type_; }; diff --git a/rclpy/src/rclpy/service.cpp b/rclpy/src/rclpy/service.cpp index 1002fbf73..c8cbf6efb 100644 --- a/rclpy/src/rclpy/service.cpp +++ b/rclpy/src/rclpy/service.cpp @@ -139,7 +139,7 @@ Service::service_take_request(py::object pyrequest_type) void define_service(py::object module) { - py::class_(module, "Service") + py::class_>(module, "Service") .def(py::init()) .def_property_readonly( "pointer", [](const Service & service) { diff --git a/rclpy/src/rclpy/service.hpp b/rclpy/src/rclpy/service.hpp index ec87880c7..5e300d684 100644 --- a/rclpy/src/rclpy/service.hpp +++ b/rclpy/src/rclpy/service.hpp @@ -33,7 +33,7 @@ namespace py = pybind11; namespace rclpy { -class Service : public Destroyable +class Service : public Destroyable, public std::enable_shared_from_this { public: /// Create a service server diff --git a/rclpy/src/rclpy/subscription.cpp b/rclpy/src/rclpy/subscription.cpp index 6a20d8f26..41dfa2d52 100644 --- a/rclpy/src/rclpy/subscription.cpp +++ b/rclpy/src/rclpy/subscription.cpp @@ -35,39 +35,12 @@ using pybind11::literals::operator""_a; namespace rclpy { -static void -_rclpy_destroy_subscription(void * p) -{ - auto sub = static_cast(p); - if (!sub) { - // Warning should use line number of the current stack frame - int stack_level = 1; - PyErr_WarnFormat( - PyExc_RuntimeWarning, stack_level, "_rclpy_destroy_subscription got NULL pointer"); - return; - } - - rcl_ret_t ret = rcl_subscription_fini(&(sub->subscription), sub->node); - 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 subscription: %s", - rcl_get_error_string().str); - } - PyMem_Free(sub); -} - -py::capsule -subscription_create( +Subscription::Subscription( py::capsule pynode, py::object pymsg_type, std::string topic, py::object pyqos_profile) +: node_handle_(std::make_shared(pynode)) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } + auto node = node_handle_->cast("rcl_node_t"); auto msg_type = static_cast( rclpy_common_get_type_support(pymsg_type.ptr())); @@ -81,19 +54,28 @@ subscription_create( subscription_ops.qos = pyqos_profile.cast(); } - // Use smart pointer to make sure memory is free'd on error - auto deleter = [](rclpy_subscription_t * ptr) {_rclpy_destroy_subscription(ptr);}; - auto sub = std::unique_ptr( - static_cast(PyMem_Malloc(sizeof(rclpy_subscription_t))), - deleter); - if (!sub) { - throw std::bad_alloc(); - } - sub->subscription = rcl_get_zero_initialized_subscription(); - sub->node = node; + rcl_subscription_ = std::shared_ptr( + new rcl_subscription_t, + [this](rcl_subscription_t * subscription) + { + auto node = node_handle_->cast_or_warn("rcl_node_t"); + + rcl_ret_t ret = rcl_subscription_fini(subscription, node); + 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 subscription: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + delete subscription; + }); + + *rcl_subscription_ = rcl_get_zero_initialized_subscription(); rcl_ret_t ret = rcl_subscription_init( - &(sub->subscription), node, msg_type, + rcl_subscription_.get(), node, msg_type, topic.c_str(), &subscription_ops); if (ret != RCL_RET_OK) { if (ret == RCL_RET_TOPIC_NAME_INVALID) { @@ -104,41 +86,23 @@ subscription_create( } throw RCLError("Failed to create subscription"); } +} - PyObject * pysub_c = - rclpy_create_handle_capsule(sub.get(), "rclpy_subscription_t", _rclpy_destroy_subscription); - if (!pysub_c) { - throw py::error_already_set(); - } - auto pysub = py::reinterpret_steal(pysub_c); - // pysub now owns the rclpy_subscription_t - sub.release(); - - auto sub_handle = static_cast(pysub); - auto node_handle = static_cast(pynode); - _rclpy_handle_add_dependency(sub_handle, node_handle); - if (PyErr_Occurred()) { - throw py::error_already_set(); - } - - return pysub; +void Subscription::destroy() +{ + rcl_subscription_.reset(); + node_handle_.reset(); } py::object -subscription_take_message(py::capsule pysubscription, py::object pymsg_type, bool raw) +Subscription::take_message(py::object pymsg_type, bool raw) { - auto wrapper = static_cast( - rclpy_handle_get_pointer_from_capsule(pysubscription.ptr(), "rclpy_subscription_t")); - if (!wrapper) { - throw py::error_already_set(); - } - py::object pytaken_msg; rmw_message_info_t message_info; if (raw) { SerializedMessage taken{rcutils_get_default_allocator()}; rcl_ret_t ret = rcl_take_serialized_message( - &(wrapper->subscription), &taken.rcl_msg, &message_info, NULL); + rcl_subscription_.get(), &taken.rcl_msg, &message_info, NULL); if (RCL_RET_OK != ret) { if (RCL_RET_BAD_ALLOC == ret) { rcl_reset_error(); @@ -156,7 +120,7 @@ subscription_take_message(py::capsule pysubscription, py::object pymsg_type, boo auto taken_msg = create_from_py(pymsg_type); rcl_ret_t ret = rcl_take( - &(wrapper->subscription), taken_msg.get(), &message_info, NULL); + rcl_subscription_.get(), taken_msg.get(), &message_info, NULL); if (RCL_RET_OK != ret) { if (RCL_RET_BAD_ALLOC == ret) { rcl_reset_error(); @@ -177,37 +141,47 @@ subscription_take_message(py::capsule pysubscription, py::object pymsg_type, boo "received_timestamp"_a = message_info.received_timestamp)); } -py::object -subscription_get_logger_name(py::capsule pysubscription) +const char * +Subscription::get_logger_name() { - auto sub = static_cast( - rclpy_handle_get_pointer_from_capsule(pysubscription.ptr(), "rclpy_subscription_t")); - if (!sub) { - throw py::error_already_set(); - } + auto node = node_handle_->cast("rcl_node_t"); - const char * node_logger_name = rcl_node_get_logger_name(sub->node); - if (nullptr == node_logger_name) { - return py::none(); + const char * node_logger_name = rcl_node_get_logger_name(node); + if (!node_logger_name) { + throw RCLError("Node logger name not set"); } - return py::str(node_logger_name); + return node_logger_name; } std::string -subscription_get_topic_name(py::capsule pysubscription) +Subscription::get_topic_name() { - auto sub = static_cast( - rclpy_handle_get_pointer_from_capsule(pysubscription.ptr(), "rclpy_subscription_t")); - if (!sub) { - throw py::error_already_set(); - } - - const char * subscription_name = rcl_subscription_get_topic_name(&(sub->subscription)); + const char * subscription_name = rcl_subscription_get_topic_name(rcl_subscription_.get()); if (nullptr == subscription_name) { throw RCLError("failed to get subscription topic name"); } return std::string(subscription_name); } +void +define_subscription(py::object module) +{ + py::class_>(module, "Subscription") + .def(py::init()) + .def_property_readonly( + "pointer", [](const Subscription & subscription) { + return reinterpret_cast(subscription.rcl_ptr()); + }, + "Get the address of the entity as an integer") + .def( + "take_message", &Subscription::take_message, + "Take a message and its metadata from a subscription") + .def( + "get_logger_name", &Subscription::get_logger_name, + "Get the name of the logger associated with the node of the subscription.") + .def( + "get_topic_name", &Subscription::get_topic_name, + "Return the resolved topic name of a subscription."); +} } // namespace rclpy diff --git a/rclpy/src/rclpy/subscription.hpp b/rclpy/src/rclpy/subscription.hpp index 5c31aa757..2f27e85e9 100644 --- a/rclpy/src/rclpy/subscription.hpp +++ b/rclpy/src/rclpy/subscription.hpp @@ -17,74 +17,92 @@ #include +#include +#include + +#include #include +#include "destroyable.hpp" +#include "handle.hpp" + namespace py = pybind11; namespace rclpy { /// Create a subscription /** - * This function will create a subscription for the given topic name. + * This class will create a subscription for the given topic name. * This subscription will use the typesupport defined in the message module - * provided as pymsg_type to send messages over the wire. - * - * On a successful call a Capsule pointing to the pointer of the created - * rcl_subscription_t * structure is returned - * - * Raises ValueError if the capsules are not the correct types - * Raises RCLError if the subscription could not be created - * - * \param[in] pynode Capsule pointing to the node to add the subscriber to - * \param[in] pymsg_type Message module associated with the subscriber - * \param[in] topic The topic name - * \param[in] pyqos_profile rmw_qos_profile_t object for this subscription - * \return capsule containing the rclpy_subscription_t + * provided as pymsg_type to send messages. */ -py::capsule -subscription_create( - py::capsule pynode, py::object pymsg_type, std::string topic, - py::object pyqos_profile); +class Subscription : public Destroyable, public std::enable_shared_from_this +{ +public: + /// Create a subscription + /* + * Raises RCLError if the subscription could not be created + * + * \param[in] pynode Capsule pointing to the node to add the subscriber to + * \param[in] pymsg_type Message module associated with the subscriber + * \param[in] topic The topic name + * \param[in] pyqos_profile rmw_qos_profile_t object for this subscription + */ + Subscription( + py::capsule pynode, py::object pymsg_type, std::string topic, + py::object pyqos_profile); -/// Take a message and its metadata from a subscription -/** - * Raises ValueError if pysubscription is not subscription capsule - * Raises MemoryError if there was error allocating memory - * Raises RCLError if there was an error within rcl - * - * \param[in] pysubscription Capsule pointing to the subscription. - * \param[in] pymsg_type Message type to be taken (i.e. a class). - * \param[in] raw If True, return the message without de-serializing it. - * \return Tuple of (message, metadata) or None if there was no message to take. - * Message is a \p pymsg_type instance if \p raw is True, otherwise a byte string. - * Metadata is a plain dictionary. - */ -py::object -subscription_take_message(py::capsule pysubscription, py::object pymsg_type, bool raw); + /// Take a message and its metadata from a subscription + /** + * Raises MemoryError if there was an error allocating memory + * Raises RCLError if there was an error within rcl + * + * \param[in] pymsg_type Message type to be taken (i.e. a class). + * \param[in] raw If True, return the message without de-serializing it. + * \return Tuple of (message, metadata) or None if there was no message to take. + * Message is a \p pymsg_type instance if \p raw is True, otherwise a byte string. + * Metadata is a plain dictionary. + */ + py::object + take_message(py::object pymsg_type, bool raw); -/// Get the name of the logger associated with the node of the subscription. -/** - * Raises ValueError if pysubscription is not a subscription capsule - * - * \param[in] pysubscription Capsule pointing to the subscription to get the logger name of - * \return logger_name, or - * \return None on failure - */ -py::object -subscription_get_logger_name(py::capsule pysubscription); + /// Get the name of the logger associated with the node of the subscription. + /** + * + * \return logger_name, or + * \return None on failure + */ + const char * + get_logger_name(); -/// Return the resolved topic name of a subscription. -/** - * The returned string is the resolved topic name after remappings have be applied. - * - * Raises ValueError if pysubscription is not a subscription capsule - * Raises RCLError if the topic name could not be determined - * - * \param[in] pysubscription Capsule pointing to the subscription to get the topic name of - * \return a string with the topic name - */ -std::string -subscription_get_topic_name(py::capsule pysubscription); + /// Return the resolved topic name of a subscription. + /** + * The returned string is the resolved topic name after remappings have be applied. + * + * Raises RCLError if the topic name could not be determined + * + * \return a string with the topic name + */ + std::string + get_topic_name(); + + /// Get rcl_client_t pointer + rcl_subscription_t * + rcl_ptr() const + { + return rcl_subscription_.get(); + } + + /// Force an early destruction of this object + void + destroy() override; + +private: + std::shared_ptr node_handle_; + std::shared_ptr rcl_subscription_; +}; +/// Define a pybind11 wrapper for an rclpy::Service +void define_subscription(py::object module); } // namespace rclpy #endif // RCLPY__SUBSCRIPTION_HPP_ diff --git a/rclpy/src/rclpy/timer.cpp b/rclpy/src/rclpy/timer.cpp index b4e192e57..997c3fb06 100644 --- a/rclpy/src/rclpy/timer.cpp +++ b/rclpy/src/rclpy/timer.cpp @@ -43,7 +43,7 @@ Timer::destroy() Timer::Timer( Clock & rclcy_clock, py::capsule pycontext, int64_t period_nsec) { - clock_handle_ = rclcy_clock.get_shared_ptr(); + clock_handle_ = rclcy_clock.shared_from_this(); auto context = static_cast( rclpy_handle_get_pointer_from_capsule(pycontext.ptr(), "rcl_context_t")); @@ -72,7 +72,7 @@ Timer::Timer( rcl_allocator_t allocator = rcl_get_default_allocator(); rcl_ret_t ret = rcl_timer_init( - rcl_timer_.get(), clock_handle_.get(), context, + rcl_timer_.get(), clock_handle_->rcl_ptr(), context, period_nsec, NULL, allocator); if (RCL_RET_OK != ret) { @@ -168,7 +168,7 @@ bool Timer::is_timer_canceled() void define_timer(py::object module) { - py::class_(module, "Timer") + py::class_>(module, "Timer") .def(py::init()) .def_property_readonly( "pointer", [](const Timer & timer) { diff --git a/rclpy/src/rclpy/timer.hpp b/rclpy/src/rclpy/timer.hpp index 57e5dde7e..786322804 100644 --- a/rclpy/src/rclpy/timer.hpp +++ b/rclpy/src/rclpy/timer.hpp @@ -30,7 +30,7 @@ namespace py = pybind11; namespace rclpy { -class Timer : public Destroyable +class Timer : public Destroyable, public std::enable_shared_from_this { public: /// Create a timer @@ -133,8 +133,7 @@ class Timer : public Destroyable void destroy() override; private: - // TODO(ahcorde) replace with std::shared_ptr when rclpy::Clock exists - std::shared_ptr clock_handle_; + std::shared_ptr clock_handle_; std::shared_ptr rcl_timer_; }; diff --git a/rclpy/src/rclpy/utils.cpp b/rclpy/src/rclpy/utils.cpp index 8eb013c87..4aca238e8 100644 --- a/rclpy/src/rclpy/utils.cpp +++ b/rclpy/src/rclpy/utils.cpp @@ -104,20 +104,10 @@ get_rmw_implementation_identifier() } void -assert_liveliness(py::capsule pyentity) +assert_liveliness(rclpy::Publisher * publisher) { - if (0 == strcmp("rclpy_publisher_t", pyentity.name())) { - auto publisher = static_cast( - rclpy_handle_get_pointer_from_capsule( - pyentity.ptr(), "rclpy_publisher_t")); - if (nullptr == publisher) { - throw py::error_already_set(); - } - if (RCL_RET_OK != rcl_publisher_assert_liveliness(&publisher->publisher)) { - throw RCLError("Failed to assert liveliness on the Publisher"); - } - } else { - throw py::type_error("Passed capsule is not a valid Publisher."); + if (RCL_RET_OK != rcl_publisher_assert_liveliness(publisher->rcl_ptr())) { + throw RCLError("Failed to assert liveliness on the Publisher"); } } diff --git a/rclpy/src/rclpy/utils.hpp b/rclpy/src/rclpy/utils.hpp index 58fa0b24a..de86d697e 100644 --- a/rclpy/src/rclpy/utils.hpp +++ b/rclpy/src/rclpy/utils.hpp @@ -21,6 +21,8 @@ #include +#include "publisher.hpp" + namespace py = pybind11; namespace rclpy @@ -78,7 +80,7 @@ get_rmw_implementation_identifier(); * \return None */ void -assert_liveliness(py::capsule pyentity); +assert_liveliness(rclpy::Publisher * publisher); /// Remove ROS specific args from a list of args. /** diff --git a/rclpy/src/rclpy/wait_set.cpp b/rclpy/src/rclpy/wait_set.cpp index bc15e432c..0bf3a58f1 100644 --- a/rclpy/src/rclpy/wait_set.cpp +++ b/rclpy/src/rclpy/wait_set.cpp @@ -128,14 +128,7 @@ wait_set_add_entity(const std::string & entity_type, py::capsule pywait_set, py: rcl_ret_t ret = RCL_RET_ERROR; size_t index; - if ("subscription" == entity_type) { - auto sub = static_cast( - rclpy_handle_get_pointer_from_capsule(pyentity.ptr(), "rclpy_subscription_t")); - if (!sub) { - throw py::error_already_set(); - } - ret = rcl_wait_set_add_subscription(wait_set, &(sub->subscription), &index); - } else if ("guard_condition" == entity_type) { + if ("guard_condition" == entity_type) { auto guard_condition = static_cast( rclpy_handle_get_pointer_from_capsule(pyentity.ptr(), "rcl_guard_condition_t")); if (!guard_condition) { @@ -173,6 +166,22 @@ wait_set_add_service(const py::capsule pywait_set, const Service & service) return index; } +size_t +wait_set_add_subscription(const py::capsule pywait_set, 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); + if (RCL_RET_OK != ret) { + throw RCLError("failed to add subscription to wait set"); + } + return index; +} + size_t wait_set_add_timer(const py::capsule pywait_set, const Timer & timer) { diff --git a/rclpy/src/rclpy/wait_set.hpp b/rclpy/src/rclpy/wait_set.hpp index f73052560..5cd1b5f84 100644 --- a/rclpy/src/rclpy/wait_set.hpp +++ b/rclpy/src/rclpy/wait_set.hpp @@ -23,6 +23,7 @@ #include "client.hpp" #include "qos_event.hpp" #include "service.hpp" +#include "subscription.hpp" #include "timer.hpp" namespace py = pybind11; @@ -90,6 +91,17 @@ 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); +/// 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 diff --git a/rclpy/src/rclpy_common/include/rclpy_common/common.h b/rclpy/src/rclpy_common/include/rclpy_common/common.h index a5ba6f069..1f048f7f6 100644 --- a/rclpy/src/rclpy_common/include/rclpy_common/common.h +++ b/rclpy/src/rclpy_common/include/rclpy_common/common.h @@ -32,21 +32,6 @@ typedef void destroy_ros_message_signature (void *); typedef bool convert_from_py_signature (PyObject *, void *); typedef PyObject * convert_to_py_signature (void *); -typedef struct -{ - // Important: a pointer to a structure is also a pointer to its first member. - // The subscription must be first in the struct to compare sub.handle.pointer to an address - // in a wait set. - rcl_subscription_t subscription; - rcl_node_t * node; -} rclpy_subscription_t; - -typedef struct -{ - rcl_publisher_t publisher; - rcl_node_t * node; -} rclpy_publisher_t; - typedef struct { // Important: a pointer to a structure is also a pointer to its first member. diff --git a/rclpy/test/test_destruction.py b/rclpy/test/test_destruction.py index c647424c5..08e3266e0 100644 --- a/rclpy/test/test_destruction.py +++ b/rclpy/test/test_destruction.py @@ -111,11 +111,7 @@ def test_destroy_subscription_asap(): with sub.handle: pass - with sub.handle: - node.destroy_subscription(sub) - # handle valid because it's still being used - with sub.handle: - pass + node.destroy_subscription(sub) with pytest.raises(InvalidHandle): # handle invalid because it was destroyed when no one was using it @@ -160,11 +156,7 @@ def test_destroy_publisher_asap(): with pub.handle: pass - with pub.handle: - node.destroy_publisher(pub) - # handle valid because it's still being used - with pub.handle: - pass + node.destroy_publisher(pub) with pytest.raises(InvalidHandle): # handle invalid because it was destroyed when no one was using it diff --git a/rclpy/test/test_node.py b/rclpy/test/test_node.py index 9f94ce323..aab804711 100644 --- a/rclpy/test/test_node.py +++ b/rclpy/test/test_node.py @@ -40,7 +40,6 @@ from rclpy.exceptions import ParameterImmutableException from rclpy.exceptions import ParameterNotDeclaredException from rclpy.executors import SingleThreadedExecutor -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.parameter import Parameter from rclpy.qos import qos_profile_sensor_data from rclpy.qos import QoSDurabilityPolicy @@ -155,8 +154,8 @@ def test_take(self): basic_types_pub.publish(basic_types_msg) cycle_count = 0 while cycle_count < 5: - with sub.handle as capsule: - result = _rclpy.rclpy_take(capsule, sub.msg_type, False) + with sub.handle: + result = sub.handle.take_message(sub.msg_type, False) if result is not None: msg, info = result self.assertNotEqual(0, info['source_timestamp']) diff --git a/rclpy/test/test_qos_event.py b/rclpy/test/test_qos_event.py index 447b9ba62..8435f141b 100644 --- a/rclpy/test/test_qos_event.py +++ b/rclpy/test/test_qos_event.py @@ -196,8 +196,8 @@ def warn(self, message, once=False): rclpy.logging._root_logger = original_logger def _create_event_handle(self, parent_entity, event_type): - with parent_entity.handle as parent_capsule: - event = _rclpy.QoSEvent(parent_capsule, event_type) + with parent_entity.handle: + event = _rclpy.QoSEvent(parent_entity.handle, event_type) self.assertIsNotNone(event) return event diff --git a/rclpy/test/test_waitable.py b/rclpy/test/test_waitable.py index 7e15fa2d6..135d36dda 100644 --- a/rclpy/test/test_waitable.py +++ b/rclpy/test/test_waitable.py @@ -174,7 +174,7 @@ def __init__(self, node): super().__init__(ReentrantCallbackGroup()) with node.handle as node_capsule: - self.subscription = _rclpy.rclpy_create_subscription( + self.subscription = _rclpy.Subscription( node_capsule, EmptyMsg, 'test_topic', QoSProfile(depth=10).get_c_qos_profile()) self.subscription_index = None self.subscription_is_ready = False @@ -192,7 +192,7 @@ def take_data(self): """Take stuff from lower level so the wait set doesn't immediately wake again.""" if self.subscription_is_ready: self.subscription_is_ready = False - msg_info = _rclpy.rclpy_take(self.subscription, EmptyMsg, False) + msg_info = self.subscription.take_message(EmptyMsg, False) if msg_info is not None: return msg_info[0] return None @@ -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_entity( - 'subscription', wait_set, self.subscription) + self.subscription_index = _rclpy.rclpy_wait_set_add_subscription( + wait_set, self.subscription) class GuardConditionWaitable(Waitable):