From 2a4eac4c3b5090d8cea9c1dd3535bd130ca103cb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 8 Apr 2021 08:17:55 +0200 Subject: [PATCH 01/10] Convert Publisher and Subscription to use C++ Classes (#756) * Convert Publisher and Subscription to C++ Classes Signed-off-by: ahcorde * Fixed destructor issues Signed-off-by: ahcorde * [pub/sub 756 addition] use std::shared_ptr holder type and shared_from_this (#757) * Use std::shared_ptr holder type Signed-off-by: Shane Loretz * Use shared_from_this for pub/sub Signed-off-by: Shane Loretz * Make Timer keep Clock wrapper Signed-off-by: Shane Loretz * Fixed publisher and subscription destructors Signed-off-by: ahcorde * Added feedback Signed-off-by: ahcorde * Fixed docs Signed-off-by: ahcorde Co-authored-by: Shane Loretz --- rclpy/rclpy/executors.py | 13 +- rclpy/rclpy/node.py | 14 +- rclpy/rclpy/publisher.py | 31 ++-- rclpy/rclpy/qos_event.py | 38 ++-- rclpy/rclpy/subscription.py | 19 +- rclpy/src/rclpy/_rclpy_pybind11.cpp | 36 +--- rclpy/src/rclpy/client.cpp | 2 +- rclpy/src/rclpy/client.hpp | 2 +- rclpy/src/rclpy/clock.cpp | 2 +- rclpy/src/rclpy/clock.hpp | 8 +- rclpy/src/rclpy/destroyable.cpp | 2 +- rclpy/src/rclpy/publisher.cpp | 174 +++++++----------- rclpy/src/rclpy/publisher.hpp | 151 ++++++++------- rclpy/src/rclpy/qos_event.cpp | 25 +-- rclpy/src/rclpy/qos_event.hpp | 20 +- rclpy/src/rclpy/service.cpp | 2 +- rclpy/src/rclpy/service.hpp | 2 +- rclpy/src/rclpy/subscription.cpp | 146 ++++++--------- rclpy/src/rclpy/subscription.hpp | 128 +++++++------ rclpy/src/rclpy/timer.cpp | 6 +- rclpy/src/rclpy/timer.hpp | 5 +- rclpy/src/rclpy/utils.cpp | 16 +- rclpy/src/rclpy/utils.hpp | 4 +- rclpy/src/rclpy/wait_set.cpp | 25 ++- rclpy/src/rclpy/wait_set.hpp | 12 ++ .../include/rclpy_common/common.h | 15 -- rclpy/test/test_destruction.py | 12 +- rclpy/test/test_node.py | 5 +- rclpy/test/test_qos_event.py | 4 +- rclpy/test/test_waitable.py | 8 +- 30 files changed, 431 insertions(+), 496 deletions(-) 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): From 62886577b4e6692c6a01099d6ea90a46871372eb Mon Sep 17 00:00:00 2001 From: Greg Balke <11024792+gbalke@users.noreply.github.com> Date: Thu, 8 Apr 2021 12:54:57 -0700 Subject: [PATCH 02/10] Use py::class_ for rcl_action_goal_handle_t (#751) * Class object Signed-off-by: Greg Balke * Python adjustments Signed-off-by: Greg Balke * Adjustments to server and enum Signed-off-by: Greg Balke * Adjust destructor calls Signed-off-by: Greg Balke * Clear out destructor Signed-off-by: Greg Balke * Do what rclcpp did Signed-off-by: Greg Balke * Remove iostream (used for debugging) Signed-off-by: Greg Balke * Matching upstream changes Signed-off-by: Greg Balke --- rclpy/CMakeLists.txt | 1 + rclpy/rclpy/action/server.py | 45 +++----- rclpy/src/rclpy/_rclpy_action.cpp | 122 ++-------------------- rclpy/src/rclpy/_rclpy_pybind11.cpp | 8 ++ rclpy/src/rclpy/action_goal_handle.cpp | 138 +++++++++++++++++++++++++ rclpy/src/rclpy/action_goal_handle.hpp | 85 +++++++++++++++ 6 files changed, 255 insertions(+), 144 deletions(-) create mode 100644 rclpy/src/rclpy/action_goal_handle.cpp create mode 100644 rclpy/src/rclpy/action_goal_handle.hpp diff --git a/rclpy/CMakeLists.txt b/rclpy/CMakeLists.txt index 9d0379dec..c1798b5cb 100644 --- a/rclpy/CMakeLists.txt +++ b/rclpy/CMakeLists.txt @@ -153,6 +153,7 @@ pybind11_add_module(_rclpy_pybind11 SHARED src/rclpy/_rclpy_logging.cpp src/rclpy/_rclpy_pybind11.cpp src/rclpy/_rclpy_pycapsule.cpp + src/rclpy/action_goal_handle.cpp src/rclpy/client.cpp src/rclpy/clock.cpp src/rclpy/context.cpp diff --git a/rclpy/rclpy/action/server.py b/rclpy/rclpy/action/server.py index 7df498bfa..aa27f9160 100644 --- a/rclpy/rclpy/action/server.py +++ b/rclpy/rclpy/action/server.py @@ -43,16 +43,6 @@ class CancelResponse(Enum): ACCEPT = 2 -class GoalEvent(Enum): - """Goal events that cause state transitions.""" - - EXECUTE = 1 - CANCEL_GOAL = 2 - SUCCEED = 3 - ABORT = 4 - CANCELED = 5 - - class ServerGoalHandle: """Goal handle for working with Action Servers.""" @@ -69,8 +59,7 @@ def __init__(self, action_server, goal_info, goal_request): :param goal_info: GoalInfo message. :param goal_request: The user defined goal request message from an ActionClient. """ - self._handle = _rclpy.rclpy_action_accept_new_goal( - action_server._handle, goal_info) + self._goal = _rclpy.ActionGoalHandle(action_server._handle, goal_info) self._action_server = action_server self._goal_info = goal_info self._goal_request = goal_request @@ -96,9 +85,9 @@ def goal_id(self): @property def is_active(self): with self._lock: - if self._handle is None: + if self._goal is None: return False - return _rclpy.rclpy_action_goal_handle_is_active(self._handle) + return self._goal.is_active() @property def is_cancel_requested(self): @@ -107,24 +96,24 @@ def is_cancel_requested(self): @property def status(self): with self._lock: - if self._handle is None: + if self._goal is None: return GoalStatus.STATUS_UNKNOWN - return _rclpy.rclpy_action_goal_handle_get_status(self._handle) + return self._goal.get_status() def _update_state(self, event): with self._lock: # Ignore updates for already destructed goal handles - if self._handle is None: + if self._goal is None: return # Update state - _rclpy.rclpy_action_update_goal_state(self._handle, event.value) + self._goal.update_goal_state(event) # Publish state change _rclpy.rclpy_action_publish_status(self._action_server._handle) # If it's a terminal state, then also notify the action server - if not _rclpy.rclpy_action_goal_handle_is_active(self._handle): + if not self._goal.is_active(): self._action_server.notify_goal_done() def execute(self, execute_callback=None): @@ -132,7 +121,7 @@ def execute(self, execute_callback=None): # In this case we want to avoid the illegal state transition to EXECUTING # but still call the users execute callback to let them handle canceling the goal. if not self.is_cancel_requested: - self._update_state(GoalEvent.EXECUTE) + self._update_state(_rclpy.GoalEvent.EXECUTE) self._action_server.notify_execute(self, execute_callback) def publish_feedback(self, feedback): @@ -141,7 +130,7 @@ def publish_feedback(self, feedback): with self._lock: # Ignore for already destructed goal handles - if self._handle is None: + if self._goal is None: return # Populate the feedback message with metadata about this goal @@ -155,20 +144,20 @@ def publish_feedback(self, feedback): self._action_server._handle, feedback_message) def succeed(self): - self._update_state(GoalEvent.SUCCEED) + self._update_state(_rclpy.GoalEvent.SUCCEED) def abort(self): - self._update_state(GoalEvent.ABORT) + self._update_state(_rclpy.GoalEvent.ABORT) def canceled(self): - self._update_state(GoalEvent.CANCELED) + self._update_state(_rclpy.GoalEvent.CANCELED) def destroy(self): with self._lock: - if self._handle is None: + if self._goal is None: return - _rclpy.rclpy_action_destroy_server_goal_handle(self._handle) - self._handle = None + self._goal.destroy_when_not_in_use() + self._goal = None self._action_server.remove_future(self._result_future) @@ -375,7 +364,7 @@ async def _execute_cancel_request(self, request_header_and_message): if CancelResponse.ACCEPT == response: # Notify goal handle - goal_handle._update_state(GoalEvent.CANCEL_GOAL) + goal_handle._update_state(_rclpy.GoalEvent.CANCEL_GOAL) else: # Remove from response cancel_response.goals_canceling.remove(goal_info) diff --git a/rclpy/src/rclpy/_rclpy_action.cpp b/rclpy/src/rclpy/_rclpy_action.cpp index d105ad6f0..cb239147d 100644 --- a/rclpy/src/rclpy/_rclpy_action.cpp +++ b/rclpy/src/rclpy/_rclpy_action.cpp @@ -27,6 +27,7 @@ #include "rclpy_common/handle.h" #include "clock.hpp" +#include "action_goal_handle.hpp" namespace py = pybind11; @@ -90,22 +91,6 @@ rclpy_action_destroy_entity(py::capsule pyentity, py::capsule pynode) } } -void -rclpy_action_destroy_server_goal_handle(py::capsule pygoal_handle) -{ - if (strcmp("rcl_action_goal_handle_t", pygoal_handle.name())) { - throw py::value_error("Capsule must be an rcl_action_goal_handle_t"); - } - - auto goal_handle = - get_pointer(pygoal_handle, "rcl_action_goal_handle_t"); - - rcl_ret_t ret = rcl_action_goal_handle_fini(goal_handle); - if (RCL_RET_OK != ret) { - throw rclpy::RCLError("Error destroying action goal handle"); - } -} - /// Fetch a predefined qos_profile from rcl_action and convert it to a Python QoSProfile object. /** * Raises RuntimeError if the QoS profile is unknown. @@ -915,27 +900,10 @@ rclpy_action_take_status(py::capsule pyaction_client, py::object pymsg_type) TAKE_MESSAGE(status) } -py::capsule +rclpy::ActionGoalHandle rclpy_action_accept_new_goal(py::capsule pyaction_server, py::object pygoal_info_msg) { - auto action_server = get_pointer(pyaction_server, "rcl_action_server_t"); - destroy_ros_message_signature * destroy_ros_message = NULL; - auto goal_info_msg = static_cast( - rclpy_convert_from_py(pygoal_info_msg.ptr(), &destroy_ros_message)); - if (!goal_info_msg) { - throw py::error_already_set(); - } - - auto goal_info_msg_ptr = std::unique_ptr( - goal_info_msg, destroy_ros_message); - - rcl_action_goal_handle_t * goal_handle = rcl_action_accept_new_goal( - action_server, goal_info_msg); - if (!goal_handle) { - throw rclpy::RCLError("Failed to accept new goal"); - } - // TODO(sloretz) capsule destructor instead of rclpy_action_destroy_server_goal_handle() - return py::capsule(goal_handle, "rcl_action_goal_handle_t"); + return rclpy::ActionGoalHandle(pyaction_server, pygoal_info_msg); } void @@ -948,60 +916,6 @@ rclpy_action_notify_goal_done(py::capsule pyaction_server) } } -/// Convert from a Python GoalEvent code to an rcl goal event code. -/** - * Raises std::runtime_error if conversion fails - * - * \param[in] pyevent The Python GoalEvent code. - * \return The rcl equivalent of the Python GoalEvent code - */ -static rcl_action_goal_event_t -convert_from_py_goal_event(const int64_t event) -{ - py::module server_module = py::module::import("rclpy.action.server"); - py::object goal_event_class = server_module.attr("GoalEvent"); - py::int_ pyevent(event); - - if (goal_event_class.attr("EXECUTE").attr("value").cast().is(pyevent)) { - return GOAL_EVENT_EXECUTE; - } - if (goal_event_class.attr("CANCEL_GOAL").attr("value").cast().is(pyevent)) { - return GOAL_EVENT_CANCEL_GOAL; - } - if (goal_event_class.attr("SUCCEED").attr("value").cast().is(pyevent)) { - return GOAL_EVENT_SUCCEED; - } - if (goal_event_class.attr("ABORT").attr("value").cast().is(pyevent)) { - return GOAL_EVENT_ABORT; - } - if (goal_event_class.attr("CANCELED").attr("value").cast().is(pyevent)) { - return GOAL_EVENT_CANCELED; - } - throw std::runtime_error("Error converting goal event type: unknown goal event"); -} - -void -rclpy_action_update_goal_state(py::capsule pygoal_handle, int64_t pyevent) -{ - auto goal_handle = get_pointer( - pygoal_handle, "rcl_action_goal_handle_t"); - - rcl_action_goal_event_t event = convert_from_py_goal_event(pyevent); - - rcl_ret_t ret = rcl_action_update_goal_state(goal_handle, event); - if (RCL_RET_OK != ret) { - throw rclpy::RCLError("Failed to update goal state"); - } -} - -bool -rclpy_action_goal_handle_is_active(py::capsule pygoal_handle) -{ - auto goal_handle = get_pointer( - pygoal_handle, "rcl_action_goal_handle_t"); - return rcl_action_goal_handle_is_active(goal_handle); -} - bool rclpy_action_server_goal_exists(py::capsule pyaction_server, py::object pygoal_info) { @@ -1019,21 +933,6 @@ rclpy_action_server_goal_exists(py::capsule pyaction_server, py::object pygoal_i return rcl_action_server_goal_exists(action_server, goal_info); } -rcl_action_goal_state_t -rclpy_action_goal_handle_get_status(py::capsule pygoal_handle) -{ - auto goal_handle = get_pointer( - pygoal_handle, "rcl_action_goal_handle_t"); - - rcl_action_goal_state_t status; - rcl_ret_t ret = rcl_action_goal_handle_get_status(goal_handle, &status); - if (RCL_RET_OK != ret) { - throw rclpy::RCLError("Failed to get goal status"); - } - - return status; -} - py::object rclpy_action_process_cancel_request( py::capsule pyaction_server, py::object pycancel_request, py::object pycancel_response_type) @@ -1209,9 +1108,6 @@ define_action_api(py::module m) m.def( "rclpy_action_destroy_entity", &rclpy_action_destroy_entity, "Destroy a rclpy_action entity."); - m.def( - "rclpy_action_destroy_server_goal_handle", &rclpy_action_destroy_server_goal_handle, - "Destroy a ServerGoalHandle."); m.def( "rclpy_action_get_rmw_qos_profile", &rclpy_action_get_rmw_qos_profile, "Get an action RMW QoS profile."); @@ -1287,18 +1183,12 @@ define_action_api(py::module m) m.def( "rclpy_action_notify_goal_done", &rclpy_action_notify_goal_done, "Notify and action server that a goal has reached a terminal state."); - m.def( - "rclpy_action_update_goal_state", &rclpy_action_update_goal_state, - "Update a goal state."); - m.def( - "rclpy_action_goal_handle_is_active", &rclpy_action_goal_handle_is_active, - "Check if a goal is active."); + + define_action_goal_handle(m); + m.def( "rclpy_action_server_goal_exists", &rclpy_action_server_goal_exists, "Check if a goal being tracked by an action server."); - m.def( - "rclpy_action_goal_handle_get_status", &rclpy_action_goal_handle_get_status, - "Get the status of a goal."); m.def( "rclpy_action_process_cancel_request", &rclpy_action_process_cancel_request, "Process a cancel request to determine what goals should be canceled."); diff --git a/rclpy/src/rclpy/_rclpy_pybind11.cpp b/rclpy/src/rclpy/_rclpy_pybind11.cpp index ef61174a6..478fe44e1 100644 --- a/rclpy/src/rclpy/_rclpy_pybind11.cpp +++ b/rclpy/src/rclpy/_rclpy_pybind11.cpp @@ -15,6 +15,7 @@ #include #include +#include #include "action_api.hpp" #include "client.hpp" @@ -57,6 +58,13 @@ PYBIND11_MODULE(_rclpy_pybind11, m) { .value("SYSTEM_TIME", RCL_SYSTEM_TIME) .value("STEADY_TIME", RCL_STEADY_TIME); + py::enum_(m, "GoalEvent") + .value("EXECUTE", GOAL_EVENT_EXECUTE) + .value("CANCEL_GOAL", GOAL_EVENT_CANCEL_GOAL) + .value("SUCCEED", GOAL_EVENT_SUCCEED) + .value("ABORT", GOAL_EVENT_ABORT) + .value("CANCELED", GOAL_EVENT_CANCELED); + m.attr("RCL_DEFAULT_DOMAIN_ID") = py::int_(RCL_DEFAULT_DOMAIN_ID); py::enum_(m, "ClockChange") diff --git a/rclpy/src/rclpy/action_goal_handle.cpp b/rclpy/src/rclpy/action_goal_handle.cpp new file mode 100644 index 000000000..9ca6b840e --- /dev/null +++ b/rclpy/src/rclpy/action_goal_handle.cpp @@ -0,0 +1,138 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include +#include + +#include "rclpy_common/common.h" +#include "rclpy_common/exceptions.hpp" + +#include "utils.hpp" +#include "action_goal_handle.hpp" + +namespace py = pybind11; + +namespace rclpy +{ +template +T +get_pointer(py::capsule & capsule, const char * name) +{ + if (strcmp(name, capsule.name())) { + std::string error_text{"Expected capusle with name '"}; + error_text += name; + error_text += "' but got '"; + error_text += capsule.name(); + error_text += "'"; + throw py::value_error(error_text); + } + // TODO(sloretz) use get_pointer() in pybind11 2.6+ + return static_cast(capsule); +} + +ActionGoalHandle::ActionGoalHandle( + py::capsule pyaction_server, py::object pygoal_info_msg) +{ + auto action_server = get_pointer( + pyaction_server, "rcl_action_server_t"); + + destroy_ros_message_signature * destroy_ros_message = NULL; + auto goal_info_msg = static_cast( + rclpy_convert_from_py(pygoal_info_msg.ptr(), &destroy_ros_message)); + + if (!goal_info_msg) { + throw py::error_already_set(); + } + + auto goal_info_msg_ptr = std::unique_ptr( + goal_info_msg, destroy_ros_message); + + auto rcl_handle = rcl_action_accept_new_goal(action_server, goal_info_msg); + if (!rcl_handle) { + throw rclpy::RCLError("Failed to accept new goal"); + } + + rcl_action_goal_handle_ = std::shared_ptr( + new rcl_action_goal_handle_t, + [](rcl_action_goal_handle_t * goal_handle) + { + rcl_ret_t ret = rcl_action_goal_handle_fini(goal_handle); + 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, "Error destroying action goal handle: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + delete goal_handle; + }); + // Copy out goal handle since action server storage disappears when it is fini'd + *rcl_action_goal_handle_ = *rcl_handle; +} + +void +ActionGoalHandle::destroy() +{ + rcl_action_goal_handle_.reset(); +} + +rcl_action_goal_state_t +ActionGoalHandle::get_status() +{ + rcl_action_goal_state_t status; + rcl_ret_t ret = rcl_action_goal_handle_get_status(rcl_action_goal_handle_.get(), &status); + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed to get goal status"); + } + + return status; +} + +void +ActionGoalHandle::update_goal_state(rcl_action_goal_event_t event) +{ + rcl_ret_t ret = rcl_action_update_goal_state(rcl_action_goal_handle_.get(), event); + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed to update goal state"); + } +} + +void +define_action_goal_handle(py::module module) +{ + py::class_>( + module, + "ActionGoalHandle") + .def(py::init()) + .def_property_readonly( + "pointer", [](const ActionGoalHandle & handle) { + return reinterpret_cast(handle.rcl_ptr()); + }, + "Get the address of the entity as an integer") + .def( + "get_status", &ActionGoalHandle::get_status, + "Get the status of a goal.") + .def( + "update_goal_state", &ActionGoalHandle::update_goal_state, + "Update a goal state.") + .def( + "is_active", &ActionGoalHandle::is_active, + "Check if a goal is active."); +} +} // namespace rclpy diff --git a/rclpy/src/rclpy/action_goal_handle.hpp b/rclpy/src/rclpy/action_goal_handle.hpp new file mode 100644 index 000000000..e2c054a35 --- /dev/null +++ b/rclpy/src/rclpy/action_goal_handle.hpp @@ -0,0 +1,85 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLPY__ACTION_GOAL_HANDLE_HPP_ +#define RCLPY__ACTION_GOAL_HANDLE_HPP_ + +#include + +#include + +#include + +#include "destroyable.hpp" +#include "handle.hpp" + +namespace py = pybind11; + +namespace rclpy +{ +class ActionGoalHandle : public Destroyable +{ +public: + /// Create an action goal handle + /** + * This function will create an action goal handle for the given info message from the action server. + * This action goal handle will use the typesupport defined in the service module + * provided as pysrv_type to send messages. + * + * Raises RCLError if the action goal handle could not be created + * + * \param[in] pyaction_server handle to the action server that is accepting the goal + * \param[in] pygoal_info_msg a message containing info about the goal being accepted + */ + ActionGoalHandle(py::capsule pyaction_server, py::object pygoal_info_msg); + + ~ActionGoalHandle() = default; + + rcl_action_goal_state_t + get_status(); + + void + update_goal_state(rcl_action_goal_event_t event); + + /// Check if the goal is still active + bool + is_active() + { + return rcl_action_goal_handle_is_active(rcl_ptr()); + } + + /// Get rcl_action goal handle_t pointer + rcl_action_goal_handle_t * + rcl_ptr() const + { + return rcl_action_goal_handle_.get(); + } + + /// Force an early destruction of this object + void + destroy() override; + +private: + std::shared_ptr rcl_action_goal_handle_; +}; + +/// Define a pybind11 wrapper for an rclpy::ActionGoalHandle +/** + * \param[in] module a pybind11 module to add the definition to + */ +void +define_action_goal_handle(py::module module); +} // namespace rclpy + +#endif // RCLPY__ACTION_GOAL_HANDLE_HPP_ From 1ab8af41aaa8b085d011b0a695b7ee7ffe800f68 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 12 Apr 2021 17:13:00 +0200 Subject: [PATCH 03/10] Convert ActionClient to use C++ classes (#759) * Convert ActionClient to use C++ classes Signed-off-by: ahcorde * Added feedback Signed-off-by: ahcorde * Added convert_from_py to utils Signed-off-by: ahcorde * improved docs Signed-off-by: ahcorde * nits to adapt the code to pybind11 Signed-off-by: ahcorde * throw a exception in convert_from_py Signed-off-by: ahcorde * Fixed docs Signed-off-by: ahcorde --- rclpy/CMakeLists.txt | 1 + rclpy/rclpy/action/client.py | 51 ++--- rclpy/src/rclpy/_rclpy_action.cpp | 252 +---------------------- rclpy/src/rclpy/action_client.cpp | 330 ++++++++++++++++++++++++++++++ rclpy/src/rclpy/action_client.hpp | 238 +++++++++++++++++++++ rclpy/src/rclpy/utils.cpp | 25 +++ rclpy/src/rclpy/utils.hpp | 10 + 7 files changed, 632 insertions(+), 275 deletions(-) create mode 100644 rclpy/src/rclpy/action_client.cpp create mode 100644 rclpy/src/rclpy/action_client.hpp diff --git a/rclpy/CMakeLists.txt b/rclpy/CMakeLists.txt index c1798b5cb..7d1f80c84 100644 --- a/rclpy/CMakeLists.txt +++ b/rclpy/CMakeLists.txt @@ -153,6 +153,7 @@ pybind11_add_module(_rclpy_pybind11 SHARED src/rclpy/_rclpy_logging.cpp src/rclpy/_rclpy_pybind11.cpp src/rclpy/_rclpy_pycapsule.cpp + src/rclpy/action_client.cpp src/rclpy/action_goal_handle.cpp src/rclpy/client.cpp src/rclpy/clock.cpp diff --git a/rclpy/rclpy/action/client.py b/rclpy/rclpy/action/client.py index 0532ea24b..22ca2d79f 100644 --- a/rclpy/rclpy/action/client.py +++ b/rclpy/rclpy/action/client.py @@ -150,7 +150,7 @@ def __init__( self._action_type = action_type self._action_name = action_name with node.handle as node_capsule: - self._client_handle = _rclpy.rclpy_action_create_client( + self._client_handle = _rclpy.ActionClient( node_capsule, action_type, action_name, @@ -220,9 +220,7 @@ def _remove_pending_result_request(self, future): # Start Waitable API def is_ready(self, wait_set): """Return True if one or more entities are ready in the wait set.""" - ready_entities = _rclpy.rclpy_action_wait_set_is_ready( - self._client_handle, - wait_set) + ready_entities = self._client_handle.is_ready(wait_set) self._is_feedback_ready = ready_entities[0] self._is_status_ready = ready_entities[1] self._is_goal_response_ready = ready_entities[2] @@ -234,36 +232,36 @@ def take_data(self): """Take stuff from lower level so the wait set doesn't immediately wake again.""" data = {} if self._is_goal_response_ready: - taken_data = _rclpy.rclpy_action_take_goal_response( - self._client_handle, self._action_type.Impl.SendGoalService.Response) + taken_data = self._client_handle.take_goal_response( + self._action_type.Impl.SendGoalService.Response) # If take fails, then we get (None, None) if all(taken_data): data['goal'] = taken_data if self._is_cancel_response_ready: - taken_data = _rclpy.rclpy_action_take_cancel_response( - self._client_handle, self._action_type.Impl.CancelGoalService.Response) + taken_data = self._client_handle.take_cancel_response( + self._action_type.Impl.CancelGoalService.Response) # If take fails, then we get (None, None) if all(taken_data): data['cancel'] = taken_data if self._is_result_response_ready: - taken_data = _rclpy.rclpy_action_take_result_response( - self._client_handle, self._action_type.Impl.GetResultService.Response) + taken_data = self._client_handle.take_result_response( + self._action_type.Impl.GetResultService.Response) # If take fails, then we get (None, None) if all(taken_data): data['result'] = taken_data if self._is_feedback_ready: - taken_data = _rclpy.rclpy_action_take_feedback( - self._client_handle, self._action_type.Impl.FeedbackMessage) + taken_data = self._client_handle.take_feedback( + self._action_type.Impl.FeedbackMessage) # If take fails, then we get None if taken_data is not None: data['feedback'] = taken_data if self._is_status_ready: - taken_data = _rclpy.rclpy_action_take_status( - self._client_handle, self._action_type.Impl.GoalStatusMessage) + taken_data = self._client_handle.take_status( + self._action_type.Impl.GoalStatusMessage) # If take fails, then we get None if taken_data is not None: data['status'] = taken_data @@ -347,12 +345,12 @@ async def execute(self, taken_data): def get_num_entities(self): """Return number of each type of entity used in the wait set.""" - num_entities = _rclpy.rclpy_action_wait_set_get_num_entities(self._client_handle) + num_entities = self._client_handle.get_num_entities() return NumberOfEntities(*num_entities) def add_to_wait_set(self, wait_set): """Add entities to wait set.""" - _rclpy.rclpy_action_wait_set_add(self._client_handle, wait_set) + self._client_handle.add_to_waitset(wait_set) # End Waitable API def send_goal(self, goal, **kwargs): @@ -423,8 +421,7 @@ def send_goal_async(self, goal, feedback_callback=None, goal_uuid=None): request = self._action_type.Impl.SendGoalService.Request() request.goal_id = self._generate_random_uuid() if goal_uuid is None else goal_uuid request.goal = goal - sequence_number = _rclpy.rclpy_action_send_goal_request( - self._client_handle, request) + sequence_number = self._client_handle.send_goal_request(request) if sequence_number in self._pending_goal_requests: raise RuntimeError( 'Sequence ({}) conflicts with pending goal request'.format(sequence_number)) @@ -482,9 +479,7 @@ def _cancel_goal_async(self, goal_handle): cancel_request = CancelGoal.Request() cancel_request.goal_info.goal_id = goal_handle.goal_id - sequence_number = _rclpy.rclpy_action_send_cancel_request( - self._client_handle, - cancel_request) + sequence_number = self._client_handle.send_cancel_request(cancel_request) if sequence_number in self._pending_cancel_requests: raise RuntimeError( 'Sequence ({}) conflicts with pending cancel request'.format(sequence_number)) @@ -536,9 +531,7 @@ def _get_result_async(self, goal_handle): result_request = self._action_type.Impl.GetResultService.Request() result_request.goal_id = goal_handle.goal_id - sequence_number = _rclpy.rclpy_action_send_result_request( - self._client_handle, - result_request) + sequence_number = self._client_handle.send_result_request(result_request) if sequence_number in self._pending_result_requests: raise RuntimeError( 'Sequence ({}) conflicts with pending result request'.format(sequence_number)) @@ -557,10 +550,8 @@ def server_is_ready(self): :return: True if an action server is ready, False otherwise. """ - with self._node.handle as node_capsule: - return _rclpy.rclpy_action_server_is_available( - node_capsule, - self._client_handle) + with self._node.handle: + return self._client_handle.is_action_server_available() def wait_for_server(self, timeout_sec=None): """ @@ -587,8 +578,8 @@ def destroy(self): """Destroy the underlying action client handle.""" if self._client_handle is None: return - with self._node.handle as node_capsule: - _rclpy.rclpy_action_destroy_entity(self._client_handle, node_capsule) + with self._node.handle: + self._client_handle.destroy_when_not_in_use() self._node.remove_waitable(self) self._client_handle = None diff --git a/rclpy/src/rclpy/_rclpy_action.cpp b/rclpy/src/rclpy/_rclpy_action.cpp index cb239147d..2f55d1856 100644 --- a/rclpy/src/rclpy/_rclpy_action.cpp +++ b/rclpy/src/rclpy/_rclpy_action.cpp @@ -26,8 +26,9 @@ #include "rclpy_common/common.h" #include "rclpy_common/handle.h" -#include "clock.hpp" +#include "action_client.hpp" #include "action_goal_handle.hpp" +#include "clock.hpp" namespace py = pybind11; @@ -66,11 +67,7 @@ rclpy_action_destroy_entity(py::capsule pyentity, py::capsule pynode) } rcl_ret_t ret; - if (0 == std::strcmp("rcl_action_client_t", pyentity.name())) { - auto action_client = get_pointer(pyentity, "rcl_action_client_t"); - ret = rcl_action_client_fini(action_client, node); - PyMem_Free(action_client); - } else if (0 == std::strcmp("rcl_action_server_t", pyentity.name())) { + if (0 == std::strcmp("rcl_action_server_t", pyentity.name())) { auto action_server = get_pointer(pyentity, "rcl_action_server_t"); ret = rcl_action_server_fini(action_server, node); PyMem_Free(action_server); @@ -127,10 +124,7 @@ rclpy_action_wait_set_add(py::capsule pyentity, py::capsule pywait_set) auto wait_set = get_pointer(pywait_set, "rcl_wait_set_t"); rcl_ret_t ret; - if (0 == strcmp(pyentity.name(), "rcl_action_client_t")) { - auto action_client = get_pointer(pyentity, "rcl_action_client_t"); - ret = rcl_action_wait_set_add_action_client(wait_set, action_client, NULL, NULL); - } else if (0 == strcmp(pyentity.name(), "rcl_action_server_t")) { + if (0 == strcmp(pyentity.name(), "rcl_action_server_t")) { auto action_server = get_pointer(pyentity, "rcl_action_server_t"); ret = rcl_action_wait_set_add_action_server(wait_set, action_server, NULL); } else { @@ -168,17 +162,7 @@ rclpy_action_wait_set_get_num_entities(py::capsule pyentity) size_t num_services = 0u; rcl_ret_t ret; - if (0 == strcmp(pyentity.name(), "rcl_action_client_t")) { - auto action_client = get_pointer(pyentity, "rcl_action_client_t"); - - ret = rcl_action_client_wait_set_get_num_entities( - action_client, - &num_subscriptions, - &num_guard_conditions, - &num_timers, - &num_clients, - &num_services); - } else if (0 == strcmp(pyentity.name(), "rcl_action_server_t")) { + if (0 == strcmp(pyentity.name(), "rcl_action_server_t")) { auto action_server = get_pointer(pyentity, "rcl_action_server_t"); ret = rcl_action_server_wait_set_get_num_entities( @@ -503,22 +487,6 @@ rclpy_action_server_is_available(py::capsule pynode, py::capsule pyaction_client return is_available; } -#define SEND_SERVICE_REQUEST(Type) \ - auto action_client = get_pointer(pyaction_client, "rcl_action_client_t"); \ - destroy_ros_message_signature * destroy_ros_message = NULL; \ - void * raw_ros_request = rclpy_convert_from_py(pyrequest.ptr(), &destroy_ros_message); \ - if (!raw_ros_request) { \ - throw py::error_already_set(); \ - } \ - int64_t sequence_number; \ - rcl_ret_t ret = rcl_action_send_ ## Type ## _request( \ - action_client, raw_ros_request, &sequence_number); \ - destroy_ros_message(raw_ros_request); \ - if (ret != RCL_RET_OK) { \ - throw rclpy::RCLError("Failed to send " #Type " request"); \ - } \ - return sequence_number; - #define SEND_SERVICE_RESPONSE(Type) \ auto action_server = get_pointer(pyaction_server, "rcl_action_server_t"); \ destroy_ros_message_signature * destroy_ros_message = NULL; \ @@ -559,50 +527,6 @@ rclpy_action_server_is_available(py::capsule pynode, py::capsule pyaction_client rclpy_convert_to_py(taken_msg_ptr.get(), pymsg_type.ptr())); \ return pytuple; -#define TAKE_SERVICE_RESPONSE(Type) \ - auto action_client = get_pointer(pyaction_client, "rcl_action_client_t"); \ - destroy_ros_message_signature * destroy_ros_message = NULL; \ - /* taken_msg is always destroyed in this function */ \ - void * taken_msg = rclpy_create_from_py(pymsg_type.ptr(), &destroy_ros_message); \ - if (!taken_msg) { \ - throw py::error_already_set(); \ - } \ - auto taken_msg_ptr = \ - std::unique_ptr(taken_msg, destroy_ros_message); \ - rmw_request_id_t header; \ - rcl_ret_t ret = rcl_action_take_ ## Type ## _response(action_client, &header, taken_msg); \ - int64_t sequence = header.sequence_number; \ - /* Create the tuple to return */ \ - py::tuple pytuple(2); \ - if (ret == RCL_RET_ACTION_CLIENT_TAKE_FAILED || ret == RCL_RET_ACTION_SERVER_TAKE_FAILED) { \ - pytuple[0] = py::none(); \ - pytuple[1] = py::none(); \ - return pytuple; \ - } else if (ret != RCL_RET_OK) { \ - throw rclpy::RCLError("Failed to take " #Type); \ - } \ - pytuple[0] = py::int_(sequence); \ - pytuple[1] = py::reinterpret_steal( \ - rclpy_convert_to_py(taken_msg_ptr.release(), pymsg_type.ptr())); \ - return pytuple; \ - - -/// Send an action goal request. -/** - * Raises AttributeError if there is an issue parsing the pygoal_request. - * Raises RuntimeError on failure. - * - * \param[in] pyaction_client The action client to use when sending the request. - * \param[in] pygoal_request The request message to send. - * \return sequence_number PyLong object representing the index of the sent request, or - * \return NULL if there is a failure. - */ -int64_t -rclpy_action_send_goal_request(py::capsule pyaction_client, py::object pyrequest) -{ - SEND_SERVICE_REQUEST(goal) -} - /// Take an action goal request. /** * Raises AttributeError if there is an issue parsing the pygoal_request_type. @@ -639,39 +563,6 @@ rclpy_action_send_goal_response( SEND_SERVICE_RESPONSE(goal) } -/// Take an action goal response. -/** - * Raises AttributeError if there is an issue parsing the pygoal_response_type. - * Raises RuntimeError if the underlying rcl library returns an error when taking the response. - * - * \param[in] pyaction_client The action client to use when sending the request. - * \param[in] pygoal_response_type An instance of the response message type to take. - * \return 2-tuple (sequence number, received response), or - * \return 2-tuple (None, None) if there is no response, or - * \return NULL if there is a failure. - */ -py::tuple -rclpy_action_take_goal_response(py::capsule pyaction_client, py::object pymsg_type) -{ - TAKE_SERVICE_RESPONSE(goal) -} - -/// Send an action result request. -/** - * Raises AttributeError if there is an issue parsing the pyresult_request. - * Raises RuntimeError if the underlying rcl library returns an error when sending the request. - * - * \param[in] pyaction_client The action client to use when sending the request. - * \param[in] pyresult_request The request message to send. - * \return sequence_number PyLong object representing the index of the sent request, or - * \return NULL if there is a failure. - */ -int64_t -rclpy_action_send_result_request(py::capsule pyaction_client, py::object pyrequest) -{ - SEND_SERVICE_REQUEST(result); -} - /// Take an action result request. /** * Raises AttributeError if there is an issue parsing the pyresult_request_type. @@ -708,39 +599,6 @@ rclpy_action_send_result_response( SEND_SERVICE_RESPONSE(result) } -/// Take an action result response. -/** - * Raises AttributeError if there is an issue parsing the pyresult_response_type. - * Raises RuntimeError if the underlying rcl library returns an error when taking the response. - * - * \param[in] pyaction_client The action client to use when sending the request. - * \param[in] pyresult_response_type An instance of the response message type to take. - * \return 2-tuple (sequence number, received response), or - * \return 2-tuple (None, None) if there is no response, or - * \return NULL if there is a failure. - */ -py::tuple -rclpy_action_take_result_response(py::capsule pyaction_client, py::object pymsg_type) -{ - TAKE_SERVICE_RESPONSE(result); -} - -/// Send an action cancel request. -/** - * Raises AttributeError if there is an issue parsing the pycancel_request. - * Raises RuntimeError if the underlying rcl library returns an error when sending the request. - * - * \param[in] pyaction_client The action client to use when sending the request. - * \param[in] pycancel_request The request message to send. - * \return sequence_number PyLong object representing the index of the sent request, or - * \return NULL if there is a failure. - */ -int64_t -rclpy_action_send_cancel_request(py::capsule pyaction_client, py::object pyrequest) -{ - SEND_SERVICE_REQUEST(cancel) -} - /// Take an action cancel request. /** * Raises AttributeError if there is an issue parsing the pycancel_request_type. @@ -777,41 +635,6 @@ rclpy_action_send_cancel_response( SEND_SERVICE_RESPONSE(cancel) } -/// Take an action cancel response. -/** - * Raises AttributeError if there is an issue parsing the pycancel_response_type. - * Raises RuntimeError if the underlying rcl library returns an error when taking the response. - * - * \param[in] pyaction_client The action client to use when sending the request. - * \param[in] pycancel_response_type An instance of the response message type to take. - * \return 2-tuple (sequence number, received response), or - * \return 2-tuple (None, None) if there is no response, or - * \return NULL if there is a failure. - */ -py::tuple -rclpy_action_take_cancel_response(py::capsule pyaction_client, py::object pymsg_type) -{ - TAKE_SERVICE_RESPONSE(cancel) -} - -#define TAKE_MESSAGE(Type) \ - auto action_client = get_pointer(pyaction_client, "rcl_action_client_t"); \ - destroy_ros_message_signature * destroy_ros_message = NULL; \ - void * taken_msg = rclpy_create_from_py(pymsg_type.ptr(), &destroy_ros_message); \ - if (!taken_msg) { \ - throw py::error_already_set(); \ - } \ - auto taken_msg_ptr = std::unique_ptr( \ - taken_msg, destroy_ros_message); \ - rcl_ret_t ret = rcl_action_take_ ## Type(action_client, taken_msg); \ - if (ret != RCL_RET_OK) { \ - if (ret == RCL_RET_ACTION_CLIENT_TAKE_FAILED) { \ - /* if take failed, just do nothing */ \ - return py::none(); \ - } \ - throw rclpy::RCLError("Failed to take " #Type " with an action client"); \ - } \ - return py::reinterpret_steal(rclpy_convert_to_py(taken_msg, pymsg_type.ptr())); /// Publish a feedback message from a given action server. /** @@ -839,24 +662,6 @@ rclpy_action_publish_feedback(py::capsule pyaction_server, py::object pymsg) } } -/// Take a feedback message from a given action client. -/** - * Raises AttributeError if there is an issue parsing the pyfeedback_type. - * Raises RuntimeError on failure while taking a feedback message. Note, this does not include - * the case where there are no messages available. - * - * \param[in] pyaction_client Capsule pointing to the action client to process the message. - * \param[in] pyfeedback_type Instance of the feedback message type to take. - * \return Python message with all fields populated with received message, or - * \return None if there is nothing to take, or - * \return NULL if there is a failure. - */ -py::object -rclpy_action_take_feedback(py::capsule pyaction_client, py::object pymsg_type) -{ - TAKE_MESSAGE(feedback) -} - /// Publish a status message from a given action server. /** * Raises RuntimeError on failure while publishing a status message. @@ -882,24 +687,6 @@ rclpy_action_publish_status(py::capsule pyaction_server) } } -/// Take a status message from a given action client. -/** - * Raises AttributeError if there is an issue parsing the pystatus_type. - * Raises RuntimeError on failure while taking a status message. Note, this does not include - * the case where there are no messages available. - * - * \param[in] pyaction_client Capsule pointing to the action client to process the message. - * \param[in] pystatus_type Instance of the status message type to take. - * \return Python message with all fields populated with received message, or - * \return None if there is nothing to take, or - * \return NULL if there is a failure. - */ -py::object -rclpy_action_take_status(py::capsule pyaction_client, py::object pymsg_type) -{ - TAKE_MESSAGE(status) -} - rclpy::ActionGoalHandle rclpy_action_accept_new_goal(py::capsule pyaction_server, py::object pygoal_info_msg) { @@ -1120,63 +907,36 @@ define_action_api(py::module m) m.def( "rclpy_action_wait_set_is_ready", &rclpy_action_wait_set_is_ready, "Check if an action entity has any sub-entities ready in a wait set."); - m.def( - "rclpy_action_create_client", &rclpy_action_create_client, - "Create an action client."); m.def( "rclpy_action_create_server", &rclpy_action_create_server, "Create an action server."); m.def( "rclpy_action_server_is_available", &rclpy_action_server_is_available, "Check if an action server is available for a given client."); - m.def( - "rclpy_action_send_goal_request", &rclpy_action_send_goal_request, - "Send a goal request."); m.def( "rclpy_action_take_goal_request", &rclpy_action_take_goal_request, "Take a goal request."); m.def( "rclpy_action_send_goal_response", &rclpy_action_send_goal_response, "Send a goal response."); - m.def( - "rclpy_action_take_goal_response", &rclpy_action_take_goal_response, - "Take a goal response."); - m.def( - "rclpy_action_send_result_request", &rclpy_action_send_result_request, - "Send a result request."); m.def( "rclpy_action_take_result_request", &rclpy_action_take_result_request, "Take a result request."); m.def( "rclpy_action_send_result_response", &rclpy_action_send_result_response, "Send a result response."); - m.def( - "rclpy_action_take_result_response", &rclpy_action_take_result_response, - "Take a result response."); - m.def( - "rclpy_action_send_cancel_request", &rclpy_action_send_cancel_request, - "Send a cancel request."); m.def( "rclpy_action_take_cancel_request", &rclpy_action_take_cancel_request, "Take a cancel request."); m.def( "rclpy_action_send_cancel_response", &rclpy_action_send_cancel_response, "Send a cancel response."); - m.def( - "rclpy_action_take_cancel_response", &rclpy_action_take_cancel_response, - "Take a cancel response."); m.def( "rclpy_action_publish_feedback", &rclpy_action_publish_feedback, "Publish a feedback message."); - m.def( - "rclpy_action_take_feedback", &rclpy_action_take_feedback, - "Take a feedback message."); m.def( "rclpy_action_publish_status", &rclpy_action_publish_status, "Publish a status message."); - m.def( - "rclpy_action_take_status", &rclpy_action_take_status, - "Take a status message."); m.def( "rclpy_action_accept_new_goal", &rclpy_action_accept_new_goal, "Accept a new goal using an action server."); @@ -1206,5 +966,7 @@ define_action_api(py::module m) m.def( "rclpy_action_get_names_and_types", &rclpy_action_get_names_and_types, "Get action names and types."); + + rclpy::define_action_client(m); } } // namespace rclpy diff --git a/rclpy/src/rclpy/action_client.cpp b/rclpy/src/rclpy/action_client.cpp new file mode 100644 index 000000000..68f78d421 --- /dev/null +++ b/rclpy/src/rclpy/action_client.cpp @@ -0,0 +1,330 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Include pybind11 before rclpy_common/handle.h includes Python.h +#include + +#include + +#include +#include + +#include "rclpy_common/common.h" +#include "rclpy_common/exceptions.hpp" +#include "rclpy_common/handle.h" + +#include "action_client.hpp" +#include "utils.hpp" + +namespace rclpy +{ + +void +ActionClient::destroy() +{ + rcl_action_client_.reset(); + node_handle_.reset(); +} + +ActionClient::ActionClient( + py::capsule pynode, + py::object pyaction_type, + const char * action_name, + const rmw_qos_profile_t & goal_service_qos, + const rmw_qos_profile_t & result_service_qos, + const rmw_qos_profile_t & cancel_service_qos, + const rmw_qos_profile_t & feedback_topic_qos, + const rmw_qos_profile_t & status_topic_qos) +: node_handle_(std::make_shared(pynode)) +{ + auto node = node_handle_->cast("rcl_node_t"); + + rosidl_action_type_support_t * ts = + static_cast(rclpy_common_get_type_support( + pyaction_type.ptr())); + if (!ts) { + throw py::error_already_set(); + } + + rcl_action_client_options_t action_client_ops = rcl_action_client_get_default_options(); + + action_client_ops.goal_service_qos = goal_service_qos; + action_client_ops.result_service_qos = result_service_qos; + action_client_ops.cancel_service_qos = cancel_service_qos; + action_client_ops.feedback_topic_qos = feedback_topic_qos; + action_client_ops.status_topic_qos = status_topic_qos; + + rcl_action_client_ = std::shared_ptr( + new rcl_action_client_t, + [this](rcl_action_client_t * action_client) + { + auto node = node_handle_->cast_or_warn("rcl_node_t"); + + rcl_ret_t ret = rcl_action_client_fini(action_client, 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 action_client; + }); + + *rcl_action_client_ = rcl_action_get_zero_initialized_client(); + + rcl_ret_t ret = rcl_action_client_init( + rcl_action_client_.get(), + node, + ts, + action_name, + &action_client_ops); + if (RCL_RET_ACTION_NAME_INVALID == ret) { + std::string error_text{"Failed to create action client due to invalid topic name '"}; + error_text += action_name; + error_text += "' : "; + error_text += rcl_get_error_string().str; + rcl_reset_error(); + throw py::value_error(error_text); + } + if (RCL_RET_OK != ret) { + std::string error_text{"Failed to create action client: "}; + error_text += rcl_get_error_string().str; + rcl_reset_error(); + throw py::value_error(error_text); + } +} + +#define TAKE_SERVICE_RESPONSE(Type) \ + /* taken_msg is always destroyed in this function */ \ + auto taken_msg = create_from_py(pymsg_type); \ + rmw_request_id_t header; \ + rcl_ret_t ret = rcl_action_take_ ## Type ## _response( \ + rcl_action_client_.get(), &header, taken_msg.get()); \ + int64_t sequence = header.sequence_number; \ + /* Create the tuple to return */ \ + if (RCL_RET_ACTION_CLIENT_TAKE_FAILED == ret || RCL_RET_ACTION_SERVER_TAKE_FAILED == ret) { \ + return py::make_tuple(py::none(), py::none()); \ + } else if (RCL_RET_OK != ret) { \ + throw rclpy::RCLError("Failed to take " #Type); \ + } \ + return py::make_tuple(sequence, convert_to_py(taken_msg.get(), pymsg_type)); \ + +py::tuple +ActionClient::take_goal_response(py::object pymsg_type) +{ + TAKE_SERVICE_RESPONSE(goal) +} + +#define SEND_SERVICE_REQUEST(Type) \ + auto ros_request = convert_from_py(pyrequest); \ + int64_t sequence_number; \ + rcl_ret_t ret = rcl_action_send_ ## Type ## _request( \ + rcl_action_client_.get(), ros_request.get(), &sequence_number); \ + if (RCL_RET_OK != ret) { \ + throw rclpy::RCLError("Failed to send " #Type " request"); \ + } \ + return sequence_number; + +int64_t +ActionClient::send_result_request(py::object pyrequest) +{ + SEND_SERVICE_REQUEST(result); +} + +py::tuple +ActionClient::take_cancel_response(py::object pymsg_type) +{ + TAKE_SERVICE_RESPONSE(cancel) +} + +#define TAKE_MESSAGE(Type) \ + auto taken_msg = create_from_py(pymsg_type); \ + rcl_ret_t ret = rcl_action_take_ ## Type(rcl_action_client_.get(), taken_msg.get()); \ + if (RCL_RET_OK != ret) { \ + if (RCL_RET_ACTION_CLIENT_TAKE_FAILED == ret) { \ + /* if take failed, just do nothing */ \ + return py::none(); \ + } \ + throw rclpy::RCLError("Failed to take " #Type " with an action client"); \ + } \ + return convert_to_py(taken_msg.get(), pymsg_type); + +py::object +ActionClient::take_feedback(py::object pymsg_type) +{ + TAKE_MESSAGE(feedback) +} + +py::object +ActionClient::take_status(py::object pymsg_type) +{ + TAKE_MESSAGE(status) +} + +int64_t +ActionClient::send_cancel_request(py::object pyrequest) +{ + SEND_SERVICE_REQUEST(cancel) +} + +int64_t +ActionClient::send_goal_request(py::object pyrequest) +{ + SEND_SERVICE_REQUEST(goal) +} + +py::tuple +ActionClient::take_result_response(py::object pymsg_type) +{ + TAKE_SERVICE_RESPONSE(result); +} + +py::tuple +ActionClient::get_num_entities() +{ + size_t num_subscriptions = 0u; + size_t num_guard_conditions = 0u; + size_t num_timers = 0u; + size_t num_clients = 0u; + size_t num_services = 0u; + + rcl_ret_t ret; + ret = rcl_action_client_wait_set_get_num_entities( + rcl_action_client_.get(), + &num_subscriptions, + &num_guard_conditions, + &num_timers, + &num_clients, + &num_services); + if (RCL_RET_OK != ret) { + std::string error_text{"Failed to get number of entities for 'rcl_action_client_t'"}; + throw rclpy::RCLError(error_text); + } + + return py::make_tuple( + num_subscriptions, num_guard_conditions, num_timers, + num_clients, num_services); +} + +bool +ActionClient::is_action_server_available() +{ + auto node = node_handle_->cast("rcl_node_t"); + + bool is_available = false; + rcl_ret_t ret = rcl_action_server_is_available( + node, rcl_action_client_.get(), &is_available); + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed to check if action server is available"); + } + return is_available; +} + +void +ActionClient::add_to_waitset(py::capsule pywait_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); + if (RCL_RET_OK != ret) { + std::string error_text{"Failed to add 'rcl_action_client_t' to wait set"}; + throw rclpy::RCLError(error_text); + } +} + +py::tuple +ActionClient::is_ready(py::capsule pywait_set) +{ + 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, + rcl_action_client_.get(), + &is_feedback_ready, + &is_status_ready, + &is_goal_response_ready, + &is_cancel_response_ready, + &is_result_response_ready); + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed to get number of ready entities for action client"); + } + + py::tuple result_tuple(5); + result_tuple[0] = py::bool_(is_feedback_ready); + result_tuple[1] = py::bool_(is_status_ready); + result_tuple[2] = py::bool_(is_goal_response_ready); + result_tuple[3] = py::bool_(is_cancel_response_ready); + result_tuple[4] = py::bool_(is_result_response_ready); + return result_tuple; +} + +void +define_action_client(py::object module) +{ + py::class_>(module, "ActionClient") + .def( + py::init()) + .def_property_readonly( + "pointer", [](const ActionClient & action_client) { + return reinterpret_cast(action_client.rcl_ptr()); + }, + "Get the address of the entity as an integer") + .def( + "take_goal_response", &ActionClient::take_goal_response, + "Take an action goal response.") + .def( + "send_result_request", &ActionClient::send_result_request, + "Send an action result request.") + .def( + "take_cancel_response", &ActionClient::take_cancel_response, + "Take an action cancel response.") + .def( + "take_feedback", &ActionClient::take_feedback, + "Take a feedback message from a given action client.") + .def( + "send_cancel_request", &ActionClient::send_cancel_request, + "Send an action cancel request.") + .def( + "send_goal_request", &ActionClient::send_goal_request, + "Send an action goal request.") + .def( + "take_result_response", &ActionClient::take_result_response, + "Take an action result response.") + .def( + "get_num_entities", &ActionClient::get_num_entities, + "Get the number of wait set entities that make up an action entity.") + .def( + "is_action_server_available", &ActionClient::is_action_server_available, + "Check if an action server is available for the given action client.") + .def( + "add_to_waitset", &ActionClient::add_to_waitset, + "Add an action entity to a wait set.") + .def( + "is_ready", &ActionClient::is_ready, + "Check if an action entity has any ready wait set entities.") + .def( + "take_status", &ActionClient::take_status, + "Take an action status response."); +} +} // namespace rclpy diff --git a/rclpy/src/rclpy/action_client.hpp b/rclpy/src/rclpy/action_client.hpp new file mode 100644 index 000000000..7ce59659f --- /dev/null +++ b/rclpy/src/rclpy/action_client.hpp @@ -0,0 +1,238 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLPY__ACTION_CLIENT_HPP_ +#define RCLPY__ACTION_CLIENT_HPP_ + +#include + +#include + +#include + +#include "destroyable.hpp" +#include "handle.hpp" + +namespace py = pybind11; + +namespace rclpy +{ +/** + * This class will create an action client for the given action name. + * This client will use the typesupport defined in the action module + * provided as pyaction_type to send messages. + */ +class ActionClient : public Destroyable, public std::enable_shared_from_this +{ +public: + /// Create an action client. + /* + * Raises ValueError if action name is invalid. + * Raises RuntimeError if the action client could not be created. + * + * \param[in] pynode Capsule pointing to the node to add the action client to. + * \param[in] pyaction_type Action module associated with the action client. + * \param[in] pyaction_name Python object containing the action name. + * \param[in] goal_service_qos rmw_qos_profile_t object for the goal service. + * \param[in] result_service_qos rmw_qos_profile_t object for the result service. + * \param[in] cancel_service_qos rmw_qos_profile_t object for the cancel service. + * \param[in] feedback_qos rmw_qos_profile_t object for the feedback subscriber. + * \param[in] status_qos rmw_qos_profile_t object for the status subscriber. + */ + ActionClient( + py::capsule pynode, + py::object pyaction_type, + const char * action_name, + const rmw_qos_profile_t & goal_service_qos, + const rmw_qos_profile_t & result_service_qos, + const rmw_qos_profile_t & cancel_service_qos, + const rmw_qos_profile_t & feedback_topic_qos, + const rmw_qos_profile_t & status_topic_qos); + + /// Take an action goal response. + /** + * Raises AttributeError if there is an issue parsing the pygoal_response_type. + * Raises RuntimeError if the underlying rcl library returns an error when taking the response. + * Raises RCLError if an error occurs in rcl + * + * \param[in] pygoal_response_type An instance of the response message type to take. + * \return 2-tuple (sequence number, received response), or + * \return 2-tuple (None, None) if there is no response, or + * \return (None, None) if there is a failure in the rcl API call. + */ + py::tuple + take_goal_response(py::object pymsg_type); + + /// Send an action result request. + /** + * Raises AttributeError if there is an issue parsing the pyresult_request. + * Raises RuntimeError if the underlying rcl library returns an error when sending the request. + * Raises RCLError if an error occurs in rcl + * + * \param[in] pyresult_request The request message to send. + * \return sequence_number the index of the sent request + */ + int64_t + send_result_request(py::object pyrequest); + + /// Take an action cancel response. + /** + * Raises AttributeError if there is an issue parsing the pycancel_response_type. + * Raises RuntimeError if the underlying rcl library returns an error when taking the response. + * Raises RCLError if an error occurs in rcl + * + * \param[in] pycancel_response_type An instance of the response message type to take. + * \return 2-tuple (sequence number, received response), or + * \return 2-tuple (None, None) if there is no response, or + * \return (None, None) if there is a failure in the rcl API call. + */ + py::tuple + take_cancel_response(py::object pymsg_type); + + /// Send an action cancel request. + /** + * Raises AttributeError if there is an issue parsing the pycancel_request. + * Raises RuntimeError if the underlying rcl library returns an error when sending the request. + * Raises RCLError if an error occurs in rcl + * + * \param[in] pycancel_request The request message to send. + * \return sequence_number the index of the sent request + */ + int64_t + send_cancel_request(py::object pyrequest); + + /// Take a feedback message from a given action client. + /** + * Raises AttributeError if there is an issue parsing the pyfeedback_type. + * Raises RuntimeError on failure while taking a feedback message. Note, this does not include + * the case where there are no messages available. + * Raises RCLError if an error occurs in rcl + * + * \param[in] pyfeedback_type Instance of the feedback message type to take. + * \return Python message with all fields populated with received message, or + * \return None if there is nothing to take, or + * \return None if there is a failure in the rcl API call. + */ + py::object + take_feedback(py::object pymsg_type); + + /// Take a status message from a given action client. + /** + * Raises AttributeError if there is an issue parsing the pystatus_type. + * Raises RuntimeError on failure while taking a status message. Note, this does not include + * the case where there are no messages available. + * Raises RCLError if an error occurs in rcl + * + * \param[in] pystatus_type Instance of the status message type to take. + * \return Python message with all fields populated with received message, or + * \return None if there is nothing to take, or + * \return None if there is a failure in the rcl API call. + */ + py::object + take_status(py::object pymsg_type); + + /// Send an action goal request. + /** + * Raises AttributeError if there is an issue parsing the pygoal_request. + * Raises RuntimeError on failure. + * Raises RCLError if an error occurs in rcl + * + * \param[in] pygoal_request The request message to send. + * \return sequence_number PyLong object representing the index of the sent request + */ + int64_t + send_goal_request(py::object pyrequest); + + /// Take an action result response. + /** + * Raises AttributeError if there is an issue parsing the pyresult_response_type. + * Raises RuntimeError if the underlying rcl library returns an error when taking the response. + * Raises RCLError if an error occurs in rcl + * + * \param[in] pyresult_response_type An instance of the response message type to take. + * \return 2-tuple (sequence number, received response), or + * \return 2-tuple (None, None) if there is no response, or + * \return (None, None) if there is a failure in the rcl API call. + */ + py::tuple + take_result_response(py::object pymsg_type); + + /// Get the number of wait set entities that make up an action entity. + /** + * \return Tuple containing the number of wait set entities: + * (num_subscriptions, + * num_guard_conditions, + * num_timers, + * num_clients, + * num_services) + */ + py::tuple + get_num_entities(); + + /// Check if an action server is available for the given action client. + /** + * Raises RuntimeError on failure. + * + * \return True if an action server is available, False otherwise. + */ + bool + is_action_server_available(); + + /// Check if an action entity has any ready wait set entities. + /** + * This must be called after waiting on the wait set. + * Raises RuntimeError on failure. + * + * \param[in] pywait_set Capsule pointing to the wait set structure. + * \return A tuple of booleans representing the sub-entities ready: + * (is_feedback_ready, + * is_status_ready, + * is_goal_response_ready, + * is_cancel_response_ready, + * is_result_response_ready) + */ + py::tuple + is_ready(py::capsule pywait_set); + + /// Add an action entitiy to a wait set. + /** + * Raises RuntimeError on failure. + * \param[in] pywait_set Capsule pointer to an rcl_wait_set_t. + */ + void + add_to_waitset(py::capsule pywait_set); + + /// Get rcl_client_t pointer + rcl_action_client_t * + rcl_ptr() const + { + return rcl_action_client_.get(); + } + + /// Force an early destruction of this object + void + destroy() override; + +private: + std::shared_ptr node_handle_; + std::shared_ptr rcl_action_client_; +}; +/// Define a pybind11 wrapper for an rcl_time_point_t +/** + * \param[in] module a pybind11 module to add the definition to + */ +void define_action_client(py::object module); +} // namespace rclpy + +#endif // RCLPY__ACTION_CLIENT_HPP_ diff --git a/rclpy/src/rclpy/utils.cpp b/rclpy/src/rclpy/utils.cpp index 4aca238e8..39adef07a 100644 --- a/rclpy/src/rclpy/utils.cpp +++ b/rclpy/src/rclpy/utils.cpp @@ -81,6 +81,31 @@ create_from_py(py::object pymessage) void, destroy_ros_message_function *>(message, destroy_ros_message); } +std::unique_ptr +convert_from_py(py::object pymessage) +{ + typedef bool convert_from_py_signature (PyObject *, void *); + + std::unique_ptr message = + create_from_py(pymessage); + + py::object pymetaclass = pymessage.attr("__class__"); + + auto capsule_ptr = static_cast( + pymetaclass.attr("_CONVERT_FROM_PY").cast()); + auto convert = + reinterpret_cast(capsule_ptr); + if (!convert) { + throw py::error_already_set(); + } + + if (!convert(pymessage.ptr(), message.get())) { + throw py::error_already_set(); + } + + return message; +} + py::object convert_to_py(void * message, py::object pyclass) { diff --git a/rclpy/src/rclpy/utils.hpp b/rclpy/src/rclpy/utils.hpp index de86d697e..1520005a1 100644 --- a/rclpy/src/rclpy/utils.hpp +++ b/rclpy/src/rclpy/utils.hpp @@ -49,6 +49,16 @@ convert_to_py_names_and_types(const rcl_names_and_types_t * topic_names_and_type std::unique_ptr create_from_py(py::object pyclass); +/// Convert a ROS message from a Python type to a C type. +/** + * Raises AttributeError if the Python message type is missing a required attribute. + * + * \param[in] pyclass ROS message Python type to extract data from. + * \return unique pointer with the C version of the input ROS message. + */ +std::unique_ptr +convert_from_py(py::object pyclass); + /// Convert a ROS message from a C type to a Python type. /** * Raises AttributeError if \p pyclass is missing a required attribute. From 9e4ba03c9f9a2c1cc6edb6b8437c798ae85115e6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 14 Apr 2021 17:22:32 +0200 Subject: [PATCH 04/10] Convert ActionServer to use C++ Classes (#766) * Convert ActionServer to use C++ Classes Signed-off-by: ahcorde * Fixed Test_action_server Signed-off-by: ahcorde * Improved docs Signed-off-by: ahcorde * Improved docs and minor fixes Signed-off-by: ahcorde --- rclpy/CMakeLists.txt | 1 + rclpy/rclpy/action/server.py | 62 +- rclpy/src/rclpy/_rclpy_action.cpp | 790 +------------------------ rclpy/src/rclpy/action_goal_handle.cpp | 29 +- rclpy/src/rclpy/action_goal_handle.hpp | 8 +- rclpy/src/rclpy/action_server.cpp | 414 +++++++++++++ rclpy/src/rclpy/action_server.hpp | 264 +++++++++ 7 files changed, 711 insertions(+), 857 deletions(-) create mode 100644 rclpy/src/rclpy/action_server.cpp create mode 100644 rclpy/src/rclpy/action_server.hpp diff --git a/rclpy/CMakeLists.txt b/rclpy/CMakeLists.txt index 7d1f80c84..b1abfd719 100644 --- a/rclpy/CMakeLists.txt +++ b/rclpy/CMakeLists.txt @@ -155,6 +155,7 @@ pybind11_add_module(_rclpy_pybind11 SHARED src/rclpy/_rclpy_pycapsule.cpp src/rclpy/action_client.cpp src/rclpy/action_goal_handle.cpp + src/rclpy/action_server.cpp src/rclpy/client.cpp src/rclpy/clock.cpp src/rclpy/context.cpp diff --git a/rclpy/rclpy/action/server.py b/rclpy/rclpy/action/server.py index aa27f9160..ef9ded1d7 100644 --- a/rclpy/rclpy/action/server.py +++ b/rclpy/rclpy/action/server.py @@ -110,7 +110,7 @@ def _update_state(self, event): self._goal.update_goal_state(event) # Publish state change - _rclpy.rclpy_action_publish_status(self._action_server._handle) + self._action_server._handle.publish_status() # If it's a terminal state, then also notify the action server if not self._goal.is_active(): @@ -140,8 +140,7 @@ def publish_feedback(self, feedback): feedback_message.feedback = feedback # Publish - _rclpy.rclpy_action_publish_feedback( - self._action_server._handle, feedback_message) + self._action_server._handle.publish_feedback(feedback_message) def succeed(self): self._update_state(_rclpy.GoalEvent.SUCCEED) @@ -239,7 +238,7 @@ def __init__( self._node = node self._action_type = action_type with node.handle as node_capsule, node.get_clock().handle: - self._handle = _rclpy.rclpy_action_create_server( + self._handle = _rclpy.ActionServer( node_capsule, node.get_clock().handle, action_type, @@ -268,7 +267,7 @@ async def _execute_goal_request(self, request_header_and_message): # Check if goal ID is already being tracked by this action server with self._lock: - goal_id_exists = _rclpy.rclpy_action_server_goal_exists(self._handle, goal_info) + goal_id_exists = self._handle.goal_exists(goal_info) accepted = False if not goal_id_exists: @@ -299,11 +298,7 @@ async def _execute_goal_request(self, request_header_and_message): response_msg = self._action_type.Impl.SendGoalService.Response() response_msg.accepted = accepted response_msg.stamp = goal_info.stamp - _rclpy.rclpy_action_send_goal_response( - self._handle, - request_header, - response_msg, - ) + self._handle.send_goal_response(request_header, response_msg) if not accepted: self._node.get_logger().debug('New goal rejected: {0}'.format(goal_uuid.uuid)) @@ -349,8 +344,8 @@ async def _execute_cancel_request(self, request_header_and_message): with self._lock: # Get list of goals that are requested to be canceled - cancel_response = _rclpy.rclpy_action_process_cancel_request( - self._handle, cancel_request, self._action_type.Impl.CancelGoalService.Response) + cancel_response = self._handle.process_cancel_request( + cancel_request, self._action_type.Impl.CancelGoalService.Response) for goal_info in cancel_response.goals_canceling: goal_uuid = bytes(goal_info.goal_id.uuid) @@ -369,11 +364,7 @@ async def _execute_cancel_request(self, request_header_and_message): # Remove from response cancel_response.goals_canceling.remove(goal_info) - _rclpy.rclpy_action_send_cancel_response( - self._handle, - request_header, - cancel_response, - ) + self._handle.send_cancel_response(request_header, cancel_response) async def _execute_get_result_request(self, request_header_and_message): request_header, result_request = request_header_and_message @@ -388,11 +379,7 @@ async def _execute_get_result_request(self, request_header_and_message): 'Sending result response for unknown goal ID: {0}'.format(goal_uuid)) result_response = self._action_type.Impl.GetResultService.Response() result_response.status = GoalStatus.STATUS_UNKNOWN - _rclpy.rclpy_action_send_result_response( - self._handle, - request_header, - result_response, - ) + self._handle.send_result_response(request_header, result_response) return # There is an accepted goal matching the goal ID, register a callback to send the @@ -406,11 +393,7 @@ async def _execute_expire_goals(self, expired_goals): del self._goal_handles[goal_uuid] def _send_result_response(self, request_header, future): - _rclpy.rclpy_action_send_result_response( - self._handle, - request_header, - future.result(), - ) + self._handle.send_result_response(request_header, future.result()) @property def action_type(self): @@ -420,7 +403,7 @@ def action_type(self): def is_ready(self, wait_set): """Return True if one or more entities are ready in the wait set.""" with self._lock: - ready_entities = _rclpy.rclpy_action_wait_set_is_ready(self._handle, wait_set) + ready_entities = self._handle.is_ready(wait_set) self._is_goal_request_ready = ready_entities[0] self._is_cancel_request_ready = ready_entities[1] self._is_result_request_ready = ready_entities[2] @@ -432,8 +415,7 @@ def take_data(self): data = {} if self._is_goal_request_ready: with self._lock: - taken_data = _rclpy.rclpy_action_take_goal_request( - self._handle, + taken_data = self._handle.take_goal_request( self._action_type.Impl.SendGoalService.Request, ) # If take fails, then we get (None, None) @@ -442,8 +424,7 @@ def take_data(self): if self._is_cancel_request_ready: with self._lock: - taken_data = _rclpy.rclpy_action_take_cancel_request( - self._handle, + taken_data = self._handle.take_cancel_request( self._action_type.Impl.CancelGoalService.Request, ) # If take fails, then we get (None, None) @@ -452,8 +433,7 @@ def take_data(self): if self._is_result_request_ready: with self._lock: - taken_data = _rclpy.rclpy_action_take_result_request( - self._handle, + taken_data = self._handle.take_result_request( self._action_type.Impl.GetResultService.Request, ) # If take fails, then we get (None, None) @@ -462,10 +442,7 @@ def take_data(self): if self._is_goal_expired: with self._lock: - data['expired'] = _rclpy.rclpy_action_expire_goals( - self._handle, - len(self._goal_handles), - ) + data['expired'] = self._handle.expire_goals(len(self._goal_handles)) return data @@ -490,7 +467,7 @@ async def execute(self, taken_data): def get_num_entities(self): """Return number of each type of entity used in the wait set.""" - num_entities = _rclpy.rclpy_action_wait_set_get_num_entities(self._handle) + num_entities = self._handle.get_num_entities() return NumberOfEntities( num_entities[0], num_entities[1], @@ -501,7 +478,7 @@ def get_num_entities(self): def add_to_wait_set(self, wait_set): """Add entities to wait set.""" with self._lock: - _rclpy.rclpy_action_wait_set_add(self._handle, wait_set) + self._handle.add_to_waitset(wait_set) # End Waitable API def notify_execute(self, goal_handle, execute_callback): @@ -516,7 +493,7 @@ def notify_execute(self, goal_handle, execute_callback): def notify_goal_done(self): with self._lock: - _rclpy.rclpy_action_notify_goal_done(self._handle) + self._handle.notify_goal_done() def register_handle_accepted_callback(self, handle_accepted_callback): """ @@ -604,8 +581,7 @@ def destroy(self): for goal_handle in self._goal_handles.values(): goal_handle.destroy() - with self._node.handle as node_capsule: - _rclpy.rclpy_action_destroy_entity(self._handle, node_capsule) + self._handle.destroy_when_not_in_use() self._node.remove_waitable(self) self._handle = None diff --git a/rclpy/src/rclpy/_rclpy_action.cpp b/rclpy/src/rclpy/_rclpy_action.cpp index 2f55d1856..8b5121921 100644 --- a/rclpy/src/rclpy/_rclpy_action.cpp +++ b/rclpy/src/rclpy/_rclpy_action.cpp @@ -28,6 +28,7 @@ #include "action_client.hpp" #include "action_goal_handle.hpp" +#include "action_server.hpp" #include "clock.hpp" namespace py = pybind11; @@ -49,45 +50,6 @@ get_pointer(py::capsule & capsule, const char * name) return static_cast(capsule); } - -/// Destroy an rcl_action entity. -/** - * Raises RuntimeError on failure. - * - * \param[in] pyentity Capsule pointing to the entity to destroy. - * \param[in] pynode Capsule pointing to the node the action client was added to. - */ -void -rclpy_action_destroy_entity(py::capsule pyentity, py::capsule pynode) -{ - rcl_node_t * node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - - rcl_ret_t ret; - if (0 == std::strcmp("rcl_action_server_t", pyentity.name())) { - auto action_server = get_pointer(pyentity, "rcl_action_server_t"); - ret = rcl_action_server_fini(action_server, node); - PyMem_Free(action_server); - } else { - std::string entity_name = pyentity.name(); - throw std::runtime_error(entity_name + " is not a known entity"); - } - - if (ret != RCL_RET_OK) { - std::string error_text = "Failed to fini '"; - error_text += pyentity.name(); - error_text += "'"; - throw rclpy::RCLError(error_text); - } - - if (PyCapsule_SetName(pyentity.ptr(), "_destroyed_by_rclpy_action_destroy_entity_")) { - throw py::error_already_set(); - } -} - /// Fetch a predefined qos_profile from rcl_action and convert it to a Python QoSProfile object. /** * Raises RuntimeError if the QoS profile is unknown. @@ -111,697 +73,6 @@ rclpy_action_get_rmw_qos_profile(const char * rmw_profile) return py::reinterpret_steal(pyqos_profile); } -/// Add an action entitiy to a wait set. -/** - * Raises RuntimeError on failure. - * \param[in] pyentity Capsule pointer to an action entity - * (rcl_action_client_t or rcl_action_server_t). - * \param[in] pywait_set Capsule pointer to an rcl_wait_set_t. - */ -void -rclpy_action_wait_set_add(py::capsule pyentity, py::capsule pywait_set) -{ - auto wait_set = get_pointer(pywait_set, "rcl_wait_set_t"); - - rcl_ret_t ret; - if (0 == strcmp(pyentity.name(), "rcl_action_server_t")) { - auto action_server = get_pointer(pyentity, "rcl_action_server_t"); - ret = rcl_action_wait_set_add_action_server(wait_set, action_server, NULL); - } else { - std::string error_text{"Unknown entity: "}; - error_text += pyentity.name(); - throw std::runtime_error(error_text); - } - - if (RCL_RET_OK != ret) { - std::string error_text{"Failed to add '"}; - error_text += pyentity.name(); - error_text += "' to wait set"; - throw rclpy::RCLError(error_text); - } -} - -/// Get the number of wait set entities that make up an action entity. -/** - * \param[in] pyentity Capsule pointer to an action entity - * (rcl_action_client_t or rcl_action_server_t). - * \return Tuple containing the number of wait set entities: - * (num_subscriptions, - * num_guard_conditions, - * num_timers, - * num_clients, - * num_services) - */ -py::tuple -rclpy_action_wait_set_get_num_entities(py::capsule pyentity) -{ - size_t num_subscriptions = 0u; - size_t num_guard_conditions = 0u; - size_t num_timers = 0u; - size_t num_clients = 0u; - size_t num_services = 0u; - - rcl_ret_t ret; - if (0 == strcmp(pyentity.name(), "rcl_action_server_t")) { - auto action_server = get_pointer(pyentity, "rcl_action_server_t"); - - ret = rcl_action_server_wait_set_get_num_entities( - action_server, - &num_subscriptions, - &num_guard_conditions, - &num_timers, - &num_clients, - &num_services); - } else { - std::string error_text{"Unknown entity: "}; - error_text += pyentity.name(); - throw std::runtime_error(error_text); - } - - if (RCL_RET_OK != ret) { - std::string error_text{"Failed to get number of entities for '"}; - error_text += pyentity.name(); - error_text += "'"; - throw rclpy::RCLError(error_text); - } - - py::tuple result_tuple(5); - result_tuple[0] = py::int_(num_subscriptions); - result_tuple[1] = py::int_(num_guard_conditions); - result_tuple[2] = py::int_(num_timers); - result_tuple[3] = py::int_(num_clients); - result_tuple[4] = py::int_(num_services); - return result_tuple; -} - -/// Check if an action entity has any ready wait set entities. -/** - * This must be called after waiting on the wait set. - * Raises RuntimeError on failure. - * - * \param[in] entity Capsule pointing to the action entity - * (rcl_action_client_t or rcl_action_server_t). - * \param[in] pywait_set Capsule pointing to the wait set structure. - * \return A tuple of Bool representing the ready sub-entities. - * For a rcl_action_client_t: - * (is_feedback_ready, - * is_status_ready, - * is_goal_response_ready, - * is_cancel_response_ready, - * is_result_response_ready) - * - * For a rcl_action_server_t: - * (is_goal_request_ready, - * is_cancel_request_ready, - * is_result_request_ready, - * is_goal_expired) - */ -py::tuple -rclpy_action_wait_set_is_ready(py::capsule pyentity, py::capsule pywait_set) -{ - auto wait_set = get_pointer(pywait_set, "rcl_wait_set_t"); - - if (0 == strcmp(pyentity.name(), "rcl_action_client_t")) { - auto action_client = get_pointer(pyentity, "rcl_action_client_t"); - 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, - action_client, - &is_feedback_ready, - &is_status_ready, - &is_goal_response_ready, - &is_cancel_response_ready, - &is_result_response_ready); - if (RCL_RET_OK != ret) { - throw rclpy::RCLError("Failed to get number of ready entities for action client"); - } - - py::tuple result_tuple(5); - result_tuple[0] = py::bool_(is_feedback_ready); - result_tuple[1] = py::bool_(is_status_ready); - result_tuple[2] = py::bool_(is_goal_response_ready); - result_tuple[3] = py::bool_(is_cancel_response_ready); - result_tuple[4] = py::bool_(is_result_response_ready); - return result_tuple; - } else if (0 == strcmp(pyentity.name(), "rcl_action_server_t")) { - auto action_server = get_pointer(pyentity, "rcl_action_server_t"); - 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, - action_server, - &is_goal_request_ready, - &is_cancel_request_ready, - &is_result_request_ready, - &is_goal_expired); - if (RCL_RET_OK != ret) { - throw rclpy::RCLError("Failed to get number of ready entities for action server"); - } - - py::tuple result_tuple(4); - result_tuple[0] = py::bool_(is_goal_request_ready); - result_tuple[1] = py::bool_(is_cancel_request_ready); - result_tuple[2] = py::bool_(is_result_request_ready); - result_tuple[3] = py::bool_(is_goal_expired); - return result_tuple; - } - - std::string error_text{"Unknown entity: "}; - error_text += pyentity.name(); - throw std::runtime_error(error_text); -} - -/// Create an action client. -/** - * This function will create an action client for the given action name. - * This client will use the typesupport defined in the action module - * provided as pyaction_type to send messages over the wire. - * - * On a successful call a capsule referencing the created rcl_action_client_t structure - * is returned. - * - * Raises ValueError if action name is invalid - * Raises RuntimeError if the action client could not be created. - * - * \remark Call rclpy_action_destroy_entity() to destroy an action client. - * \param[in] pynode Capsule pointing to the node to add the action client to. - * \param[in] pyaction_type Action module associated with the action client. - * \param[in] pyaction_name Python object containing the action name. - * \param[in] goal_service_qos rmw_qos_profile_t object for the goal service. - * \param[in] result_service_qos rmw_qos_profile_t object for the result service. - * \param[in] cancel_service_qos rmw_qos_profile_t object for the cancel service. - * \param[in] feedback_qos rmw_qos_profile_t object for the feedback subscriber. - * \param[in] status_qos rmw_qos_profile_t object for the status subscriber. - * \return Capsule named 'rcl_action_client_t', or - * \return NULL on failure. - */ -py::capsule -rclpy_action_create_client( - py::capsule pynode, - py::object pyaction_type, - const char * action_name, - const rmw_qos_profile_t & goal_service_qos, - const rmw_qos_profile_t & result_service_qos, - const rmw_qos_profile_t & cancel_service_qos, - const rmw_qos_profile_t & feedback_topic_qos, - const rmw_qos_profile_t & status_topic_qos) -{ - rcl_node_t * node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - - rosidl_action_type_support_t * ts = - static_cast(rclpy_common_get_type_support( - pyaction_type.ptr())); - if (!ts) { - throw py::error_already_set(); - } - - rcl_action_client_options_t action_client_ops = rcl_action_client_get_default_options(); - - action_client_ops.goal_service_qos = goal_service_qos; - action_client_ops.result_service_qos = result_service_qos; - action_client_ops.cancel_service_qos = cancel_service_qos; - action_client_ops.feedback_topic_qos = feedback_topic_qos; - action_client_ops.status_topic_qos = status_topic_qos; - - auto deleter = [](rcl_action_client_t * ptr) {PyMem_Free(ptr);}; - auto action_client = std::unique_ptr( - static_cast(PyMem_Malloc(sizeof(rcl_action_client_t))), - deleter); - if (!action_client) { - throw std::bad_alloc(); - } - - *action_client = rcl_action_get_zero_initialized_client(); - rcl_ret_t ret = rcl_action_client_init( - action_client.get(), - node, - ts, - action_name, - &action_client_ops); - if (ret == RCL_RET_ACTION_NAME_INVALID) { - std::string error_text{"Failed to create action client due to invalid topic name '"}; - error_text += action_name; - error_text += "' : "; - error_text += rcl_get_error_string().str; - rcl_reset_error(); - throw py::value_error(error_text); - } else if (ret != RCL_RET_OK) { - std::string error_text{"Failed to create action client: "}; - error_text += rcl_get_error_string().str; - rcl_reset_error(); - throw py::value_error(error_text); - } - - return py::capsule(action_client.release(), "rcl_action_client_t"); -} - -/// Create an action server. -/** - * This function will create an action server for the given action name. - * This server will use the typesupport defined in the action module - * provided as pyaction_type to send messages over the wire. - * - * On a successful call a capsule referencing the created rcl_action_server_t structure - * is returned. - * - * Raises AttributeError if action type is invalid - * Raises ValueError if action name is invalid - * Raises RuntimeError if the action server could not be created. - * - * \remark Call rclpy_action_destroy_entity() to destroy an action server. - * \param[in] pynode Capsule pointing to the node to add the action server to. - * \param[in] pyaction_type Action module associated with the action server. - * \param[in] pyaction_name Python object containing the action name. - * \param[in] goal_service_qos rmw_qos_profile_t object for the goal service. - * \param[in] result_service_qos rmw_qos_profile_t object for the result service. - * \param[in] cancel_service_qos rmw_qos_profile_t object for the cancel service. - * \param[in] feedback_qos rmw_qos_profile_t object for the feedback subscriber. - * \param[in] status_qos rmw_qos_profile_t object for the status subscriber. - * \return Capsule named 'rcl_action_server_t', or - * \return NULL on failure. - */ -py::capsule -rclpy_action_create_server( - py::capsule pynode, - const rclpy::Clock & rclpy_clock, - py::object pyaction_type, - const char * action_name, - const rmw_qos_profile_t & goal_service_qos, - const rmw_qos_profile_t & result_service_qos, - const rmw_qos_profile_t & cancel_service_qos, - const rmw_qos_profile_t & feedback_topic_qos, - const rmw_qos_profile_t & status_topic_qos, - double result_timeout) -{ - rcl_node_t * node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - - rcl_clock_t * clock = rclpy_clock.rcl_ptr(); - - rosidl_action_type_support_t * ts = static_cast( - rclpy_common_get_type_support(pyaction_type.ptr())); - if (!ts) { - throw py::error_already_set(); - } - - rcl_action_server_options_t action_server_ops = rcl_action_server_get_default_options(); - - action_server_ops.goal_service_qos = goal_service_qos; - action_server_ops.result_service_qos = result_service_qos; - action_server_ops.cancel_service_qos = cancel_service_qos; - action_server_ops.feedback_topic_qos = feedback_topic_qos; - action_server_ops.status_topic_qos = status_topic_qos; - action_server_ops.result_timeout.nanoseconds = (rcl_duration_value_t)RCL_S_TO_NS(result_timeout); - - auto deleter = [](rcl_action_server_t * ptr) {PyMem_Free(ptr);}; - auto action_server = std::unique_ptr( - static_cast(PyMem_Malloc(sizeof(rcl_action_server_t))), - deleter); - if (!action_server) { - throw std::bad_alloc(); - } - - *action_server = rcl_action_get_zero_initialized_server(); - rcl_ret_t ret = rcl_action_server_init( - action_server.get(), - node, - clock, - ts, - action_name, - &action_server_ops); - if (ret == RCL_RET_ACTION_NAME_INVALID) { - std::string error_text{"Failed to create action server due to invalid topic name '"}; - error_text += action_name; - error_text += "' : "; - error_text += rcl_get_error_string().str; - rcl_reset_error(); - throw py::value_error(error_text); - } else if (ret != RCL_RET_OK) { - std::string error_text{"Failed to create action server: "}; - error_text += rcl_get_error_string().str; - rcl_reset_error(); - throw py::value_error(error_text); - } - - return py::capsule(action_server.release(), "rcl_action_server_t"); -} - - -/// Check if an action server is available for the given action client. -/** - * Raises RuntimeError on failure. - * - * \param[in] pynode Capsule pointing to the node to associated with the action client. - * \param[in] pyaction_client The action client to use when checking for an available server. - * \return True if an action server is available, False otherwise. - */ -bool -rclpy_action_server_is_available(py::capsule pynode, py::capsule pyaction_client) -{ - rcl_node_t * node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - - auto action_client = get_pointer(pyaction_client, "rcl_action_client_t"); - - bool is_available = false; - rcl_ret_t ret = rcl_action_server_is_available(node, action_client, &is_available); - if (RCL_RET_OK != ret) { - throw rclpy::RCLError("Failed to check if action server is available"); - } - return is_available; -} - -#define SEND_SERVICE_RESPONSE(Type) \ - auto action_server = get_pointer(pyaction_server, "rcl_action_server_t"); \ - destroy_ros_message_signature * destroy_ros_message = NULL; \ - void * raw_ros_response = rclpy_convert_from_py(pyresponse.ptr(), &destroy_ros_message); \ - if (!raw_ros_response) { \ - throw py::error_already_set(); \ - } \ - rcl_ret_t ret = rcl_action_send_ ## Type ## _response(action_server, header, raw_ros_response); \ - destroy_ros_message(raw_ros_response); \ - if (ret != RCL_RET_OK) { \ - throw rclpy::RCLError("Failed to send " #Type " response"); \ - } - -#define TAKE_SERVICE_REQUEST(Type) \ - auto action_server = get_pointer(pyaction_server, "rcl_action_server_t"); \ - destroy_ros_message_signature * destroy_ros_message = NULL; \ - /* taken_msg is always destroyed in this function */ \ - void * taken_msg = rclpy_create_from_py(pymsg_type.ptr(), &destroy_ros_message); \ - if (!taken_msg) { \ - throw py::error_already_set(); \ - } \ - auto taken_msg_ptr = \ - std::unique_ptr(taken_msg, destroy_ros_message); \ - rmw_request_id_t header; \ - rcl_ret_t ret = \ - rcl_action_take_ ## Type ## _request(action_server, &header, taken_msg_ptr.get()); \ - /* Create the tuple to return */ \ - py::tuple pytuple(2); \ - if (ret == RCL_RET_ACTION_CLIENT_TAKE_FAILED || ret == RCL_RET_ACTION_SERVER_TAKE_FAILED) { \ - pytuple[0] = py::none(); \ - pytuple[1] = py::none(); \ - return pytuple; \ - } else if (ret != RCL_RET_OK) { \ - throw rclpy::RCLError("Failed to take " #Type); \ - } \ - pytuple[0] = header; \ - pytuple[1] = py::reinterpret_steal( \ - rclpy_convert_to_py(taken_msg_ptr.get(), pymsg_type.ptr())); \ - return pytuple; - -/// Take an action goal request. -/** - * Raises AttributeError if there is an issue parsing the pygoal_request_type. - * Raises RuntimeError on failure. - * - * \param[in] pyaction_server The action server to use when taking the request. - * \param[in] pygoal_request_type An instance of the type of request message to take. - * \return 2-tuple (header, received request message) where the header is an - * "rclpy.rmw_request_id_t" type, or - * \return 2-tuple (None, None) if there as no message to take, or - * \return NULL if there is a failure. - */ -py::tuple -rclpy_action_take_goal_request(py::capsule pyaction_server, py::object pymsg_type) -{ - TAKE_SERVICE_REQUEST(goal) -} - -/// Send an action goal response. -/** - * Raises AttributeError if there is an issue parsing the pygoal_response. - * Raises RuntimeError on failure. - * - * \param[in] pyaction_server The action server to use when sending the response. - * \param[in] header Pointer to the message header. - * \param[in] pygoal_response The response message to send. - * \return None - * \return NULL if there is a failure. - */ -void -rclpy_action_send_goal_response( - py::capsule pyaction_server, rmw_request_id_t * header, py::object pyresponse) -{ - SEND_SERVICE_RESPONSE(goal) -} - -/// Take an action result request. -/** - * Raises AttributeError if there is an issue parsing the pyresult_request_type. - * Raises RuntimeError on failure. - * - * \param[in] pyaction_server The action server to use when taking the request. - * \param[in] pyresult_request_type An instance of the type of request message to take. - * \return 2-tuple (header, received request message) where the header is an - * "rclpy.rmw_request_id_t" type, or - * \return 2-tuple (None, None) if there as no message to take, or - * \return NULL if there is a failure. - */ -py::tuple -rclpy_action_take_result_request(py::capsule pyaction_server, py::object pymsg_type) -{ - TAKE_SERVICE_REQUEST(result) -} - -/// Send an action result response. -/** - * Raises AttributeError if there is an issue parsing the pyresult_response. - * Raises RuntimeError on failure. - * - * \param[in] pyaction_server The action server to use when sending the response. - * \param[in] pyheader Pointer to the message header. - * \param[in] pyresult_response The response message to send. - * \return None - * \return NULL if there is a failure. - */ -void -rclpy_action_send_result_response( - py::capsule pyaction_server, rmw_request_id_t * header, py::object pyresponse) -{ - SEND_SERVICE_RESPONSE(result) -} - -/// Take an action cancel request. -/** - * Raises AttributeError if there is an issue parsing the pycancel_request_type. - * Raises RuntimeError on failure. - * - * \param[in] pyaction_server The action server to use when taking the request. - * \param[in] pycancel_request_type An instance of the type of request message to take. - * \return 2-tuple (header, received request message) where the header is an - * "rmw_request_id_t" type, or - * \return 2-tuple (None, None) if there as no message to take, or - * \return NULL if there is a failure. - */ -py::tuple -rclpy_action_take_cancel_request(py::capsule pyaction_server, py::object pymsg_type) -{ - TAKE_SERVICE_REQUEST(cancel) -} - -/// Send an action cancel response. -/** - * Raises AttributeError if there is an issue parsing the pycancel_response. - * Raises RuntimeError on failure. - * - * \param[in] pyaction_server The action server to use when sending the response. - * \param[in] pyheader Pointer to the message header. - * \param[in] pycancel_response The response message to send. - * \return sequence_number PyLong object representing the index of the sent response, or - * \return NULL if there is a failure. - */ -void -rclpy_action_send_cancel_response( - py::capsule pyaction_server, rmw_request_id_t * header, py::object pyresponse) -{ - SEND_SERVICE_RESPONSE(cancel) -} - - -/// Publish a feedback message from a given action server. -/** - * Raises AttributeError if there is an issue parsing the pyfeedback_msg. - * Raises RuntimeError on failure while publishing a feedback message. - * - * \param[in] pyaction_server Capsule pointing to the action server to publish the message. - * \param[in] pyfeedback_msg The feedback message to publish. - * \return None - */ -void -rclpy_action_publish_feedback(py::capsule pyaction_server, py::object pymsg) -{ - auto action_server = get_pointer(pyaction_server, "rcl_action_server_t"); - 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(); - } - auto raw_ros_message_ptr = std::unique_ptr( - raw_ros_message, destroy_ros_message); - rcl_ret_t ret = rcl_action_publish_feedback(action_server, raw_ros_message); - if (ret != RCL_RET_OK) { - throw rclpy::RCLError("Failed to publish feedback with an action server"); - } -} - -/// Publish a status message from a given action server. -/** - * Raises RuntimeError on failure while publishing a status message. - * - * \param[in] pyaction_server Capsule pointing to the action server to publish the message. - * \return None - */ -void -rclpy_action_publish_status(py::capsule pyaction_server) -{ - auto action_server = get_pointer(pyaction_server, "rcl_action_server_t"); - rcl_action_goal_status_array_t status_message = - rcl_action_get_zero_initialized_goal_status_array(); - rcl_ret_t ret = rcl_action_get_goal_status_array(action_server, &status_message); - if (RCL_RET_OK != ret) { - throw rclpy::RCLError("Failed get goal status array"); - } - - ret = rcl_action_publish_status(action_server, &status_message); - - if (RCL_RET_OK != ret) { - throw rclpy::RCLError("Failed publish goal status array"); - } -} - -rclpy::ActionGoalHandle -rclpy_action_accept_new_goal(py::capsule pyaction_server, py::object pygoal_info_msg) -{ - return rclpy::ActionGoalHandle(pyaction_server, pygoal_info_msg); -} - -void -rclpy_action_notify_goal_done(py::capsule pyaction_server) -{ - auto action_server = get_pointer(pyaction_server, "rcl_action_server_t"); - rcl_ret_t ret = rcl_action_notify_goal_done(action_server); - if (RCL_RET_OK != ret) { - throw rclpy::RCLError("Failed to notfiy action server of goal done"); - } -} - -bool -rclpy_action_server_goal_exists(py::capsule pyaction_server, py::object pygoal_info) -{ - auto action_server = get_pointer(pyaction_server, "rcl_action_server_t"); - destroy_ros_message_signature * destroy_ros_message = NULL; - rcl_action_goal_info_t * goal_info = static_cast( - rclpy_convert_from_py(pygoal_info.ptr(), &destroy_ros_message)); - if (!goal_info) { - throw py::error_already_set(); - } - - auto goal_info_ptr = std::unique_ptr( - goal_info, destroy_ros_message); - - return rcl_action_server_goal_exists(action_server, goal_info); -} - -py::object -rclpy_action_process_cancel_request( - py::capsule pyaction_server, py::object pycancel_request, py::object pycancel_response_type) -{ - auto action_server = get_pointer(pyaction_server, "rcl_action_server_t"); - - destroy_ros_message_signature * destroy_cancel_request = NULL; - rcl_action_cancel_request_t * cancel_request = static_cast( - rclpy_convert_from_py(pycancel_request.ptr(), &destroy_cancel_request)); - if (!cancel_request) { - throw py::error_already_set(); - } - auto cancel_request_ptr = - std::unique_ptr( - cancel_request, destroy_cancel_request); - - rcl_action_cancel_response_t cancel_response = rcl_action_get_zero_initialized_cancel_response(); - rcl_ret_t ret = rcl_action_process_cancel_request( - action_server, cancel_request, &cancel_response); - - if (RCL_RET_OK != ret) { - std::string error_text{"Failed to process cancel request: "}; - error_text += rcl_get_error_string().str; - rcl_reset_error(); - - ret = rcl_action_cancel_response_fini(&cancel_response); - if (RCL_RET_OK != ret) { - error_text += ". Also failed to cleanup response: "; - error_text += rcl_get_error_string().str; - rcl_reset_error(); - } - throw std::runtime_error(error_text); - } - - PyObject * pycancel_response = - rclpy_convert_to_py(&cancel_response.msg, pycancel_response_type.ptr()); - if (!pycancel_response) { - rcl_ret_t ignore = rcl_action_cancel_response_fini(&cancel_response); - (void) ignore; - throw py::error_already_set(); - } - py::object return_value = py::reinterpret_steal(pycancel_response); - - ret = rcl_action_cancel_response_fini(&cancel_response); - - if (RCL_RET_OK != ret) { - throw rclpy::RCLError("Failed to finalize cancel response"); - } - return return_value; -} - -py::tuple -rclpy_action_expire_goals(py::capsule pyaction_server, int64_t max_num_goals) -{ - auto action_server = get_pointer(pyaction_server, "rcl_action_server_t"); - - auto expired_goals = - std::unique_ptr(new rcl_action_goal_info_t[max_num_goals]); - size_t num_expired; - rcl_ret_t ret = rcl_action_expire_goals( - action_server, expired_goals.get(), max_num_goals, &num_expired); - if (RCL_RET_OK != ret) { - throw rclpy::RCLError("Failed to expire goals"); - } - - // Get Python GoalInfo type - py::module pyaction_msgs_module = py::module::import("action_msgs.msg"); - py::object pygoal_info_class = pyaction_msgs_module.attr("GoalInfo"); - py::object pygoal_info_type = pygoal_info_class(); - - // Create a tuple of GoalInfo instances to return - py::tuple result_tuple(num_expired); - - for (size_t i = 0; i < num_expired; ++i) { - result_tuple[i] = py::reinterpret_steal( - rclpy_convert_to_py(&(expired_goals.get()[i]), pygoal_info_type.ptr())); - } - - return result_tuple; -} - - py::object rclpy_action_get_client_names_and_types_by_node( py::capsule pynode, const char * remote_node_name, const char * remote_node_namespace) @@ -892,69 +163,12 @@ namespace rclpy void define_action_api(py::module m) { - m.def( - "rclpy_action_destroy_entity", &rclpy_action_destroy_entity, - "Destroy a rclpy_action entity."); m.def( "rclpy_action_get_rmw_qos_profile", &rclpy_action_get_rmw_qos_profile, "Get an action RMW QoS profile."); - m.def( - "rclpy_action_wait_set_add", &rclpy_action_wait_set_add, - "Add an action entitiy to a wait set."); - m.def( - "rclpy_action_wait_set_get_num_entities", &rclpy_action_wait_set_get_num_entities, - "Get the number of wait set entities for an action entitity."); - m.def( - "rclpy_action_wait_set_is_ready", &rclpy_action_wait_set_is_ready, - "Check if an action entity has any sub-entities ready in a wait set."); - m.def( - "rclpy_action_create_server", &rclpy_action_create_server, - "Create an action server."); - m.def( - "rclpy_action_server_is_available", &rclpy_action_server_is_available, - "Check if an action server is available for a given client."); - m.def( - "rclpy_action_take_goal_request", &rclpy_action_take_goal_request, - "Take a goal request."); - m.def( - "rclpy_action_send_goal_response", &rclpy_action_send_goal_response, - "Send a goal response."); - m.def( - "rclpy_action_take_result_request", &rclpy_action_take_result_request, - "Take a result request."); - m.def( - "rclpy_action_send_result_response", &rclpy_action_send_result_response, - "Send a result response."); - m.def( - "rclpy_action_take_cancel_request", &rclpy_action_take_cancel_request, - "Take a cancel request."); - m.def( - "rclpy_action_send_cancel_response", &rclpy_action_send_cancel_response, - "Send a cancel response."); - m.def( - "rclpy_action_publish_feedback", &rclpy_action_publish_feedback, - "Publish a feedback message."); - m.def( - "rclpy_action_publish_status", &rclpy_action_publish_status, - "Publish a status message."); - m.def( - "rclpy_action_accept_new_goal", &rclpy_action_accept_new_goal, - "Accept a new goal using an action server."); - m.def( - "rclpy_action_notify_goal_done", &rclpy_action_notify_goal_done, - "Notify and action server that a goal has reached a terminal state."); - define_action_goal_handle(m); + define_action_server(m); - m.def( - "rclpy_action_server_goal_exists", &rclpy_action_server_goal_exists, - "Check if a goal being tracked by an action server."); - m.def( - "rclpy_action_process_cancel_request", &rclpy_action_process_cancel_request, - "Process a cancel request to determine what goals should be canceled."); - m.def( - "rclpy_action_expire_goals", &rclpy_action_expire_goals, - "Expire goals associated with an action server."); m.def( "rclpy_action_get_client_names_and_types_by_node", &rclpy_action_get_client_names_and_types_by_node, diff --git a/rclpy/src/rclpy/action_goal_handle.cpp b/rclpy/src/rclpy/action_goal_handle.cpp index 9ca6b840e..45904043c 100644 --- a/rclpy/src/rclpy/action_goal_handle.cpp +++ b/rclpy/src/rclpy/action_goal_handle.cpp @@ -29,28 +29,9 @@ namespace py = pybind11; namespace rclpy { -template -T -get_pointer(py::capsule & capsule, const char * name) -{ - if (strcmp(name, capsule.name())) { - std::string error_text{"Expected capusle with name '"}; - error_text += name; - error_text += "' but got '"; - error_text += capsule.name(); - error_text += "'"; - throw py::value_error(error_text); - } - // TODO(sloretz) use get_pointer() in pybind11 2.6+ - return static_cast(capsule); -} - ActionGoalHandle::ActionGoalHandle( - py::capsule pyaction_server, py::object pygoal_info_msg) + rclpy::ActionServer & action_server, py::object pygoal_info_msg) { - auto action_server = get_pointer( - pyaction_server, "rcl_action_server_t"); - destroy_ros_message_signature * destroy_ros_message = NULL; auto goal_info_msg = static_cast( rclpy_convert_from_py(pygoal_info_msg.ptr(), &destroy_ros_message)); @@ -62,7 +43,8 @@ ActionGoalHandle::ActionGoalHandle( auto goal_info_msg_ptr = std::unique_ptr( goal_info_msg, destroy_ros_message); - auto rcl_handle = rcl_action_accept_new_goal(action_server, goal_info_msg); + auto rcl_handle = rcl_action_accept_new_goal( + action_server.rcl_ptr(), goal_info_msg); if (!rcl_handle) { throw rclpy::RCLError("Failed to accept new goal"); } @@ -117,9 +99,8 @@ void define_action_goal_handle(py::module module) { py::class_>( - module, - "ActionGoalHandle") - .def(py::init()) + module, "ActionGoalHandle") + .def(py::init()) .def_property_readonly( "pointer", [](const ActionGoalHandle & handle) { return reinterpret_cast(handle.rcl_ptr()); diff --git a/rclpy/src/rclpy/action_goal_handle.hpp b/rclpy/src/rclpy/action_goal_handle.hpp index e2c054a35..b1e1bfc94 100644 --- a/rclpy/src/rclpy/action_goal_handle.hpp +++ b/rclpy/src/rclpy/action_goal_handle.hpp @@ -21,6 +21,7 @@ #include +#include "action_server.hpp" #include "destroyable.hpp" #include "handle.hpp" @@ -28,7 +29,10 @@ namespace py = pybind11; namespace rclpy { -class ActionGoalHandle : public Destroyable + +class ActionServer; + +class ActionGoalHandle : public Destroyable, public std::enable_shared_from_this { public: /// Create an action goal handle @@ -42,7 +46,7 @@ class ActionGoalHandle : public Destroyable * \param[in] pyaction_server handle to the action server that is accepting the goal * \param[in] pygoal_info_msg a message containing info about the goal being accepted */ - ActionGoalHandle(py::capsule pyaction_server, py::object pygoal_info_msg); + ActionGoalHandle(rclpy::ActionServer & action_server, py::object pygoal_info_msg); ~ActionGoalHandle() = default; diff --git a/rclpy/src/rclpy/action_server.cpp b/rclpy/src/rclpy/action_server.cpp new file mode 100644 index 000000000..5bc791999 --- /dev/null +++ b/rclpy/src/rclpy/action_server.cpp @@ -0,0 +1,414 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Include pybind11 before rclpy_common/handle.h includes Python.h +#include + +#include +#include + +#include +#include + +#include "rclpy_common/common.h" +#include "rclpy_common/exceptions.hpp" +#include "rclpy_common/handle.h" + +#include "action_server.hpp" + +namespace rclpy +{ + +void +ActionServer::destroy() +{ + rcl_action_server_.reset(); + node_handle_.reset(); +} + +ActionServer::ActionServer( + py::capsule pynode, + const rclpy::Clock & rclpy_clock, + py::object pyaction_type, + const char * action_name, + const rmw_qos_profile_t & goal_service_qos, + const rmw_qos_profile_t & result_service_qos, + const rmw_qos_profile_t & cancel_service_qos, + const rmw_qos_profile_t & feedback_topic_qos, + const rmw_qos_profile_t & status_topic_qos, + double result_timeout) +: node_handle_(std::make_shared(pynode)) +{ + auto node = node_handle_->cast("rcl_node_t"); + + rcl_clock_t * clock = rclpy_clock.rcl_ptr(); + + rosidl_action_type_support_t * ts = static_cast( + rclpy_common_get_type_support(pyaction_type.ptr())); + if (!ts) { + throw py::error_already_set(); + } + + rcl_action_server_options_t action_server_ops = rcl_action_server_get_default_options(); + + action_server_ops.goal_service_qos = goal_service_qos; + action_server_ops.result_service_qos = result_service_qos; + action_server_ops.cancel_service_qos = cancel_service_qos; + action_server_ops.feedback_topic_qos = feedback_topic_qos; + action_server_ops.status_topic_qos = status_topic_qos; + action_server_ops.result_timeout.nanoseconds = (rcl_duration_value_t)RCL_S_TO_NS(result_timeout); + + rcl_action_server_ = std::shared_ptr( + new rcl_action_server_t, + [this](rcl_action_server_t * action_server) + { + auto node = node_handle_->cast_or_warn("rcl_node_t"); + + rcl_ret_t ret = rcl_action_server_fini(action_server, 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 action_server; + }); + + *rcl_action_server_ = rcl_action_get_zero_initialized_server(); + + rcl_ret_t ret = rcl_action_server_init( + rcl_action_server_.get(), + node, + clock, + ts, + action_name, + &action_server_ops); + if (RCL_RET_ACTION_NAME_INVALID == ret) { + std::string error_text{"Failed to create action server due to invalid topic name '"}; + error_text += action_name; + error_text += "' : "; + error_text += rcl_get_error_string().str; + rcl_reset_error(); + throw py::value_error(error_text); + } else if (RCL_RET_OK != ret) { + std::string error_text{"Failed to create action server: "}; + error_text += rcl_get_error_string().str; + rcl_reset_error(); + throw py::value_error(error_text); + } +} + +#define TAKE_SERVICE_REQUEST(Type) \ + /* taken_msg is always destroyed in this function */ \ + auto taken_msg = create_from_py(pymsg_type); \ + rmw_request_id_t header; \ + rcl_ret_t ret = \ + rcl_action_take_ ## Type ## _request(rcl_action_server_.get(), &header, taken_msg.get()); \ + /* Create the tuple to return */ \ + if (ret == RCL_RET_ACTION_CLIENT_TAKE_FAILED || ret == RCL_RET_ACTION_SERVER_TAKE_FAILED) { \ + return py::make_tuple(py::none(), py::none()); \ + } else if (RCL_RET_OK != ret) { \ + throw rclpy::RCLError("Failed to take " #Type); \ + } \ + return py::make_tuple(header, convert_to_py(taken_msg.get(), pymsg_type)); \ + +py::tuple +ActionServer::take_goal_request(py::object pymsg_type) +{ + TAKE_SERVICE_REQUEST(goal) +} + +py::tuple +ActionServer::take_result_request(py::object pymsg_type) +{ + TAKE_SERVICE_REQUEST(result) +} + +#define SEND_SERVICE_RESPONSE(Type) \ + auto ros_response = convert_from_py(pyresponse); \ + rcl_ret_t ret = rcl_action_send_ ## Type ## _response( \ + rcl_action_server_.get(), header, ros_response.get()); \ + if (RCL_RET_OK != ret) { \ + throw rclpy::RCLError("Failed to send " #Type " response"); \ + } + +void +ActionServer::send_goal_response( + rmw_request_id_t * header, py::object pyresponse) +{ + SEND_SERVICE_RESPONSE(goal) +} + +void +ActionServer::send_result_response( + rmw_request_id_t * header, py::object pyresponse) +{ + SEND_SERVICE_RESPONSE(result) +} + +py::tuple +ActionServer::take_cancel_request(py::object pymsg_type) +{ + TAKE_SERVICE_REQUEST(cancel) +} + +void +ActionServer::send_cancel_response( + rmw_request_id_t * header, py::object pyresponse) +{ + SEND_SERVICE_RESPONSE(cancel) +} + +void +ActionServer::publish_feedback(py::object pymsg) +{ + auto ros_message = convert_from_py(pymsg); + rcl_ret_t ret = rcl_action_publish_feedback(rcl_action_server_.get(), ros_message.get()); + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed to publish feedback with an action server"); + } +} + +void +ActionServer::publish_status() +{ + rcl_action_goal_status_array_t status_message = + rcl_action_get_zero_initialized_goal_status_array(); + rcl_ret_t ret = rcl_action_get_goal_status_array(rcl_action_server_.get(), &status_message); + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed get goal status array"); + } + + ret = rcl_action_publish_status(rcl_action_server_.get(), &status_message); + + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed publish goal status array"); + } +} + +void +ActionServer::notify_goal_done() +{ + rcl_ret_t ret = rcl_action_notify_goal_done(rcl_action_server_.get()); + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed to notfiy action server of goal done"); + } +} + +bool +ActionServer::goal_exists(py::object pygoal_info) +{ + auto goal_info = convert_from_py(pygoal_info); + rcl_action_goal_info_t * goal_info_type = static_cast(goal_info.get()); + return rcl_action_server_goal_exists(rcl_action_server_.get(), goal_info_type); +} + +py::tuple +ActionServer::get_num_entities() +{ + size_t num_subscriptions = 0u; + size_t num_guard_conditions = 0u; + size_t num_timers = 0u; + size_t num_clients = 0u; + size_t num_services = 0u; + + rcl_ret_t ret = rcl_action_server_wait_set_get_num_entities( + rcl_action_server_.get(), + &num_subscriptions, + &num_guard_conditions, + &num_timers, + &num_clients, + &num_services); + + if (RCL_RET_OK != ret) { + std::string error_text{"Failed to get number of entities for 'rcl_action_server_t'"}; + throw rclpy::RCLError(error_text); + } + + py::tuple result_tuple(5); + result_tuple[0] = py::int_(num_subscriptions); + result_tuple[1] = py::int_(num_guard_conditions); + result_tuple[2] = py::int_(num_timers); + result_tuple[3] = py::int_(num_clients); + result_tuple[4] = py::int_(num_services); + return result_tuple; +} + +py::tuple +ActionServer::is_ready(py::capsule pywait_set) +{ + auto wait_set = static_cast(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, + rcl_action_server_.get(), + &is_goal_request_ready, + &is_cancel_request_ready, + &is_result_request_ready, + &is_goal_expired); + + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed to get number of ready entities for action server"); + } + + py::tuple result_tuple(4); + result_tuple[0] = py::bool_(is_goal_request_ready); + result_tuple[1] = py::bool_(is_cancel_request_ready); + result_tuple[2] = py::bool_(is_result_request_ready); + result_tuple[3] = py::bool_(is_goal_expired); + return result_tuple; +} + +void +ActionServer::add_to_waitset(py::capsule pywait_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); + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed to add 'rcl_action_server_t' to wait set"); + } +} + +py::object +ActionServer::process_cancel_request( + py::object pycancel_request, py::object pycancel_response_type) +{ + auto cancel_request = convert_from_py(pycancel_request); + rcl_action_cancel_request_t * cancel_request_tmp = static_cast( + cancel_request.get()); + + rcl_action_cancel_response_t cancel_response = rcl_action_get_zero_initialized_cancel_response(); + rcl_ret_t ret = rcl_action_process_cancel_request( + rcl_action_server_.get(), cancel_request_tmp, &cancel_response); + + if (RCL_RET_OK != ret) { + std::string error_text{"Failed to process cancel request: "}; + error_text += rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_action_cancel_response_fini(&cancel_response); + if (RCL_RET_OK != ret) { + error_text += ". Also failed to cleanup response: "; + error_text += rcl_get_error_string().str; + rcl_reset_error(); + } + throw std::runtime_error(error_text); + } + + py::object return_value = convert_to_py(&cancel_response.msg, pycancel_response_type); + RCPPUTILS_SCOPE_EXIT( + { + ret = rcl_action_cancel_response_fini(&cancel_response); + + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed to finalize cancel response"); + } + }); + return return_value; +} + +py::tuple +ActionServer::expire_goals(int64_t max_num_goals) +{ + auto expired_goals = + std::unique_ptr(new rcl_action_goal_info_t[max_num_goals]); + size_t num_expired; + rcl_ret_t ret = rcl_action_expire_goals( + rcl_action_server_.get(), expired_goals.get(), max_num_goals, &num_expired); + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed to expire goals"); + } + + // Get Python GoalInfo type + py::module pyaction_msgs_module = py::module::import("action_msgs.msg"); + py::object pygoal_info_class = pyaction_msgs_module.attr("GoalInfo"); + py::object pygoal_info_type = pygoal_info_class(); + + // Create a tuple of GoalInfo instances to return + py::tuple result_tuple(num_expired); + + for (size_t i = 0; i < num_expired; ++i) { + result_tuple[i] = + convert_to_py(&(expired_goals.get()[i]), pygoal_info_type); + } + + return result_tuple; +} + +void +define_action_server(py::object module) +{ + py::class_>(module, "ActionServer") + .def( + py::init()) + .def_property_readonly( + "pointer", [](const ActionServer & action_server) { + return reinterpret_cast(action_server.rcl_ptr()); + }, + "Get the address of the entity as an integer") + .def( + "take_goal_request", &ActionServer::take_goal_request, + "Take an action goal request.") + .def( + "send_goal_response", &ActionServer::send_goal_response, + "Send an action goal response.") + .def( + "send_result_response", &ActionServer::send_result_response, + "Send an action result response.") + .def( + "take_cancel_request", &ActionServer::take_cancel_request, + "Take an action cancel request.") + .def( + "take_result_request", &ActionServer::take_result_request, + "Take an action result request.") + .def( + "send_cancel_response", &ActionServer::send_cancel_response, + "Send an action cancel response.") + .def( + "publish_feedback", &ActionServer::publish_feedback, + " Publish a feedback message from a given action server.") + .def( + "publish_status", &ActionServer::publish_status, + "Publish a status message from a given action server.") + .def( + "notify_goal_done", &ActionServer::notify_goal_done, + "Notify goal is done.") + .def( + "goal_exists", &ActionServer::goal_exists, + "Check is a goal exists in the server.") + .def( + "process_cancel_request", &ActionServer::process_cancel_request, + "Process a cancel request") + .def( + "expire_goals", &ActionServer::expire_goals, + "Expired goals.") + .def( + "get_num_entities", &ActionServer::get_num_entities, + "Get the number of wait set entities that make up an action entity.") + .def( + "is_ready", &ActionServer::is_ready, + "Check if an action entity has any ready wait set entities.") + .def( + "add_to_waitset", &ActionServer::add_to_waitset, + "Add an action entitiy to a wait set."); +} + +} // namespace rclpy diff --git a/rclpy/src/rclpy/action_server.hpp b/rclpy/src/rclpy/action_server.hpp new file mode 100644 index 000000000..f5008a9dd --- /dev/null +++ b/rclpy/src/rclpy/action_server.hpp @@ -0,0 +1,264 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLPY__ACTION_SERVER_HPP_ +#define RCLPY__ACTION_SERVER_HPP_ + +#include + +#include + +#include + +#include "clock.hpp" +#include "destroyable.hpp" +#include "handle.hpp" + +namespace py = pybind11; + +namespace rclpy +{ +/* + * This class will create an action server for the given action name. + * This client will use the typesupport defined in the action module + * provided as pyaction_type to send messages. + */ +class ActionServer : public Destroyable, public std::enable_shared_from_this +{ +public: + /// Create an action server. + /** + * Raises AttributeError if action type is invalid + * Raises ValueError if action name is invalid + * Raises RuntimeError if the action server could not be created. + * + * \param[in] pynode Capsule pointing to the node to add the action server to. + * \param[in] rclpy_clock Clock use to create the action server. + * \param[in] pyaction_type Action module associated with the action server. + * \param[in] pyaction_name Python object containing the action name. + * \param[in] goal_service_qos rmw_qos_profile_t object for the goal service. + * \param[in] result_service_qos rmw_qos_profile_t object for the result service. + * \param[in] cancel_service_qos rmw_qos_profile_t object for the cancel service. + * \param[in] feedback_qos rmw_qos_profile_t object for the feedback subscriber. + * \param[in] status_qos rmw_qos_profile_t object for the status subscriber. + */ + ActionServer( + py::capsule pynode, + const rclpy::Clock & rclpy_clock, + py::object pyaction_type, + const char * action_name, + const rmw_qos_profile_t & goal_service_qos, + const rmw_qos_profile_t & result_service_qos, + const rmw_qos_profile_t & cancel_service_qos, + const rmw_qos_profile_t & feedback_topic_qos, + const rmw_qos_profile_t & status_topic_qos, + double result_timeout); + + /// Take an action goal request. + /** + * Raises RCLError if an error occurs in rcl + * Raises RuntimeError on failure. + * + * \param[in] pymsg_type An instance of the type of request message to take. + * \return 2-tuple (header, received request message) where the header is an + * "rclpy.rmw_request_id_t" type, or + * \return 2-tuple (None, None) if there as no message to take + */ + py::tuple + take_goal_request(py::object pymsg_type); + + /// Send an action goal response. + /** + * Raises RCLError if an error occurs in rcl + * Raises RuntimeError on failure. + * + * \param[in] header Pointer to the message header. + * \param[in] pyresponse The response message to send. + */ + void + send_goal_response( + rmw_request_id_t * header, py::object pyresponse); + + /// Send an action result response. + /** + * Raises RCLError if an error occurs in rcl + * Raises RuntimeError on failure. + * + * \param[in] pyheader Pointer to the message header. + * \param[in] pyresponse The response message to send. + */ + void + send_result_response( + rmw_request_id_t * header, py::object pyresponse); + + /// Take an action cancel request. + /** + * Raises RCLError if an error occurs in rcl + * Raises RuntimeError on failure. + * + * \param[in] pymsg_type An instance of the type of request message to take. + * \return 2-tuple (header, received request message) where the header is an + * "rmw_request_id_t" type, or + * \return 2-tuple (None, None) if there as no message to take + */ + py::tuple + take_cancel_request(py::object pymsg_type); + + /// Take an action result request. + /** + * Raises RCLError if an error occurs in rcl + * Raises RuntimeError on failure. + * + * \param[in] pymsg_type An instance of the type of request message to take. + * \return 2-tuple (header, received request message) where the header is an + * "rclpy.rmw_request_id_t" type, or + * \return 2-tuple (None, None) if there as no message to take + */ + py::tuple + take_result_request(py::object pymsg_type); + + /// Send an action cancel response. + /** + * Raises RCLError if an error occurs in rcl + * Raises RuntimeError on failure. + * + * \param[in] pyheader Pointer to the message header. + * \param[in] pyresponse The response message to send. + */ + void + send_cancel_response( + rmw_request_id_t * header, py::object pyresponse); + + /// Publish a feedback message from a given action server. + /** + * Raises RCLError if an error occurs in rcl + * Raises RuntimeError on failure while publishing a feedback message. + * + * \param[in] pymsg The feedback message to publish. + */ + void + publish_feedback(py::object pymsg); + + /// Publish a status message from a given action server. + /** + * Raises RCLError if an error occurs in rcl + * Raises RuntimeError on failure while publishing a status message. + */ + void + publish_status(); + + /// Notifies action server that a goal handle reached a terminal state. + /** + * Raises RCLError if an error occurs in rcl + */ + void + notify_goal_done(); + + /// Check if a goal is already being tracked by an action server. + /* + * Raises AttributeError if there is an issue parsing the pygoal_info. + * + * \param[in] pygoal_info the identifiers of goals that expired, or set to `NULL` if unused + * \return True if the goal exists, false otherwise. + */ + bool + goal_exists(py::object pygoal_info); + + /// Process a cancel request using an action server. + /** + * This is a non-blocking call. + * + * Raises RuntimeError on failure while publishing a status message. + * Raises RCLError if an error occurs in rcl + * + * \return the cancel response message + */ + py::object + process_cancel_request( + py::object pycancel_request, py::object pycancel_response_type); + + /// Expires goals associated with an action server. + py::tuple + expire_goals(int64_t max_num_goals); + + /// Get the number of wait set entities that make up an action entity. + /** + * Raises RCLError if an error occurs in rcl + * + * \return Tuple containing the number of wait set entities: + * (num_subscriptions, + * num_guard_conditions, + * num_timers, + * num_clients, + * num_services) + */ + py::tuple + get_num_entities(); + + /// Check if an action entity has any ready wait set entities. + /** + * This must be called after waiting on the wait set. + * Raises RuntimeError on failure. + * Raises RCLError if an error occurs in rcl + * + * \param[in] pywait_set Capsule pointing to the wait set structure. + * \return A tuple of booleans representing ready sub-entities. + * (is_goal_request_ready, + * is_cancel_request_ready, + * is_result_request_ready, + * is_goal_expired) + */ + py::tuple + is_ready(py::capsule pywait_set); + + /// Add an action entitiy to a wait set. + /** + * Raises RuntimeError on failure. + * Raises RCLError if an error occurs in rcl + * + * \param[in] pywait_set Capsule pointer to an rcl_wait_set_t. + */ + void + add_to_waitset(py::capsule pywait_set); + + /// Get rcl_client_t pointer + rcl_action_server_t * + rcl_ptr() const + { + return rcl_action_server_.get(); + } + + /// Get rcl_client_t pointer + std::shared_ptr + get_rcl_shared_ptr() + { + return rcl_action_server_; + } + + /// Force an early destruction of this object + void + destroy() override; + +private: + std::shared_ptr node_handle_; + std::shared_ptr rcl_action_server_; +}; +/// Define a pybind11 wrapper for an rcl_time_point_t +/** + * \param[in] module a pybind11 module to add the definition to + */ +void define_action_server(py::object module); +} // namespace rclpy + +#endif // RCLPY__ACTION_SERVER_HPP_ From 9e4e09cd929ab81427cc3845323f6f71426d21e9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 14 Apr 2021 20:44:42 +0200 Subject: [PATCH 05/10] Convert WaitSet to use C++ Classes (#769) * Convert WaitSet to use C++ Classes Signed-off-by: ahcorde * added feedback Signed-off-by: ahcorde --- rclpy/rclpy/executors.py | 29 ++- rclpy/rclpy/qos_event.py | 4 +- rclpy/src/rclpy/_rclpy_pybind11.cpp | 38 +--- rclpy/src/rclpy/action_client.cpp | 12 +- rclpy/src/rclpy/action_client.hpp | 9 +- rclpy/src/rclpy/action_server.cpp | 11 +- rclpy/src/rclpy/action_server.hpp | 5 +- rclpy/src/rclpy/wait_set.cpp | 252 ++++++++++++------------ rclpy/src/rclpy/wait_set.hpp | 289 ++++++++++++++-------------- rclpy/test/test_qos_event.py | 46 ++--- rclpy/test/test_waitable.py | 24 +-- 11 files changed, 340 insertions(+), 379 deletions(-) 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): From 3777953b083fc20f48377eab752f8c04d55095ae Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 15 Apr 2021 16:58:26 +0200 Subject: [PATCH 06/10] Removed unused structs (#770) Signed-off-by: ahcorde --- .../rclpy_common/include/rclpy_common/common.h | 18 ------------------ 1 file changed, 18 deletions(-) diff --git a/rclpy/src/rclpy_common/include/rclpy_common/common.h b/rclpy/src/rclpy_common/include/rclpy_common/common.h index 1f048f7f6..606f5ad0e 100644 --- a/rclpy/src/rclpy_common/include/rclpy_common/common.h +++ b/rclpy/src/rclpy_common/include/rclpy_common/common.h @@ -32,24 +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 client must be first in the struct to compare cli.handle.pointer to an address - // in a wait set. - rcl_client_t client; - rcl_node_t * node; -} rclpy_client_t; - -typedef struct -{ - // Important: a pointer to a structure is also a pointer to its first member. - // The service must be first in the struct to compare srv.handle.pointer to an address - // in a wait set. - rcl_service_t service; - rcl_node_t * node; -} rclpy_service_t; - /// Finalize names and types struct with error setting. /** * \param[in] names_and_types The struct to finalize. From 65e9a4b5823fd8ed3311cfdfd996ba15d00f36b9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 16 Apr 2021 18:29:32 +0200 Subject: [PATCH 07/10] Convert Guardcondition to use C++ classes (#772) * Convert Guardcondition to use C++ classes Signed-off-by: ahcorde * make destroy guard condition capsule function private Signed-off-by: ahcorde --- rclpy/rclpy/executors.py | 9 +-- rclpy/rclpy/guard_condition.py | 11 ++- rclpy/rclpy/signals.py | 10 +-- rclpy/src/rclpy/_rclpy_pybind11.cpp | 9 +-- rclpy/src/rclpy/guard_condition.cpp | 108 ++++++++++++---------------- rclpy/src/rclpy/guard_condition.hpp | 82 +++++++++++++++------ rclpy/src/rclpy/wait_set.cpp | 14 ++++ rclpy/src/rclpy/wait_set.hpp | 11 +++ rclpy/test/test_waitable.py | 7 +- 9 files changed, 151 insertions(+), 110 deletions(-) diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py index 9595924f9..bfd0b2fe0 100644 --- a/rclpy/rclpy/executors.py +++ b/rclpy/rclpy/executors.py @@ -541,10 +541,11 @@ def _wait_for_ready_callbacks( except InvalidHandle: entity_count.num_timers -= 1 - guard_capsules = [] + guard_handles = [] for gc in guards: try: - guard_capsules.append(context_stack.enter_context(gc.handle)) + context_stack.enter_context(gc.handle) + guard_handles.append(gc.handle) except InvalidHandle: entity_count.num_guard_conditions -= 1 @@ -574,8 +575,8 @@ def _wait_for_ready_callbacks( wait_set.add_service(srv_capsule) for tmr_handle in timer_handles: wait_set.add_timer(tmr_handle) - for gc_capsule in guard_capsules: - wait_set.add_entity('guard_condition', gc_capsule) + for gc_handle in guard_handles: + wait_set.add_guard_condition(gc_handle) for waitable in waitables: waitable.add_to_wait_set(wait_set) diff --git a/rclpy/rclpy/guard_condition.py b/rclpy/rclpy/guard_condition.py index 319b55fb8..b8ba32664 100644 --- a/rclpy/rclpy/guard_condition.py +++ b/rclpy/rclpy/guard_condition.py @@ -12,7 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -from rclpy.handle import Handle from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.utilities import get_default_context @@ -22,7 +21,7 @@ class GuardCondition: def __init__(self, callback, callback_group, context=None): self._context = get_default_context() if context is None else context with self._context.handle as capsule: - self.__handle = Handle(_rclpy.rclpy_create_guard_condition(capsule)) + self.__gc = _rclpy.GuardCondition(capsule) self.callback = callback self.callback_group = callback_group # True when the callback is ready to fire but has not been "taken" by an executor @@ -31,12 +30,12 @@ def __init__(self, callback, callback_group, context=None): self._executor_triggered = False def trigger(self): - with self.handle as capsule: - _rclpy.rclpy_trigger_guard_condition(capsule) + with self.__gc: + self.__gc.trigger_guard_condition() @property def handle(self): - return self.__handle + return self.__gc def destroy(self): - self.handle.destroy() + self.handle.destroy_when_not_in_use() diff --git a/rclpy/rclpy/signals.py b/rclpy/rclpy/signals.py index 0dc425af7..df47ffc18 100644 --- a/rclpy/rclpy/signals.py +++ b/rclpy/rclpy/signals.py @@ -21,8 +21,9 @@ class SignalHandlerGuardCondition(GuardCondition): def __init__(self, context=None): super().__init__(callback=None, callback_group=None, context=context) - with self.handle as capsule: - _signals.rclpy_register_sigint_guard_condition(capsule) + with self.handle: + # TODO(ahcorde): Remove the pycapsule method when #728 is in + _signals.rclpy_register_sigint_guard_condition(self.handle.pycapsule()) def __del__(self): try: @@ -35,6 +36,7 @@ def __del__(self): pass def destroy(self): - with self.handle as capsule: - _signals.rclpy_unregister_sigint_guard_condition(capsule) + with self.handle: + # TODO(ahcorde): Remove the pycapsule method when #728 is in + _signals.rclpy_unregister_sigint_guard_condition(self.handle.pycapsule()) super().destroy() diff --git a/rclpy/src/rclpy/_rclpy_pybind11.cpp b/rclpy/src/rclpy/_rclpy_pybind11.cpp index 1f0ad401e..19512db24 100644 --- a/rclpy/src/rclpy/_rclpy_pybind11.cpp +++ b/rclpy/src/rclpy/_rclpy_pybind11.cpp @@ -139,15 +139,8 @@ PYBIND11_MODULE(_rclpy_pybind11, m) { "rclpy_qos_check_compatible", &rclpy::qos_check_compatible, "Check if two QoS profiles are compatible."); - m.def( - "rclpy_create_guard_condition", &rclpy::guard_condition_create, - "Create a general purpose guard condition"); - m.def( - "rclpy_trigger_guard_condition", &rclpy::guard_condition_trigger, - "Trigger a general purpose guard condition"); - + rclpy::define_guard_condition(m); rclpy::define_timer(m); - rclpy::define_subscription(m); rclpy::define_time_point(m); rclpy::define_clock(m); diff --git a/rclpy/src/rclpy/guard_condition.cpp b/rclpy/src/rclpy/guard_condition.cpp index d5f6e43f6..5c0d4a302 100644 --- a/rclpy/src/rclpy/guard_condition.cpp +++ b/rclpy/src/rclpy/guard_condition.cpp @@ -31,32 +31,7 @@ namespace rclpy { -/// Handle destructor for guard condition -static void -_rclpy_destroy_guard_condition(void * p) -{ - auto gc = static_cast(p); - if (!gc) { - // Warning should use line number of the current stack frame - int stack_level = 1; - PyErr_WarnFormat( - PyExc_RuntimeWarning, stack_level, "_rclpy_destroy_guard_condition got NULL pointer"); - return; - } - - rcl_ret_t ret = rcl_guard_condition_fini(gc); - if (RCL_RET_OK != ret) { - // Warning should use line number of the current stack frame - int stack_level = 1; - PyErr_WarnFormat( - PyExc_RuntimeWarning, stack_level, "Failed to fini guard condition: %s", - rcl_get_error_string().str); - } - PyMem_Free(gc); -} - -py::capsule -guard_condition_create(py::capsule pycontext) +GuardCondition::GuardCondition(py::capsule pycontext) { auto context = static_cast( rclpy_handle_get_pointer_from_capsule(pycontext.ptr(), "rcl_context_t")); @@ -64,56 +39,61 @@ guard_condition_create(py::capsule pycontext) throw py::error_already_set(); } - // Use smart pointer to make sure memory is free'd on error - auto deleter = [](rcl_guard_condition_t * ptr) {_rclpy_destroy_guard_condition(ptr);}; - auto gc = std::unique_ptr( - static_cast(PyMem_Malloc(sizeof(rcl_guard_condition_t))), - deleter); - if (!gc) { - throw std::bad_alloc(); - } - - *gc = rcl_get_zero_initialized_guard_condition(); + rcl_guard_condition_ = std::shared_ptr( + new rcl_guard_condition_t, + [](rcl_guard_condition_t * guard_condition) + { + rcl_ret_t ret = rcl_guard_condition_fini(guard_condition); + if (RCL_RET_OK != ret) { + // Warning should use line number of the current stack frame + int stack_level = 1; + PyErr_WarnFormat( + PyExc_RuntimeWarning, stack_level, "Failed to fini guard condition: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + delete guard_condition; + }); + + *rcl_guard_condition_ = rcl_get_zero_initialized_guard_condition(); rcl_guard_condition_options_t gc_options = rcl_guard_condition_get_default_options(); - rcl_ret_t ret = rcl_guard_condition_init(gc.get(), context, gc_options); + rcl_ret_t ret = rcl_guard_condition_init(rcl_guard_condition_.get(), context, gc_options); if (ret != RCL_RET_OK) { throw RCLError("failed to create guard_condition"); } - - PyObject * pygc_c = - rclpy_create_handle_capsule(gc.get(), "rcl_guard_condition_t", _rclpy_destroy_guard_condition); - if (!pygc_c) { - throw py::error_already_set(); - } - auto pygc = py::reinterpret_steal(pygc_c); - // pygc now owns the rcl_guard_contition_t - gc.release(); - - auto gc_handle = static_cast(pygc); - auto context_handle = static_cast(pycontext); - _rclpy_handle_add_dependency(gc_handle, context_handle); - if (PyErr_Occurred()) { - _rclpy_handle_dec_ref(gc_handle); - throw py::error_already_set(); - } - - return pygc; } void -guard_condition_trigger(py::capsule pygc) +GuardCondition::destroy() { - auto gc = static_cast( - rclpy_handle_get_pointer_from_capsule(pygc.ptr(), "rcl_guard_condition_t")); - if (!gc) { - throw py::error_already_set(); - } + rcl_guard_condition_.reset(); +} - rcl_ret_t ret = rcl_trigger_guard_condition(gc); +void +GuardCondition::trigger_guard_condition() +{ + rcl_ret_t ret = rcl_trigger_guard_condition(rcl_guard_condition_.get()); - if (ret != RCL_RET_OK) { + if (RCL_RET_OK != ret) { throw RCLError("failed to trigger guard condition"); } } + +void define_guard_condition(py::object module) +{ + py::class_>(module, "GuardCondition") + .def(py::init()) + .def_property_readonly( + "pointer", [](const GuardCondition & guard_condition) { + return reinterpret_cast(guard_condition.rcl_ptr()); + }, + "Get the address of the entity as an integer") + .def( + "trigger_guard_condition", &GuardCondition::trigger_guard_condition, + "Trigger a general purpose guard condition") + .def( + "pycapsule", &GuardCondition::pycapsule, + "Return a pycapsule object with the guardcondition pointer"); +} } // namespace rclpy diff --git a/rclpy/src/rclpy/guard_condition.hpp b/rclpy/src/rclpy/guard_condition.hpp index 4a61c9be9..f23c5e266 100644 --- a/rclpy/src/rclpy/guard_condition.hpp +++ b/rclpy/src/rclpy/guard_condition.hpp @@ -17,31 +17,73 @@ #include +#include +#include + +#include + +#include "destroyable.hpp" +#include "rclpy_common/exceptions.hpp" +#include "rclpy_common/handle.h" +#include "utils.hpp" + namespace py = pybind11; namespace rclpy { /// Create a general purpose guard condition -/** - * A successful call will return a Capsule with the pointer of the created - * rcl_guard_condition_t * structure - * - * Raises RuntimeError if initializing the guard condition fails - * - * \return a guard condition capsule - */ -py::capsule -guard_condition_create(py::capsule pycontext); - -/// Trigger a general purpose guard condition -/** - * Raises ValueError if pygc is not a guard condition capsule - * Raises RCLError if the guard condition could not be triggered - * - * \param[in] pygc Capsule pointing to guard condtition - */ -void -guard_condition_trigger(py::capsule pygc); +class GuardCondition : public Destroyable, public std::enable_shared_from_this +{ +public: + /** + * Raises RuntimeError if initializing the guard condition fails + */ + explicit GuardCondition(py::capsule pycontext); + + /// Trigger a general purpose guard condition + /** + * Raises ValueError if pygc is not a guard condition capsule + * Raises RCLError if the guard condition could not be triggered + */ + void + trigger_guard_condition(); + + /// Get rcl_client_t pointer + rcl_guard_condition_t * rcl_ptr() const + { + return rcl_guard_condition_.get(); + } + + // TODO(ahcorde): Remove the pycapsule method when #728 is in + /// Return a pycapsule object to be able to handle the signal in C. + py::capsule + pycapsule() const + { + PyObject * capsule = rclpy_create_handle_capsule( + rcl_guard_condition_.get(), "rcl_guard_condition_t", _rclpy_destroy_guard_condition); + if (!capsule) { + throw py::error_already_set(); + } + return py::reinterpret_steal(capsule); + } + + /// Force an early destruction of this object + void destroy() override; + +private: + std::shared_ptr rcl_guard_condition_; + + /// Handle destructor for guard condition + static void + _rclpy_destroy_guard_condition(void * p) + { + (void)p; + // Empty destructor, the class should take care of the lifecycle. + } +}; + +/// Define a pybind11 wrapper for an rclpy::Service +void define_guard_condition(py::object module); } // namespace rclpy #endif // RCLPY__GUARD_CONDITION_HPP_ diff --git a/rclpy/src/rclpy/wait_set.cpp b/rclpy/src/rclpy/wait_set.cpp index 273856427..e901b8ec4 100644 --- a/rclpy/src/rclpy/wait_set.cpp +++ b/rclpy/src/rclpy/wait_set.cpp @@ -178,6 +178,17 @@ WaitSet::add_timer(const Timer & timer) return index; } +size_t +WaitSet::add_guard_condition(const GuardCondition & gc) +{ + size_t index; + rcl_ret_t ret = rcl_wait_set_add_guard_condition(rcl_wait_set_.get(), gc.rcl_ptr(), &index); + if (RCL_RET_OK != ret) { + throw RCLError("failed to add guard condition to wait set"); + } + return index; +} + size_t WaitSet::add_client(const Client & client) { @@ -323,6 +334,9 @@ void define_waitset(py::object module) .def( "add_client", &WaitSet::add_client, "Add a client to the wait set structure") + .def( + "add_guard_condition", &WaitSet::add_guard_condition, + "Add a guard condition to the wait set structure") .def( "add_timer", &WaitSet::add_timer, "Add a timer to the wait set structure") diff --git a/rclpy/src/rclpy/wait_set.hpp b/rclpy/src/rclpy/wait_set.hpp index bf5923999..6e10cc16e 100644 --- a/rclpy/src/rclpy/wait_set.hpp +++ b/rclpy/src/rclpy/wait_set.hpp @@ -22,6 +22,7 @@ #include "client.hpp" #include "destroyable.hpp" +#include "guard_condition.hpp" #include "qos_event.hpp" #include "service.hpp" #include "subscription.hpp" @@ -107,6 +108,16 @@ class WaitSet : public Destroyable, public std::enable_shared_from_this size_t add_client(const Client & client); + /// Add a guard condition to the wait set structure + /** + * Raises RCLError if any lower level error occurs + * + * \param[in] timer a guard condition to add to the wait set + * \return Index in waitset entity was added at + */ + size_t + add_guard_condition(const GuardCondition & gc); + /// Add a timer to the wait set structure /** * Raises RCLError if any lower level error occurs diff --git a/rclpy/test/test_waitable.py b/rclpy/test/test_waitable.py index 18603fdd0..c7f422de0 100644 --- a/rclpy/test/test_waitable.py +++ b/rclpy/test/test_waitable.py @@ -220,7 +220,7 @@ def __init__(self, node): super().__init__(ReentrantCallbackGroup()) with node.context.handle as context_capsule: - self.guard_condition = _rclpy.rclpy_create_guard_condition(context_capsule) + self.guard_condition = _rclpy.GuardCondition(context_capsule) self.guard_condition_index = None self.guard_is_ready = False @@ -253,8 +253,7 @@ def get_num_entities(self): def add_to_wait_set(self, wait_set): """Add entities to wait set.""" - self.guard_condition_index = wait_set.add_entity( - 'guard_condition', self.guard_condition) + self.guard_condition_index = wait_set.add_guard_condition(self.guard_condition) class MutuallyExclusiveWaitable(Waitable): @@ -369,7 +368,7 @@ def test_waitable_with_guard_condition(self): self.node.add_waitable(self.waitable) thr = self.start_spin_thread(self.waitable) - _rclpy.rclpy_trigger_guard_condition(self.waitable.guard_condition) + self.waitable.guard_condition.trigger_guard_condition() thr.join() assert self.waitable.future.done() From fb619b7806e3a5cea9e2ce96f4c8a80cfea588d9 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 16 Apr 2021 09:39:55 -0700 Subject: [PATCH 08/10] Pybind11 action goal handle nitpicks (#767) * Add GoalEvent back to module Signed-off-by: Shane Loretz * self._goal -> self._goal_handle Signed-off-by: Shane Loretz * Remove unused API Signed-off-by: Shane Loretz Co-authored-by: ahcorde --- rclpy/rclpy/action/server.py | 27 +++++++++++++++------------ 1 file changed, 15 insertions(+), 12 deletions(-) diff --git a/rclpy/rclpy/action/server.py b/rclpy/rclpy/action/server.py index ef9ded1d7..72632dd5c 100644 --- a/rclpy/rclpy/action/server.py +++ b/rclpy/rclpy/action/server.py @@ -43,6 +43,9 @@ class CancelResponse(Enum): ACCEPT = 2 +GoalEvent = _rclpy.GoalEvent + + class ServerGoalHandle: """Goal handle for working with Action Servers.""" @@ -59,7 +62,7 @@ def __init__(self, action_server, goal_info, goal_request): :param goal_info: GoalInfo message. :param goal_request: The user defined goal request message from an ActionClient. """ - self._goal = _rclpy.ActionGoalHandle(action_server._handle, goal_info) + self._goal_handle = _rclpy.ActionGoalHandle(action_server._handle, goal_info) self._action_server = action_server self._goal_info = goal_info self._goal_request = goal_request @@ -85,9 +88,9 @@ def goal_id(self): @property def is_active(self): with self._lock: - if self._goal is None: + if self._goal_handle is None: return False - return self._goal.is_active() + return self._goal_handle.is_active() @property def is_cancel_requested(self): @@ -96,24 +99,24 @@ def is_cancel_requested(self): @property def status(self): with self._lock: - if self._goal is None: + if self._goal_handle is None: return GoalStatus.STATUS_UNKNOWN - return self._goal.get_status() + return self._goal_handle.get_status() def _update_state(self, event): with self._lock: # Ignore updates for already destructed goal handles - if self._goal is None: + if self._goal_handle is None: return # Update state - self._goal.update_goal_state(event) + self._goal_handle.update_goal_state(event) # Publish state change self._action_server._handle.publish_status() # If it's a terminal state, then also notify the action server - if not self._goal.is_active(): + if not self._goal_handle.is_active(): self._action_server.notify_goal_done() def execute(self, execute_callback=None): @@ -130,7 +133,7 @@ def publish_feedback(self, feedback): with self._lock: # Ignore for already destructed goal handles - if self._goal is None: + if self._goal_handle is None: return # Populate the feedback message with metadata about this goal @@ -153,10 +156,10 @@ def canceled(self): def destroy(self): with self._lock: - if self._goal is None: + if self._goal_handle is None: return - self._goal.destroy_when_not_in_use() - self._goal = None + self._goal_handle.destroy_when_not_in_use() + self._goal_handle = None self._action_server.remove_future(self._result_future) From 32819424aa3c53a4ca3cc983a157e6e52b1c38fe Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 26 Apr 2021 08:40:49 +0200 Subject: [PATCH 09/10] Pybind11 actionserver nitpicks and docblock improvements (#774) Signed-off-by: ahcorde --- rclpy/src/rclpy/_rclpy_action.cpp | 9 --------- rclpy/src/rclpy/_rclpy_pybind11.cpp | 6 ++++++ rclpy/src/rclpy/action_client.hpp | 2 +- rclpy/src/rclpy/action_server.cpp | 27 ++++++++++----------------- rclpy/src/rclpy/action_server.hpp | 9 +-------- rclpy/src/rclpy/clock.hpp | 2 +- rclpy/src/rclpy/guard_condition.hpp | 2 +- rclpy/src/rclpy/publisher.hpp | 2 +- rclpy/src/rclpy/service.hpp | 2 +- rclpy/src/rclpy/subscription.hpp | 2 +- rclpy/src/rclpy/timer.hpp | 2 +- rclpy/src/rclpy/wait_set.hpp | 2 +- 12 files changed, 25 insertions(+), 42 deletions(-) diff --git a/rclpy/src/rclpy/_rclpy_action.cpp b/rclpy/src/rclpy/_rclpy_action.cpp index 8b5121921..ae140bcef 100644 --- a/rclpy/src/rclpy/_rclpy_action.cpp +++ b/rclpy/src/rclpy/_rclpy_action.cpp @@ -26,11 +26,6 @@ #include "rclpy_common/common.h" #include "rclpy_common/handle.h" -#include "action_client.hpp" -#include "action_goal_handle.hpp" -#include "action_server.hpp" -#include "clock.hpp" - namespace py = pybind11; @@ -166,8 +161,6 @@ define_action_api(py::module m) m.def( "rclpy_action_get_rmw_qos_profile", &rclpy_action_get_rmw_qos_profile, "Get an action RMW QoS profile."); - define_action_goal_handle(m); - define_action_server(m); m.def( "rclpy_action_get_client_names_and_types_by_node", @@ -180,7 +173,5 @@ define_action_api(py::module m) m.def( "rclpy_action_get_names_and_types", &rclpy_action_get_names_and_types, "Get action names and types."); - - rclpy::define_action_client(m); } } // namespace rclpy diff --git a/rclpy/src/rclpy/_rclpy_pybind11.cpp b/rclpy/src/rclpy/_rclpy_pybind11.cpp index 19512db24..82c9e04aa 100644 --- a/rclpy/src/rclpy/_rclpy_pybind11.cpp +++ b/rclpy/src/rclpy/_rclpy_pybind11.cpp @@ -18,6 +18,9 @@ #include #include "action_api.hpp" +#include "action_client.hpp" +#include "action_goal_handle.hpp" +#include "action_server.hpp" #include "client.hpp" #include "clock.hpp" #include "context.hpp" @@ -139,6 +142,9 @@ PYBIND11_MODULE(_rclpy_pybind11, m) { "rclpy_qos_check_compatible", &rclpy::qos_check_compatible, "Check if two QoS profiles are compatible."); + rclpy::define_action_client(m); + rclpy::define_action_goal_handle(m); + rclpy::define_action_server(m); rclpy::define_guard_condition(m); rclpy::define_timer(m); rclpy::define_subscription(m); diff --git a/rclpy/src/rclpy/action_client.hpp b/rclpy/src/rclpy/action_client.hpp index 360e92343..cfb0aa898 100644 --- a/rclpy/src/rclpy/action_client.hpp +++ b/rclpy/src/rclpy/action_client.hpp @@ -214,7 +214,7 @@ class ActionClient : public Destroyable, public std::enable_shared_from_this - get_rcl_shared_ptr() - { - return rcl_action_server_; - } - /// Force an early destruction of this object void destroy() override; diff --git a/rclpy/src/rclpy/clock.hpp b/rclpy/src/rclpy/clock.hpp index c55682dff..37b8dbb75 100644 --- a/rclpy/src/rclpy/clock.hpp +++ b/rclpy/src/rclpy/clock.hpp @@ -105,7 +105,7 @@ class Clock : public Destroyable, public std::enable_shared_from_this void remove_clock_callback(py::object pyjump_handle); - /// Get rcl_client_t pointer + /// Get rcl_clock_t pointer rcl_clock_t * rcl_ptr() const { return rcl_clock_.get(); diff --git a/rclpy/src/rclpy/guard_condition.hpp b/rclpy/src/rclpy/guard_condition.hpp index f23c5e266..ef71bbb81 100644 --- a/rclpy/src/rclpy/guard_condition.hpp +++ b/rclpy/src/rclpy/guard_condition.hpp @@ -48,7 +48,7 @@ class GuardCondition : public Destroyable, public std::enable_shared_from_this py::tuple service_take_request(py::object pyrequest_type); - /// Get rcl_client_t pointer + /// Get rcl_service_t pointer rcl_service_t * rcl_ptr() const { diff --git a/rclpy/src/rclpy/subscription.hpp b/rclpy/src/rclpy/subscription.hpp index 2f27e85e9..bafabaf4e 100644 --- a/rclpy/src/rclpy/subscription.hpp +++ b/rclpy/src/rclpy/subscription.hpp @@ -86,7 +86,7 @@ class Subscription : public Destroyable, public std::enable_shared_from_this */ bool is_timer_canceled(); - /// Get rcl_client_t pointer + /// Get rcl_timer_t pointer rcl_timer_t * rcl_ptr() const { diff --git a/rclpy/src/rclpy/wait_set.hpp b/rclpy/src/rclpy/wait_set.hpp index 6e10cc16e..2ba9efd8e 100644 --- a/rclpy/src/rclpy/wait_set.hpp +++ b/rclpy/src/rclpy/wait_set.hpp @@ -172,7 +172,7 @@ class WaitSet : public Destroyable, public std::enable_shared_from_this void wait(int64_t timeout); - /// Get rcl_client_t pointer + /// Get rcl_wait_set_t pointer rcl_wait_set_t * rcl_ptr() const { return rcl_wait_set_.get(); From afdd25e5cdb5bd48fddf3e7ccdae50d6d21270cb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 28 Apr 2021 17:41:35 +0200 Subject: [PATCH 10/10] Convert Node and Context to use C++ Classes (#771) * convert Node and Context to use C++ Classes Signed-off-by: ahcorde * Added feedback Signed-off-by: ahcorde * Added feedback Signed-off-by: ahcorde * Implement solution 1 Signed-off-by: ahcorde * Prevent double early destruction of destroyable This along with an extra call to `destroy_node()` in `Node.__del__` was causing a segfault in the test_publisher test. The problem is `destroy_when_not_in_use()` was being called twice, and the second time is now a problem because rcl_ptrs_ is dereferenced in the qos_event destroy() method. Signed-off-by: Shane Loretz * Remove unnecessary Node.__del__ This should not be required. If it is, it means there's a bug that shows when the Node is garbage collected. Signed-off-by: Shane Loretz * Fix order of pointers in RclPtrs structs The order of the pointers in the RclPtrs structs is important. Things at the bottom are destroyed first, so children must be placed underneath the parent's that must be kept alive. Signed-off-by: Shane Loretz * Intentionally copy rclpy:: objects instead of having RclPtrs structs (#773) * Rough pass converting everything Signed-off-by: Shane Loretz * Fixes to get copy destruction order working Signed-off-by: Shane Loretz * Fix qos_event test flakiness Signed-off-by: Shane Loretz Signed-off-by: ahcorde * MacOS warning Signed-off-by: ahcorde * added feedback and removed some files Signed-off-by: ahcorde * Create only one context otherwise throw a exception Signed-off-by: ahcorde Co-authored-by: Shane Loretz Co-authored-by: Shane Loretz --- rclpy/CMakeLists.txt | 2 - rclpy/rclpy/action/client.py | 4 +- rclpy/rclpy/action/graph.py | 17 +- rclpy/rclpy/action/server.py | 4 +- rclpy/rclpy/context.py | 60 ++-- rclpy/rclpy/executors.py | 5 +- rclpy/rclpy/guard_condition.py | 4 +- rclpy/rclpy/node.py | 111 +++--- rclpy/rclpy/timer.py | 4 +- rclpy/src/rclpy/_rclpy_action.cpp | 177 ---------- rclpy/src/rclpy/_rclpy_pybind11.cpp | 59 +--- rclpy/src/rclpy/action_api.hpp | 31 -- rclpy/src/rclpy/action_client.cpp | 23 +- rclpy/src/rclpy/action_client.hpp | 8 +- rclpy/src/rclpy/action_goal_handle.cpp | 2 + rclpy/src/rclpy/action_goal_handle.hpp | 1 + rclpy/src/rclpy/action_server.cpp | 19 +- rclpy/src/rclpy/action_server.hpp | 8 +- rclpy/src/rclpy/client.cpp | 22 +- rclpy/src/rclpy/client.hpp | 9 +- rclpy/src/rclpy/context.cpp | 172 +++++---- rclpy/src/rclpy/context.hpp | 107 ++++-- rclpy/src/rclpy/destroyable.cpp | 13 + rclpy/src/rclpy/destroyable.hpp | 6 + rclpy/src/rclpy/graph.cpp | 81 ++--- rclpy/src/rclpy/graph.hpp | 40 +-- rclpy/src/rclpy/guard_condition.cpp | 17 +- rclpy/src/rclpy/guard_condition.hpp | 4 +- rclpy/src/rclpy/init.cpp | 154 -------- rclpy/src/rclpy/init.hpp | 50 --- rclpy/src/rclpy/logging.cpp | 9 +- rclpy/src/rclpy/logging.hpp | 4 +- rclpy/src/rclpy/names.cpp | 28 +- rclpy/src/rclpy/names.hpp | 10 +- rclpy/src/rclpy/node.cpp | 308 ++++++++-------- rclpy/src/rclpy/node.hpp | 328 ++++++++++-------- rclpy/src/rclpy/publisher.cpp | 24 +- rclpy/src/rclpy/publisher.hpp | 8 +- rclpy/src/rclpy/qos_event.cpp | 18 +- rclpy/src/rclpy/qos_event.hpp | 5 +- rclpy/src/rclpy/service.cpp | 22 +- rclpy/src/rclpy/service.hpp | 8 +- rclpy/src/rclpy/subscription.cpp | 24 +- rclpy/src/rclpy/subscription.hpp | 8 +- rclpy/src/rclpy/timer.cpp | 18 +- rclpy/src/rclpy/timer.hpp | 6 +- rclpy/src/rclpy/utils.cpp | 53 ++- rclpy/src/rclpy/utils.hpp | 16 + rclpy/src/rclpy/wait_set.cpp | 67 +--- rclpy/src/rclpy/wait_set.hpp | 16 +- .../include/rclpy_common/common.h | 21 -- rclpy/src/rclpy_common/src/common.c | 66 ---- rclpy/test/test_destruction.py | 10 +- rclpy/test/test_handle.py | 6 +- rclpy/test/test_qos_event.py | 14 +- rclpy/test/test_subscription.py | 17 +- rclpy/test/test_waitable.py | 20 +- 57 files changed, 944 insertions(+), 1404 deletions(-) delete mode 100644 rclpy/src/rclpy/_rclpy_action.cpp delete mode 100644 rclpy/src/rclpy/action_api.hpp delete mode 100644 rclpy/src/rclpy/init.cpp delete mode 100644 rclpy/src/rclpy/init.hpp diff --git a/rclpy/CMakeLists.txt b/rclpy/CMakeLists.txt index b1abfd719..1481e6a05 100644 --- a/rclpy/CMakeLists.txt +++ b/rclpy/CMakeLists.txt @@ -148,7 +148,6 @@ install(TARGETS rclpy_common # Split from main extension and converted to pybind11 pybind11_add_module(_rclpy_pybind11 SHARED - src/rclpy/_rclpy_action.cpp src/rclpy/_rclpy_handle.cpp src/rclpy/_rclpy_logging.cpp src/rclpy/_rclpy_pybind11.cpp @@ -164,7 +163,6 @@ pybind11_add_module(_rclpy_pybind11 SHARED src/rclpy/graph.cpp src/rclpy/guard_condition.cpp src/rclpy/handle.cpp - src/rclpy/init.cpp src/rclpy/logging.cpp src/rclpy/names.cpp src/rclpy/node.cpp diff --git a/rclpy/rclpy/action/client.py b/rclpy/rclpy/action/client.py index 22ca2d79f..d476dc131 100644 --- a/rclpy/rclpy/action/client.py +++ b/rclpy/rclpy/action/client.py @@ -149,9 +149,9 @@ def __init__( self._node = node self._action_type = action_type self._action_name = action_name - with node.handle as node_capsule: + with node.handle: self._client_handle = _rclpy.ActionClient( - node_capsule, + node.handle, action_type, action_name, goal_service_qos_profile.get_c_qos_profile(), diff --git a/rclpy/rclpy/action/graph.py b/rclpy/rclpy/action/graph.py index 828fd9a8c..362e48b97 100644 --- a/rclpy/rclpy/action/graph.py +++ b/rclpy/rclpy/action/graph.py @@ -15,7 +15,6 @@ from typing import List from typing import Tuple -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.node import Node @@ -34,9 +33,9 @@ def get_action_client_names_and_types_by_node( The first element of each tuple is the action name and the second element is a list of action types. """ - with node.handle as node_capsule: - return _rclpy.rclpy_action_get_client_names_and_types_by_node( - node_capsule, remote_node_name, remote_node_namespace) + with node.handle: + return node.handle.get_action_client_names_and_types_by_node( + remote_node_name, remote_node_namespace) def get_action_server_names_and_types_by_node( @@ -54,9 +53,9 @@ def get_action_server_names_and_types_by_node( The first element of each tuple is the action name and the second element is a list of action types. """ - with node.handle as node_capsule: - return _rclpy.rclpy_action_get_server_names_and_types_by_node( - node_capsule, remote_node_name, remote_node_namespace) + with node.handle: + return node.handle.get_action_server_names_and_types_by_node( + remote_node_name, remote_node_namespace) def get_action_names_and_types(node: Node) -> List[Tuple[str, List[str]]]: @@ -68,5 +67,5 @@ def get_action_names_and_types(node: Node) -> List[Tuple[str, List[str]]]: The first element of each tuple is the action name and the second element is a list of action types. """ - with node.handle as node_capsule: - return _rclpy.rclpy_action_get_names_and_types(node_capsule) + with node.handle: + return node.handle.get_action_names_and_types() diff --git a/rclpy/rclpy/action/server.py b/rclpy/rclpy/action/server.py index 72632dd5c..64e7aab72 100644 --- a/rclpy/rclpy/action/server.py +++ b/rclpy/rclpy/action/server.py @@ -240,9 +240,9 @@ def __init__( check_for_type_support(action_type) self._node = node self._action_type = action_type - with node.handle as node_capsule, node.get_clock().handle: + with node.handle, node.get_clock().handle: self._handle = _rclpy.ActionServer( - node_capsule, + node.handle, node.get_clock().handle, action_type, action_name, diff --git a/rclpy/rclpy/context.py b/rclpy/rclpy/context.py index d133ddb1d..f7a45ff8b 100644 --- a/rclpy/rclpy/context.py +++ b/rclpy/rclpy/context.py @@ -34,9 +34,6 @@ class Context: """ def __init__(self): - from rclpy.impl.implementation_singleton import rclpy_implementation - from .handle import Handle - self._handle = Handle(rclpy_implementation.rclpy_create_context()) self._lock = threading.Lock() self._callbacks = [] self._callbacks_lock = threading.Lock() @@ -44,7 +41,10 @@ def __init__(self): @property def handle(self): - return self._handle + return self.__context + + def destroy(self): + self.__context.destroy_when_not_in_use() def init(self, args: Optional[List[str]] = None, @@ -57,31 +57,36 @@ def init(self, :param args: List of command line arguments. """ # imported locally to avoid loading extensions on module import - from rclpy.impl.implementation_singleton import rclpy_implementation + from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy global g_logging_ref_count - with self._handle as capsule, self._lock: + with self._lock: if domain_id is not None and domain_id < 0: raise RuntimeError( 'Domain id ({}) should not be lower than zero.' .format(domain_id)) - rclpy_implementation.rclpy_init( - args if args is not None else sys.argv, - capsule, - domain_id if domain_id is not None else rclpy_implementation.RCL_DEFAULT_DOMAIN_ID) - if initialize_logging and not self._logging_initialized: - with g_logging_configure_lock: - g_logging_ref_count += 1 - if g_logging_ref_count == 1: - rclpy_implementation.rclpy_logging_configure(capsule) - self._logging_initialized = True + try: + if self.__context is not None: + raise RuntimeError + except AttributeError: + self.__context = _rclpy.Context( + args if args is not None else sys.argv, + domain_id if domain_id is not None else _rclpy.RCL_DEFAULT_DOMAIN_ID) + if initialize_logging and not self._logging_initialized: + with g_logging_configure_lock: + g_logging_ref_count += 1 + if g_logging_ref_count == 1: + _rclpy.rclpy_logging_configure(self.__context) + self._logging_initialized = True def ok(self): """Check if context hasn't been shut down.""" # imported locally to avoid loading extensions on module import - from rclpy.impl.implementation_singleton import rclpy_implementation - with self._handle as capsule, self._lock: - return rclpy_implementation.rclpy_ok(capsule) + try: + with self.__context, self._lock: + return self.__context.ok() + except AttributeError: + return False def _call_on_shutdown_callbacks(self): with self._callbacks_lock: @@ -94,19 +99,17 @@ def _call_on_shutdown_callbacks(self): def shutdown(self): """Shutdown this context.""" # imported locally to avoid loading extensions on module import - from rclpy.impl.implementation_singleton import rclpy_implementation - with self._handle as capsule, self._lock: - rclpy_implementation.rclpy_shutdown(capsule) + with self.__context, self._lock: + self.__context.shutdown() self._call_on_shutdown_callbacks() self._logging_fini() def try_shutdown(self): """Shutdown this context, if not already shutdown.""" # imported locally to avoid loading extensions on module import - from rclpy.impl.implementation_singleton import rclpy_implementation - with self._handle as capsule, self._lock: - if rclpy_implementation.rclpy_ok(capsule): - rclpy_implementation.rclpy_shutdown(capsule) + with self.__context, self._lock: + if self.__context.ok(): + self.__context.shutdown() self._call_on_shutdown_callbacks() def _remove_callback(self, weak_method): @@ -138,6 +141,5 @@ def _logging_fini(self): def get_domain_id(self): """Get domain id of context.""" - from rclpy.impl.implementation_singleton import rclpy_implementation - with self._handle as capsule, self._lock: - return rclpy_implementation.rclpy_context_get_domain_id(capsule) + with self.__context, self._lock: + return self.__context.get_domain_id() diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py index bfd0b2fe0..09a34d661 100644 --- a/rclpy/rclpy/executors.py +++ b/rclpy/rclpy/executors.py @@ -556,7 +556,8 @@ def _wait_for_ready_callbacks( except InvalidHandle: pass - context_capsule = context_stack.enter_context(self._context.handle) + context_stack.enter_context(self._context.handle) + wait_set = _rclpy.WaitSet( entity_count.num_subscriptions, entity_count.num_guard_conditions, @@ -564,7 +565,7 @@ def _wait_for_ready_callbacks( entity_count.num_clients, entity_count.num_services, entity_count.num_events, - context_capsule) + self._context.handle) wait_set.clear_entities() for sub_handle in sub_handles: diff --git a/rclpy/rclpy/guard_condition.py b/rclpy/rclpy/guard_condition.py index b8ba32664..8b83df621 100644 --- a/rclpy/rclpy/guard_condition.py +++ b/rclpy/rclpy/guard_condition.py @@ -20,8 +20,8 @@ class GuardCondition: def __init__(self, callback, callback_group, context=None): self._context = get_default_context() if context is None else context - with self._context.handle as capsule: - self.__gc = _rclpy.GuardCondition(capsule) + with self._context.handle: + self.__gc = _rclpy.GuardCondition(self._context.handle) self.callback = callback self.callback_group = callback_group # True when the callback is ready to fire but has not been "taken" by an executor diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index b8f36137b..4a090704b 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -56,7 +56,6 @@ from rclpy.executors import Executor from rclpy.expand_topic_name import expand_topic_name from rclpy.guard_condition import GuardCondition -from rclpy.handle import Handle from rclpy.handle import InvalidHandle from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.logging import get_logger @@ -170,16 +169,16 @@ def __init__( namespace = namespace or '' if not self._context.ok(): raise NotInitializedException('cannot create node') - with self._context.handle as capsule: + with self._context.handle: try: - self.__handle = Handle(_rclpy.rclpy_create_node( + self.__node = _rclpy.Node( node_name, namespace, - capsule, + self._context.handle, cli_args, use_global_arguments, enable_rosout - )) + ) except ValueError: # these will raise more specific errors if the name or namespace is bad validate_node_name(node_name) @@ -191,16 +190,16 @@ def __init__( validate_namespace(namespace) # Should not get to this point raise RuntimeError('rclpy_create_node failed for unknown reason') - with self.handle as capsule: - self._logger = get_logger(_rclpy.rclpy_get_node_logger_name(capsule)) + with self.handle: + self._logger = get_logger(self.__node.logger_name()) self.__executor_weakref = None self._parameter_event_publisher = self.create_publisher( ParameterEvent, '/parameter_events', qos_profile_parameter_events) - with self.handle as capsule: - self._parameter_overrides = _rclpy.rclpy_get_node_parameters(Parameter, capsule) + with self.handle: + self._parameter_overrides = self.__node.get_parameters(Parameter) # Combine parameters from params files with those from the node constructor and # use the set_parameters_atomically API so a parameter event is published. if parameter_overrides is not None: @@ -305,7 +304,7 @@ def handle(self): :raises: AttributeError if modified after creation. """ - return self.__handle + return self.__node @handle.setter def handle(self, value): @@ -313,13 +312,13 @@ def handle(self, value): def get_name(self) -> str: """Get the name of the node.""" - with self.handle as capsule: - return _rclpy.rclpy_get_node_name(capsule) + with self.handle: + return self.handle.get_node_name() def get_namespace(self) -> str: """Get the namespace of the node.""" - with self.handle as capsule: - return _rclpy.rclpy_get_node_namespace(capsule) + with self.handle: + return self.handle.get_namespace() def get_clock(self) -> Clock: """Get the clock used by the node.""" @@ -1190,8 +1189,8 @@ def resolve_topic_name(self, topic: str, *, only_expand: bool = False) -> str: :return: a fully qualified topic name, result of applying expansion and remapping to the given `topic`. """ - with self.handle as capsule: - return _rclpy.rclpy_resolve_name(capsule, topic, only_expand, False) + with self.handle: + return _rclpy.rclpy_resolve_name(self.handle, topic, only_expand, False) def resolve_service_name( self, service: str, *, only_expand: bool = False @@ -1204,8 +1203,8 @@ def resolve_service_name( :return: a fully qualified service name, result of applying expansion and remapping to the given `service`. """ - with self.handle as capsule: - return _rclpy.rclpy_resolve_name(capsule, service, only_expand, True) + with self.handle: + return _rclpy.rclpy_resolve_name(self.handle, service, only_expand, True) def create_publisher( self, @@ -1256,9 +1255,9 @@ def create_publisher( failed = False check_is_valid_msg_type(msg_type) try: - with self.handle as node_capsule: + with self.handle: publisher_object = _rclpy.Publisher( - node_capsule, msg_type, topic, qos_profile.get_c_qos_profile()) + self.handle, msg_type, topic, qos_profile.get_c_qos_profile()) except ValueError: failed = True if failed: @@ -1333,9 +1332,9 @@ def create_subscription( failed = False check_is_valid_msg_type(msg_type) try: - with self.handle as capsule: + with self.handle: subscription_object = _rclpy.Subscription( - capsule, msg_type, topic, qos_profile.get_c_qos_profile()) + self.handle, msg_type, topic, qos_profile.get_c_qos_profile()) except ValueError: failed = True if failed: @@ -1380,9 +1379,9 @@ def create_client( check_is_valid_srv_type(srv_type) failed = False try: - with self.handle as node_capsule: + with self.handle: client_impl = _rclpy.Client( - node_capsule, + self.handle, srv_type, srv_name, qos_profile.get_c_qos_profile()) @@ -1425,9 +1424,9 @@ def create_service( check_is_valid_srv_type(srv_type) failed = False try: - with self.handle as node_capsule: + with self.handle: service_impl = _rclpy.Service( - node_capsule, + self.handle, srv_type, srv_name, qos_profile.get_c_qos_profile()) @@ -1653,7 +1652,7 @@ def destroy_node(self) -> bool: self.destroy_timer(self.__timers[0]) while self.__guards: self.destroy_guard_condition(self.__guards[0]) - self.handle.destroy() + self.__node.destroy_when_not_in_use() self._wake_executor() def get_publisher_names_and_types_by_node( @@ -1674,9 +1673,9 @@ def get_publisher_names_and_types_by_node( :raise NodeNameNonExistentError: If the node wasn't found. :raise RuntimeError: Unexpected failure. """ - with self.handle as capsule: + with self.handle: return _rclpy.rclpy_get_publisher_names_and_types_by_node( - capsule, no_demangle, node_name, node_namespace) + self.handle, no_demangle, node_name, node_namespace) def get_subscriber_names_and_types_by_node( self, @@ -1696,9 +1695,9 @@ def get_subscriber_names_and_types_by_node( :raise NodeNameNonExistentError: If the node wasn't found. :raise RuntimeError: Unexpected failure. """ - with self.handle as capsule: + with self.handle: return _rclpy.rclpy_get_subscriber_names_and_types_by_node( - capsule, no_demangle, node_name, node_namespace) + self.handle, no_demangle, node_name, node_namespace) def get_service_names_and_types_by_node( self, @@ -1716,9 +1715,9 @@ def get_service_names_and_types_by_node( :raise NodeNameNonExistentError: If the node wasn't found. :raise RuntimeError: Unexpected failure. """ - with self.handle as capsule: + with self.handle: return _rclpy.rclpy_get_service_names_and_types_by_node( - capsule, node_name, node_namespace) + self.handle, node_name, node_namespace) def get_client_names_and_types_by_node( self, @@ -1736,9 +1735,9 @@ def get_client_names_and_types_by_node( :raise NodeNameNonExistentError: If the node wasn't found. :raise RuntimeError: Unexpected failure. """ - with self.handle as capsule: + with self.handle: return _rclpy.rclpy_get_client_names_and_types_by_node( - capsule, node_name, node_namespace) + self.handle, node_name, node_namespace) def get_topic_names_and_types(self, no_demangle: bool = False) -> List[Tuple[str, List[str]]]: """ @@ -1749,8 +1748,8 @@ def get_topic_names_and_types(self, no_demangle: bool = False) -> List[Tuple[str The first element of each tuple is the topic name and the second element is a list of topic types. """ - with self.handle as capsule: - return _rclpy.rclpy_get_topic_names_and_types(capsule, no_demangle) + with self.handle: + return _rclpy.rclpy_get_topic_names_and_types(self.handle, no_demangle) def get_service_names_and_types(self) -> List[Tuple[str, List[str]]]: """ @@ -1760,8 +1759,8 @@ def get_service_names_and_types(self) -> List[Tuple[str, List[str]]]: The first element of each tuple is the service name and the second element is a list of service types. """ - with self.handle as capsule: - return _rclpy.rclpy_get_service_names_and_types(capsule) + with self.handle: + return _rclpy.rclpy_get_service_names_and_types(self.handle) def get_node_names(self) -> List[str]: """ @@ -1769,8 +1768,8 @@ def get_node_names(self) -> List[str]: :return: List of node names. """ - with self.handle as capsule: - names_ns = _rclpy.rclpy_get_node_names_and_namespaces(capsule) + with self.handle: + names_ns = self.handle.get_node_names_and_namespaces() return [n[0] for n in names_ns] def get_node_names_and_namespaces(self) -> List[Tuple[str, str]]: @@ -1779,8 +1778,8 @@ def get_node_names_and_namespaces(self) -> List[Tuple[str, str]]: :return: List of tuples containing two strings: the node name and node namespace. """ - with self.handle as capsule: - return _rclpy.rclpy_get_node_names_and_namespaces(capsule) + with self.handle: + return self.handle.get_node_names_and_namespaces() def get_node_names_and_namespaces_with_enclaves(self) -> List[Tuple[str, str, str]]: """ @@ -1789,8 +1788,8 @@ def get_node_names_and_namespaces_with_enclaves(self) -> List[Tuple[str, str, st :return: List of tuples containing three strings: the node name, node namespace and enclave. """ - with self.handle as capsule: - return _rclpy.rclpy_get_node_names_and_namespaces_with_enclaves(capsule) + with self.handle: + return self.handle.get_node_names_and_namespaces_with_enclaves() def get_fully_qualified_name(self) -> str: """ @@ -1798,14 +1797,14 @@ def get_fully_qualified_name(self) -> str: :return: Fully qualified node name. """ - with self.handle as capsule: - return _rclpy.rclpy_node_get_fully_qualified_name(capsule) + with self.handle: + return self.handle.get_fully_qualified_name() def _count_publishers_or_subscribers(self, topic_name, func): fq_topic_name = expand_topic_name(topic_name, self.get_name(), self.get_namespace()) validate_full_topic_name(fq_topic_name) - with self.handle as node_capsule: - return func(node_capsule, fq_topic_name) + with self.handle: + return func(fq_topic_name) def count_publishers(self, topic_name: str) -> int: """ @@ -1818,7 +1817,9 @@ def count_publishers(self, topic_name: str) -> int: :param topic_name: the topic_name on which to count the number of publishers. :return: the number of publishers on the topic. """ - return self._count_publishers_or_subscribers(topic_name, _rclpy.rclpy_count_publishers) + with self.handle: + return self._count_publishers_or_subscribers( + topic_name, self.handle.get_count_publishers) def count_subscribers(self, topic_name: str) -> int: """ @@ -1831,7 +1832,9 @@ def count_subscribers(self, topic_name: str) -> int: :param topic_name: the topic_name on which to count the number of subscribers. :return: the number of subscribers on the topic. """ - return self._count_publishers_or_subscribers(topic_name, _rclpy.rclpy_count_subscribers) + with self.handle: + return self._count_publishers_or_subscribers( + topic_name, self.handle.get_count_subscribers) def _get_info_by_topic( self, @@ -1839,16 +1842,16 @@ def _get_info_by_topic( no_mangle: bool, func: Callable[[object, str, bool], List[Dict]] ) -> List[TopicEndpointInfo]: - with self.handle as node_capsule: + with self.handle: if no_mangle: fq_topic_name = topic_name else: fq_topic_name = expand_topic_name( topic_name, self.get_name(), self.get_namespace()) validate_full_topic_name(fq_topic_name) - fq_topic_name = _rclpy.rclpy_remap_topic_name(node_capsule, fq_topic_name) + fq_topic_name = _rclpy.rclpy_remap_topic_name(self.handle, fq_topic_name) - info_dicts = func(node_capsule, fq_topic_name, no_mangle) + info_dicts = func(self.handle, fq_topic_name, no_mangle) infos = [TopicEndpointInfo(**x) for x in info_dicts] return infos diff --git a/rclpy/rclpy/timer.py b/rclpy/rclpy/timer.py index 83d4fc4f8..c570524d9 100644 --- a/rclpy/rclpy/timer.py +++ b/rclpy/rclpy/timer.py @@ -25,9 +25,9 @@ class Timer: def __init__(self, callback, callback_group, timer_period_ns, clock, *, context=None): self._context = get_default_context() if context is None else context self._clock = clock - with self._clock.handle, self._context.handle as context_capsule: + with self._clock.handle, self._context.handle: self.__timer = _rclpy.Timer( - self._clock.handle, context_capsule, timer_period_ns) + self._clock.handle, self._context.handle, timer_period_ns) self.timer_period_ns = timer_period_ns self.callback = callback self.callback_group = callback_group diff --git a/rclpy/src/rclpy/_rclpy_action.cpp b/rclpy/src/rclpy/_rclpy_action.cpp deleted file mode 100644 index ae140bcef..000000000 --- a/rclpy/src/rclpy/_rclpy_action.cpp +++ /dev/null @@ -1,177 +0,0 @@ -// Copyright 2019 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include -#include - -#include -#include -#include - -#include "rclpy_common/exceptions.hpp" - -#include "rclpy_common/common.h" -#include "rclpy_common/handle.h" - -namespace py = pybind11; - - -template -T -get_pointer(py::capsule & capsule, const char * name) -{ - if (strcmp(name, capsule.name())) { - std::string error_text{"Expected capusle with name '"}; - error_text += name; - error_text += "' but got '"; - error_text += capsule.name(); - error_text += "'"; - throw py::value_error(error_text); - } - // TODO(sloretz) use get_pointer() in pybind11 2.6+ - return static_cast(capsule); -} - -/// Fetch a predefined qos_profile from rcl_action and convert it to a Python QoSProfile object. -/** - * Raises RuntimeError if the QoS profile is unknown. - * - * This function takes a string defining a rmw_qos_profile_t and returns the - * corresponding Python QoSProfile object. - * \param[in] rmw_profile String with the name of the profile to load. - * \return QoSProfile object. - */ -py::dict -rclpy_action_get_rmw_qos_profile(const char * rmw_profile) -{ - PyObject * pyqos_profile = NULL; - if (0 == strcmp(rmw_profile, "rcl_action_qos_profile_status_default")) { - pyqos_profile = rclpy_common_convert_to_qos_dict(&rcl_action_qos_profile_status_default); - } else { - std::string error_text = "Requested unknown rmw_qos_profile: "; - error_text += rmw_profile; - throw std::runtime_error(error_text); - } - return py::reinterpret_steal(pyqos_profile); -} - -py::object -rclpy_action_get_client_names_and_types_by_node( - py::capsule pynode, const char * remote_node_name, const char * remote_node_namespace) -{ - rcl_node_t * node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - - rcl_names_and_types_t names_and_types = rcl_get_zero_initialized_names_and_types(); - rcl_allocator_t allocator = rcl_get_default_allocator(); - rcl_ret_t ret = rcl_action_get_client_names_and_types_by_node( - node, - &allocator, - remote_node_name, - remote_node_namespace, - &names_and_types); - if (RCL_RET_OK != ret) { - throw rclpy::RCLError("Failed to get action client names and type"); - } - - py::object pynames_and_types = py::reinterpret_steal( - rclpy_convert_to_py_names_and_types(&names_and_types)); - if (!rclpy_names_and_types_fini(&names_and_types)) { - throw py::error_already_set(); - } - return pynames_and_types; -} - -py::object -rclpy_action_get_server_names_and_types_by_node( - py::capsule pynode, const char * remote_node_name, const char * remote_node_namespace) -{ - rcl_node_t * node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - - rcl_names_and_types_t names_and_types = rcl_get_zero_initialized_names_and_types(); - rcl_allocator_t allocator = rcl_get_default_allocator(); - rcl_ret_t ret = rcl_action_get_server_names_and_types_by_node( - node, - &allocator, - remote_node_name, - remote_node_namespace, - &names_and_types); - if (RCL_RET_OK != ret) { - throw rclpy::RCLError("Failed to get action server names and type"); - } - - py::object pynames_and_types = py::reinterpret_steal( - rclpy_convert_to_py_names_and_types(&names_and_types)); - if (!rclpy_names_and_types_fini(&names_and_types)) { - throw py::error_already_set(); - } - return pynames_and_types; -} - -py::object -rclpy_action_get_names_and_types(py::capsule pynode) -{ - rcl_node_t * node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - - rcl_names_and_types_t names_and_types = rcl_get_zero_initialized_names_and_types(); - rcl_allocator_t allocator = rcl_get_default_allocator(); - rcl_ret_t ret = rcl_action_get_names_and_types(node, &allocator, &names_and_types); - if (RCL_RET_OK != ret) { - throw rclpy::RCLError("Failed to get action names and type"); - } - - py::object pynames_and_types = py::reinterpret_steal( - rclpy_convert_to_py_names_and_types(&names_and_types)); - if (!rclpy_names_and_types_fini(&names_and_types)) { - throw py::error_already_set(); - } - return pynames_and_types; -} - - -namespace rclpy -{ -void -define_action_api(py::module m) -{ - m.def( - "rclpy_action_get_rmw_qos_profile", &rclpy_action_get_rmw_qos_profile, - "Get an action RMW QoS profile."); - - m.def( - "rclpy_action_get_client_names_and_types_by_node", - &rclpy_action_get_client_names_and_types_by_node, - "Get action client names and types by node."); - m.def( - "rclpy_action_get_server_names_and_types_by_node", - &rclpy_action_get_server_names_and_types_by_node, - "Get action server names and types by node."); - m.def( - "rclpy_action_get_names_and_types", &rclpy_action_get_names_and_types, - "Get action names and types."); -} -} // namespace rclpy diff --git a/rclpy/src/rclpy/_rclpy_pybind11.cpp b/rclpy/src/rclpy/_rclpy_pybind11.cpp index 82c9e04aa..e191984b3 100644 --- a/rclpy/src/rclpy/_rclpy_pybind11.cpp +++ b/rclpy/src/rclpy/_rclpy_pybind11.cpp @@ -17,7 +17,6 @@ #include #include -#include "action_api.hpp" #include "action_client.hpp" #include "action_goal_handle.hpp" #include "action_server.hpp" @@ -29,7 +28,6 @@ #include "graph.hpp" #include "guard_condition.hpp" #include "handle_api.hpp" -#include "init.hpp" #include "logging.hpp" #include "logging_api.hpp" #include "names.hpp" @@ -111,24 +109,9 @@ PYBIND11_MODULE(_rclpy_pybind11, m) { py::register_exception( m, "InvalidHandle", PyExc_RuntimeError); - m.def( - "rclpy_init", &rclpy::init, - "Initialize RCL."); - m.def( - "rclpy_shutdown", &rclpy::shutdown, - "rclpy_shutdown."); - rclpy::define_client(m); - m.def( - "rclpy_context_get_domain_id", &rclpy::context_get_domain_id, - "Retrieves domain ID from init_options of context"); - m.def( - "rclpy_create_context", &rclpy::create_context, - "Create a capsule with an rcl_context_t instance"); - m.def( - "rclpy_ok", &rclpy::context_is_valid, - "Return true if the context is valid"); + rclpy::define_context(m); rclpy::define_duration(m); @@ -145,6 +128,9 @@ PYBIND11_MODULE(_rclpy_pybind11, m) { rclpy::define_action_client(m); rclpy::define_action_goal_handle(m); rclpy::define_action_server(m); + m.def( + "rclpy_action_get_rmw_qos_profile", &rclpy::rclpy_action_get_rmw_qos_profile, + "Get an action RMW QoS profile."); rclpy::define_guard_condition(m); rclpy::define_timer(m); rclpy::define_subscription(m); @@ -215,43 +201,9 @@ PYBIND11_MODULE(_rclpy_pybind11, m) { "rclpy_deserialize", &rclpy::deserialize, "Deserialize a ROS message."); - m.def( - "rclpy_create_node", &rclpy::create_node, - "Create a Node."); - m.def( - "rclpy_node_get_fully_qualified_name", &rclpy::get_node_fully_qualified_name, - "Get the fully qualified name of node."); - m.def( - "rclpy_get_node_logger_name", &rclpy::get_node_logger_name, - "Get the logger name associated with a node."); - m.def( - "rclpy_get_node_name", &rclpy::get_node_name, - "Get the name of a node."); - m.def( - "rclpy_get_node_namespace", &rclpy::get_node_namespace, - "Get the namespace of a node."); - m.def( - "rclpy_get_node_names_and_namespaces", &rclpy::get_node_names_and_namespaces, - "Get node names and namespaces list from graph API."); - m.def( - "rclpy_get_node_names_and_namespaces_with_enclaves", - &rclpy::get_node_names_and_namespaces_with_enclaves, - "Get node names, namespaces, and enclaves list from graph API."); - - m.def( - "rclpy_count_publishers", &rclpy::get_count_publishers, - "Count publishers for a topic."); - - m.def( - "rclpy_count_subscribers", &rclpy::get_count_subscribers, - "Count subscribers for a topic."); - + rclpy::define_node(m); rclpy::define_qos_event(m); - m.def( - "rclpy_get_node_parameters", &rclpy::get_node_parameters, - "Get the initial parameters for a node from the command line."); - m.def( "rclpy_get_rmw_implementation_identifier", &rclpy::get_rmw_implementation_identifier, @@ -277,5 +229,4 @@ PYBIND11_MODULE(_rclpy_pybind11, m) { rclpy::define_pycapsule_api(m); rclpy::define_handle_api(m); rclpy::define_logging_api(m); - rclpy::define_action_api(m); } diff --git a/rclpy/src/rclpy/action_api.hpp b/rclpy/src/rclpy/action_api.hpp deleted file mode 100644 index 6da5c7d3c..000000000 --- a/rclpy/src/rclpy/action_api.hpp +++ /dev/null @@ -1,31 +0,0 @@ -// Copyright 2021 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef RCLPY__ACTION_API_HPP_ -#define RCLPY__ACTION_API_HPP_ - -#include - -namespace py = pybind11; - -namespace rclpy -{ -/// Define methods on a module for the action API -/** - * \param[in] module a pybind11 module to add the definition to - */ -void -define_action_api(py::module module); -} // namespace rclpy -#endif // RCLPY__ACTION_API_HPP_ diff --git a/rclpy/src/rclpy/action_client.cpp b/rclpy/src/rclpy/action_client.cpp index 3fa78cc76..6fac5bf81 100644 --- a/rclpy/src/rclpy/action_client.cpp +++ b/rclpy/src/rclpy/action_client.cpp @@ -34,11 +34,11 @@ void ActionClient::destroy() { rcl_action_client_.reset(); - node_handle_.reset(); + node_.destroy(); } ActionClient::ActionClient( - py::capsule pynode, + Node & node, py::object pyaction_type, const char * action_name, const rmw_qos_profile_t & goal_service_qos, @@ -46,10 +46,8 @@ ActionClient::ActionClient( const rmw_qos_profile_t & cancel_service_qos, const rmw_qos_profile_t & feedback_topic_qos, const rmw_qos_profile_t & status_topic_qos) -: node_handle_(std::make_shared(pynode)) +: node_(node) { - auto node = node_handle_->cast("rcl_node_t"); - rosidl_action_type_support_t * ts = static_cast(rclpy_common_get_type_support( pyaction_type.ptr())); @@ -67,11 +65,10 @@ ActionClient::ActionClient( rcl_action_client_ = std::shared_ptr( new rcl_action_client_t, - [this](rcl_action_client_t * action_client) + [node](rcl_action_client_t * action_client) { - auto node = node_handle_->cast_or_warn("rcl_node_t"); - - rcl_ret_t ret = rcl_action_client_fini(action_client, node); + // Intentionally capture node by value so shared_ptr can be transferred to copies + rcl_ret_t ret = rcl_action_client_fini(action_client, node.rcl_ptr()); if (RCL_RET_OK != ret) { // Warning should use line number of the current stack frame int stack_level = 1; @@ -87,7 +84,7 @@ ActionClient::ActionClient( rcl_ret_t ret = rcl_action_client_init( rcl_action_client_.get(), - node, + node_.rcl_ptr(), ts, action_name, &action_client_ops); @@ -222,11 +219,9 @@ ActionClient::get_num_entities() bool ActionClient::is_action_server_available() { - auto node = node_handle_->cast("rcl_node_t"); - bool is_available = false; rcl_ret_t ret = rcl_action_server_is_available( - node, rcl_action_client_.get(), &is_available); + node_.rcl_ptr(), rcl_action_client_.get(), &is_available); if (RCL_RET_OK != ret) { throw rclpy::RCLError("Failed to check if action server is available"); } @@ -278,7 +273,7 @@ define_action_client(py::object module) { py::class_>(module, "ActionClient") .def( - py::init()) .def_property_readonly( diff --git a/rclpy/src/rclpy/action_client.hpp b/rclpy/src/rclpy/action_client.hpp index cfb0aa898..e8a24229c 100644 --- a/rclpy/src/rclpy/action_client.hpp +++ b/rclpy/src/rclpy/action_client.hpp @@ -22,7 +22,7 @@ #include #include "destroyable.hpp" -#include "handle.hpp" +#include "node.hpp" #include "wait_set.hpp" namespace py = pybind11; @@ -42,7 +42,7 @@ class ActionClient : public Destroyable, public std::enable_shared_from_this node_handle_; + Node node_; std::shared_ptr rcl_action_client_; }; /// Define a pybind11 wrapper for an rcl_time_point_t diff --git a/rclpy/src/rclpy/action_goal_handle.cpp b/rclpy/src/rclpy/action_goal_handle.cpp index 45904043c..771cde2a1 100644 --- a/rclpy/src/rclpy/action_goal_handle.cpp +++ b/rclpy/src/rclpy/action_goal_handle.cpp @@ -31,6 +31,7 @@ namespace rclpy { ActionGoalHandle::ActionGoalHandle( rclpy::ActionServer & action_server, py::object pygoal_info_msg) +: action_server_(action_server) { destroy_ros_message_signature * destroy_ros_message = NULL; auto goal_info_msg = static_cast( @@ -72,6 +73,7 @@ void ActionGoalHandle::destroy() { rcl_action_goal_handle_.reset(); + action_server_.destroy(); } rcl_action_goal_state_t diff --git a/rclpy/src/rclpy/action_goal_handle.hpp b/rclpy/src/rclpy/action_goal_handle.hpp index b1e1bfc94..bb9da8819 100644 --- a/rclpy/src/rclpy/action_goal_handle.hpp +++ b/rclpy/src/rclpy/action_goal_handle.hpp @@ -75,6 +75,7 @@ class ActionGoalHandle : public Destroyable, public std::enable_shared_from_this destroy() override; private: + ActionServer action_server_; std::shared_ptr rcl_action_goal_handle_; }; diff --git a/rclpy/src/rclpy/action_server.cpp b/rclpy/src/rclpy/action_server.cpp index f8639a0c8..a0c1ee302 100644 --- a/rclpy/src/rclpy/action_server.cpp +++ b/rclpy/src/rclpy/action_server.cpp @@ -34,11 +34,11 @@ void ActionServer::destroy() { rcl_action_server_.reset(); - node_handle_.reset(); + node_.destroy(); } ActionServer::ActionServer( - py::capsule pynode, + Node & node, const rclpy::Clock & rclpy_clock, py::object pyaction_type, const char * action_name, @@ -48,10 +48,8 @@ ActionServer::ActionServer( const rmw_qos_profile_t & feedback_topic_qos, const rmw_qos_profile_t & status_topic_qos, double result_timeout) -: node_handle_(std::make_shared(pynode)) +: node_(node) { - auto node = node_handle_->cast("rcl_node_t"); - rcl_clock_t * clock = rclpy_clock.rcl_ptr(); rosidl_action_type_support_t * ts = static_cast( @@ -71,11 +69,10 @@ ActionServer::ActionServer( rcl_action_server_ = std::shared_ptr( new rcl_action_server_t, - [this](rcl_action_server_t * action_server) + [node](rcl_action_server_t * action_server) { - auto node = node_handle_->cast_or_warn("rcl_node_t"); - - rcl_ret_t ret = rcl_action_server_fini(action_server, node); + // Intentionally capture node by value so shared_ptr can be transferred to copies + rcl_ret_t ret = rcl_action_server_fini(action_server, node.rcl_ptr()); if (RCL_RET_OK != ret) { // Warning should use line number of the current stack frame int stack_level = 1; @@ -91,7 +88,7 @@ ActionServer::ActionServer( rcl_ret_t ret = rcl_action_server_init( rcl_action_server_.get(), - node, + node_.rcl_ptr(), clock, ts, action_name, @@ -348,7 +345,7 @@ define_action_server(py::object module) { py::class_>(module, "ActionServer") .def( - py::init()) .def_property_readonly( diff --git a/rclpy/src/rclpy/action_server.hpp b/rclpy/src/rclpy/action_server.hpp index ae21b56b0..6482bbd06 100644 --- a/rclpy/src/rclpy/action_server.hpp +++ b/rclpy/src/rclpy/action_server.hpp @@ -23,7 +23,7 @@ #include "clock.hpp" #include "destroyable.hpp" -#include "handle.hpp" +#include "node.hpp" #include "wait_set.hpp" namespace py = pybind11; @@ -44,7 +44,7 @@ class ActionServer : public Destroyable, public std::enable_shared_from_this node_handle_; + Node node_; std::shared_ptr rcl_action_server_; }; /// Define a pybind11 wrapper for an rcl_time_point_t diff --git a/rclpy/src/rclpy/client.cpp b/rclpy/src/rclpy/client.cpp index d277dee71..4cba6cd88 100644 --- a/rclpy/src/rclpy/client.cpp +++ b/rclpy/src/rclpy/client.cpp @@ -34,15 +34,13 @@ void Client::destroy() { rcl_client_.reset(); - node_handle_.reset(); + node_.destroy(); } Client::Client( - py::capsule pynode, py::object pysrv_type, const char * service_name, py::object pyqos_profile) -: node_handle_(std::make_shared(pynode)) + Node & node, py::object pysrv_type, const char * service_name, py::object pyqos_profile) +: node_(node) { - auto node = node_handle_->cast("rcl_node_t"); - auto srv_type = static_cast( rclpy_common_get_type_support(pysrv_type.ptr())); if (!srv_type) { @@ -59,11 +57,10 @@ Client::Client( // Create a client rcl_client_ = std::shared_ptr( PythonAllocator().allocate(1), - [this](rcl_client_t * client) + [node](rcl_client_t * client) { - auto node = node_handle_->cast_or_warn("rcl_node_t"); - - rcl_ret_t ret = rcl_client_fini(client, node); + // Intentionally capture node by value so shared_ptr can be transferred to copies + rcl_ret_t ret = rcl_client_fini(client, node.rcl_ptr()); if (RCL_RET_OK != ret) { // Warning should use line number of the current stack frame int stack_level = 1; @@ -78,7 +75,7 @@ Client::Client( *rcl_client_ = rcl_get_zero_initialized_client(); rcl_ret_t ret = rcl_client_init( - rcl_client_.get(), node, srv_type, service_name, &client_ops); + rcl_client_.get(), node_.rcl_ptr(), srv_type, service_name, &client_ops); if (RCL_RET_OK != ret) { if (RCL_RET_SERVICE_NAME_INVALID == ret) { std::string error_text{"failed to create client due to invalid service name '"}; @@ -114,9 +111,8 @@ Client::send_request(py::object pyrequest) bool Client::service_server_is_available() { - auto node = node_handle_->cast("rcl_node_t"); bool is_ready; - rcl_ret_t ret = rcl_service_server_is_available(node, rcl_client_.get(), &is_ready); + rcl_ret_t ret = rcl_service_server_is_available(node_.rcl_ptr(), rcl_client_.get(), &is_ready); if (RCL_RET_OK != ret) { throw RCLError("failed to check service availability"); } @@ -156,7 +152,7 @@ void define_client(py::object module) { py::class_>(module, "Client") - .def(py::init()) + .def(py::init()) .def_property_readonly( "pointer", [](const Client & client) { return reinterpret_cast(client.rcl_ptr()); diff --git a/rclpy/src/rclpy/client.hpp b/rclpy/src/rclpy/client.hpp index 8c7621a8d..1280bd099 100644 --- a/rclpy/src/rclpy/client.hpp +++ b/rclpy/src/rclpy/client.hpp @@ -22,7 +22,7 @@ #include #include "destroyable.hpp" -#include "handle.hpp" +#include "node.hpp" namespace py = pybind11; @@ -40,12 +40,12 @@ class Client : public Destroyable, public std::enable_shared_from_this * Raises ValueError if the capsules are not the correct types * Raises RuntimeError if the client could not be created * - * \param[in] pynode Capsule pointing to the node to add the client to + * \param[in] node Node to add the client to * \param[in] pysrv_type Service module associated with the client * \param[in] service_name Python object containing the service name * \param[in] pyqos rmw_qos_profile_t object for this client */ - Client(py::capsule pynode, py::object pysrv_type, const char * service_name, py::object pyqos); + Client(Node & node, py::object pysrv_type, const char * service_name, py::object pyqos); ~Client() = default; @@ -91,8 +91,7 @@ class Client : public Destroyable, public std::enable_shared_from_this destroy() override; private: - // TODO(sloretz) replace with std::shared_ptr when rclpy::Node exists - std::shared_ptr node_handle_; + Node node_; std::shared_ptr rcl_client_; }; diff --git a/rclpy/src/rclpy/context.cpp b/rclpy/src/rclpy/context.cpp index 794c29a78..e1c155c45 100644 --- a/rclpy/src/rclpy/context.cpp +++ b/rclpy/src/rclpy/context.cpp @@ -20,103 +20,137 @@ #include #include -#include +#include +#include +#include #include "rclpy_common/handle.h" #include "rclpy_common/exceptions.hpp" #include "context.hpp" +#include "utils.hpp" namespace rclpy { -size_t -context_get_domain_id(py::capsule pycontext) +Context::Context(py::list pyargs, size_t domain_id) { - auto context = static_cast( - rclpy_handle_get_pointer_from_capsule(pycontext.ptr(), "rcl_context_t")); - if (!context) { - throw py::error_already_set(); + rcl_context_ = std::shared_ptr( + new rcl_context_t, + [](rcl_context_t * context) + { + if (NULL != context->impl) { + rcl_ret_t ret; + if (rcl_context_is_valid(context)) { + // shutdown first, if still valid + ret = rcl_shutdown(context); + 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, + "[rclpy| %s : %s ]: failed to shutdown rcl_context_t: %s", + RCUTILS_STRINGIFY(__FILE__), RCUTILS_STRINGIFY(__LINE__), rcl_get_error_string().str); + rcl_reset_error(); + } + } + ret = rcl_context_fini(context); + 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, + "[rclpy| %s : %s ]: failed to fini rcl_context_t: %s", + RCUTILS_STRINGIFY(__FILE__), RCUTILS_STRINGIFY(__LINE__), rcl_get_error_string().str); + rcl_reset_error(); + } + } + delete context; + }); + *rcl_context_ = rcl_get_zero_initialized_context(); + + // turn the arguments into an array of C-style strings + std::vector arg_c_values(pyargs.size()); + for (size_t i = 0; i < pyargs.size(); ++i) { + // CPython owns const char * memory - no need to free it + arg_c_values[i] = PyUnicode_AsUTF8(pyargs[i].ptr()); + if (!arg_c_values[i]) { + throw py::error_already_set(); + } } - size_t domain_id; - rcl_ret_t ret = rcl_context_get_domain_id(context, &domain_id); + InitOptions init_options(rcl_get_default_allocator()); + + // Set domain id + rcl_ret_t ret = rcl_init_options_set_domain_id(&init_options.rcl_options, domain_id); if (RCL_RET_OK != ret) { - throw RCLError("Failed to get domain id"); + throw RCLError("failed to set domain id to init options"); } - return domain_id; + if (arg_c_values.size() > static_cast(std::numeric_limits::max())) { + throw std::range_error("Too many cli arguments"); + } + int argc = static_cast(arg_c_values.size()); + const char ** argv = argc > 0 ? &(arg_c_values[0]) : nullptr; + ret = rcl_init(argc, argv, &init_options.rcl_options, rcl_context_.get()); + if (RCL_RET_OK != ret) { + throw RCLError("failed to initialize rcl"); + } + + throw_if_unparsed_ros_args(pyargs, rcl_context_.get()->global_arguments); } void -_rclpy_context_handle_destructor(void * p) +Context::destroy() { - auto context = static_cast(p); - if (!context) { - // Warning should use line number of the current stack frame - int stack_level = 1; - PyErr_WarnFormat( - PyExc_RuntimeWarning, stack_level, "_rclpy_context_handle_destructor failed to get pointer"); - return; - } - if (NULL != context->impl) { - rcl_ret_t ret; - if (rcl_context_is_valid(context)) { - // shutdown first, if still valid - ret = rcl_shutdown(context); - if (RCL_RET_OK != ret) { - fprintf( - stderr, - "[rclpy|" RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__LINE__) "]: " - "failed to shutdown rcl_context_t (%d) during PyCapsule destructor: %s\n", - ret, - rcl_get_error_string().str); - rcl_reset_error(); - } - } - ret = rcl_context_fini(context); - if (RCL_RET_OK != ret) { - fprintf( - stderr, - "[rclpy|" RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__LINE__) "]: " - "failed to fini rcl_context_t (%d) during PyCapsule destructor: %s\n", - ret, - rcl_get_error_string().str); - rcl_reset_error(); - } - } - PyMem_FREE(context); + rcl_context_.reset(); } -py::capsule -create_context() +size_t +Context::get_domain_id() { - auto context = static_cast(PyMem_Malloc(sizeof(rcl_context_t))); - if (!context) { - throw std::bad_alloc(); - } - *context = rcl_get_zero_initialized_context(); - PyObject * capsule = rclpy_create_handle_capsule( - context, "rcl_context_t", _rclpy_context_handle_destructor); - if (!capsule) { - throw py::error_already_set(); + size_t domain_id; + rcl_ret_t ret = rcl_context_get_domain_id(rcl_context_.get(), &domain_id); + if (RCL_RET_OK != ret) { + throw RCLError("Failed to get domain id"); } - return py::reinterpret_steal(capsule); + + return domain_id; } -/// Status of the the client library -/** - * \return True if rcl is running properly, False otherwise - */ bool -context_is_valid(py::capsule pycontext) +Context::ok() { - auto context = static_cast(rclpy_handle_get_pointer_from_capsule( - pycontext.ptr(), "rcl_context_t")); - if (!context) { - throw py::error_already_set(); + return rcl_context_is_valid(rcl_context_.get()); +} + +void +Context::shutdown() +{ + rcl_ret_t ret = rcl_shutdown(rcl_context_.get()); + if (RCL_RET_OK != ret) { + throw RCLError("failed to shutdown"); } +} - return rcl_context_is_valid(context); +void define_context(py::object module) +{ + py::class_>(module, "Context") + .def(py::init()) + .def_property_readonly( + "pointer", [](const Context & context) { + return reinterpret_cast(context.rcl_ptr()); + }, + "Get the address of the entity as an integer") + .def( + "get_domain_id", &Context::get_domain_id, + "Retrieves domain id from init_options of context.") + .def( + "ok", &Context::ok, + "Status of the the client library") + .def( + "shutdown", &Context::shutdown, + "Shutdown context"); } + } // namespace rclpy diff --git a/rclpy/src/rclpy/context.hpp b/rclpy/src/rclpy/context.hpp index f6f4181a1..ed00992ef 100644 --- a/rclpy/src/rclpy/context.hpp +++ b/rclpy/src/rclpy/context.hpp @@ -17,36 +17,91 @@ #include +#include + +#include +#include + +#include "destroyable.hpp" +#include "rclpy_common/exceptions.hpp" + namespace py = pybind11; namespace rclpy { -/// Retrieves domain id from init_options of context -/** - * \param[in] pycontext Capsule containing rcl_context_t - * \return domain id - */ -size_t -context_get_domain_id(py::capsule pycontext); - -/// Create a capsule with an rcl_context_t instance. -/** - * The returned context is zero-initialized for use with rclpy_init(). - * - * Raises MemoryError if allocating memory fails. - * Raises RuntimeError if creating the context fails. - * - * \return capsule with the rcl_context_t instance - */ -py::capsule -create_context(); - -/// Status of the the client library -/** - * \return True if rcl is running properly, False otherwise - */ -bool -context_is_valid(py::capsule context); +struct InitOptions +{ + explicit InitOptions(rcl_allocator_t allocator) + { + rcl_options = rcl_get_zero_initialized_init_options(); + rcl_ret_t ret = rcl_init_options_init(&rcl_options, allocator); + if (RCL_RET_OK != ret) { + throw RCLError("Failed to initialize init options"); + } + } + + ~InitOptions() + { + rcl_ret_t ret = rcl_init_options_fini(&rcl_options); + if (RCL_RET_OK != ret) { + int stack_level = 1; + PyErr_WarnFormat( + PyExc_RuntimeWarning, stack_level, + "[rclpy| %s : %s ]: failed to fini rcl_init_options_t in destructor: %s", + RCUTILS_STRINGIFY(__FILE__), RCUTILS_STRINGIFY(__LINE__), rcl_get_error_string().str); + rcl_reset_error(); + } + } + + rcl_init_options_t rcl_options; +}; + +class Context : public Destroyable, public std::enable_shared_from_this +{ +public: + /// Create a Context instance. + /** + * Raises MemoryError if allocating memory fails. + * Raises RuntimeError if creating the context fails. + * + * \param[in] pyargs List of command line arguments + * \param[in] domain_id domain id to be set in this context + */ + Context(py::list pyargs, size_t domain_id); + + /// Retrieves domain id from init_options of context + /** + * \param[in] pycontext Capsule containing rcl_context_t + * \return domain id + */ + size_t + get_domain_id(); + + /// Status of the the client library + /** + * \return True if rcl is running properly, False otherwise + */ + bool + ok(); + + void + shutdown(); + + /// Get rcl_context_t pointer + rcl_context_t * rcl_ptr() const + { + return rcl_context_.get(); + } + + /// Force an early destruction of this object + void destroy() override; + +private: + std::shared_ptr rcl_context_; +}; + +/// Define a pybind11 wrapper for an rclpy::Service +void define_context(py::object module); } // namespace rclpy #endif // RCLPY__CONTEXT_HPP_ diff --git a/rclpy/src/rclpy/destroyable.cpp b/rclpy/src/rclpy/destroyable.cpp index fc8fda849..2865983e8 100644 --- a/rclpy/src/rclpy/destroyable.cpp +++ b/rclpy/src/rclpy/destroyable.cpp @@ -19,8 +19,17 @@ #include "destroyable.hpp" #include "rclpy_common/exceptions.hpp" + namespace rclpy { +Destroyable::Destroyable(const Destroyable &) +{ + // When a destroyable is copied, it does not matter if someone asked + // to destroy the original. The copy has its own lifetime. + use_count = 0; + please_destroy_ = false; +} + void Destroyable::enter() { @@ -53,6 +62,10 @@ Destroyable::destroy() void Destroyable::destroy_when_not_in_use() { + if (please_destroy_) { + // already asked to destroy + return; + } please_destroy_ = true; if (0u == use_count) { destroy(); diff --git a/rclpy/src/rclpy/destroyable.hpp b/rclpy/src/rclpy/destroyable.hpp index 333db0916..92d6b0ad3 100644 --- a/rclpy/src/rclpy/destroyable.hpp +++ b/rclpy/src/rclpy/destroyable.hpp @@ -25,6 +25,12 @@ namespace rclpy class Destroyable { public: + /// Default constructor + Destroyable() = default; + + /// Copy constructor + Destroyable(const Destroyable & other); + /// Context manager __enter__ - block destruction void enter(); diff --git a/rclpy/src/rclpy/graph.cpp b/rclpy/src/rclpy/graph.cpp index b63c8f366..b9af8fa28 100644 --- a/rclpy/src/rclpy/graph.cpp +++ b/rclpy/src/rclpy/graph.cpp @@ -36,19 +36,13 @@ namespace rclpy py::list graph_get_publisher_names_and_types_by_node( - py::capsule pynode, bool no_demangle, + Node & node, bool no_demangle, std::string node_name, std::string node_namespace) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - rcl_names_and_types_t publisher_names_and_types = rcl_get_zero_initialized_names_and_types(); rcl_allocator_t allocator = rcl_get_default_allocator(); rcl_ret_t ret = rcl_get_publisher_names_and_types_by_node( - node, &allocator, no_demangle, node_name.c_str(), + node.rcl_ptr(), &allocator, no_demangle, node_name.c_str(), node_namespace.c_str(), &publisher_names_and_types); if (RCL_RET_OK != ret) { if (RCL_RET_NODE_NAME_NON_EXISTENT == ret) { @@ -75,19 +69,13 @@ graph_get_publisher_names_and_types_by_node( py::list graph_get_subscriber_names_and_types_by_node( - py::capsule pynode, bool no_demangle, + Node & node, bool no_demangle, std::string node_name, std::string node_namespace) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - rcl_names_and_types_t subscriber_names_and_types = rcl_get_zero_initialized_names_and_types(); rcl_allocator_t allocator = rcl_get_default_allocator(); rcl_ret_t ret = rcl_get_subscriber_names_and_types_by_node( - node, &allocator, no_demangle, node_name.c_str(), + node.rcl_ptr(), &allocator, no_demangle, node_name.c_str(), node_namespace.c_str(), &subscriber_names_and_types); if (RCL_RET_OK != ret) { if (RCL_RET_NODE_NAME_NON_EXISTENT == ret) { @@ -114,18 +102,13 @@ graph_get_subscriber_names_and_types_by_node( py::list graph_get_service_names_and_types_by_node( - py::capsule pynode, std::string node_name, std::string node_namespace) + Node & node, std::string node_name, std::string node_namespace) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - rcl_names_and_types_t service_names_and_types = rcl_get_zero_initialized_names_and_types(); rcl_allocator_t allocator = rcl_get_default_allocator(); rcl_ret_t ret = rcl_get_service_names_and_types_by_node( - node, &allocator, node_name.c_str(), node_namespace.c_str(), &service_names_and_types); + node.rcl_ptr(), &allocator, node_name.c_str(), node_namespace.c_str(), + &service_names_and_types); if (RCL_RET_OK != ret) { if (RCL_RET_NODE_NAME_NON_EXISTENT == ret) { throw NodeNameNonExistentError( @@ -151,18 +134,12 @@ graph_get_service_names_and_types_by_node( py::list graph_get_client_names_and_types_by_node( - py::capsule pynode, std::string node_name, std::string node_namespace) + Node & node, std::string node_name, std::string node_namespace) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - rcl_names_and_types_t client_names_and_types = rcl_get_zero_initialized_names_and_types(); rcl_allocator_t allocator = rcl_get_default_allocator(); rcl_ret_t ret = rcl_get_client_names_and_types_by_node( - node, &allocator, node_name.c_str(), node_namespace.c_str(), &client_names_and_types); + node.rcl_ptr(), &allocator, node_name.c_str(), node_namespace.c_str(), &client_names_and_types); if (RCL_RET_OK != ret) { if (RCL_RET_NODE_NAME_NON_EXISTENT == ret) { throw NodeNameNonExistentError( @@ -187,18 +164,12 @@ graph_get_client_names_and_types_by_node( } py::list -graph_get_topic_names_and_types(py::capsule pynode, bool no_demangle) +graph_get_topic_names_and_types(Node & node, bool no_demangle) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - rcl_names_and_types_t topic_names_and_types = rcl_get_zero_initialized_names_and_types(); rcl_allocator_t allocator = rcl_get_default_allocator(); rcl_ret_t ret = - rcl_get_topic_names_and_types(node, &allocator, no_demangle, &topic_names_and_types); + rcl_get_topic_names_and_types(node.rcl_ptr(), &allocator, no_demangle, &topic_names_and_types); if (RCL_RET_OK != ret) { throw RCLError("failed to get topic names and types"); } @@ -220,17 +191,12 @@ graph_get_topic_names_and_types(py::capsule pynode, bool no_demangle) } py::list -graph_get_service_names_and_types(py::capsule pynode) +graph_get_service_names_and_types(Node & node) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - rcl_names_and_types_t service_names_and_types = rcl_get_zero_initialized_names_and_types(); rcl_allocator_t allocator = rcl_get_default_allocator(); - rcl_ret_t ret = rcl_get_service_names_and_types(node, &allocator, &service_names_and_types); + rcl_ret_t ret = rcl_get_service_names_and_types( + node.rcl_ptr(), &allocator, &service_names_and_types); if (RCL_RET_OK != ret) { throw RCLError("failed to get service names and types"); } @@ -259,18 +225,12 @@ typedef rcl_ret_t (* rcl_get_info_by_topic_func_t)( py::list _get_info_by_topic( - py::capsule pynode, + Node & node, const char * topic_name, bool no_mangle, const char * type, rcl_get_info_by_topic_func_t rcl_get_info_by_topic) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - rcutils_allocator_t allocator = rcutils_get_default_allocator(); rcl_topic_endpoint_info_array_t info_array = rcl_get_zero_initialized_topic_endpoint_info_array(); @@ -287,7 +247,8 @@ _get_info_by_topic( } }); - rcl_ret_t ret = rcl_get_info_by_topic(node, &allocator, topic_name, no_mangle, &info_array); + rcl_ret_t ret = rcl_get_info_by_topic( + node.rcl_ptr(), &allocator, topic_name, no_mangle, &info_array); if (RCL_RET_OK != ret) { if (RCL_RET_UNSUPPORTED == ret) { throw NotImplementedError( @@ -304,19 +265,19 @@ _get_info_by_topic( py::list graph_get_publishers_info_by_topic( - py::capsule pynode, const char * topic_name, bool no_mangle) + Node & node, const char * topic_name, bool no_mangle) { return _get_info_by_topic( - pynode, topic_name, no_mangle, "publishers", + node, topic_name, no_mangle, "publishers", rcl_get_publishers_info_by_topic); } py::list graph_get_subscriptions_info_by_topic( - py::capsule pynode, const char * topic_name, bool no_mangle) + Node & node, const char * topic_name, bool no_mangle) { return _get_info_by_topic( - pynode, topic_name, no_mangle, "subscriptions", + node, topic_name, no_mangle, "subscriptions", rcl_get_subscriptions_info_by_topic); } diff --git a/rclpy/src/rclpy/graph.hpp b/rclpy/src/rclpy/graph.hpp index 05ca47604..22967626f 100644 --- a/rclpy/src/rclpy/graph.hpp +++ b/rclpy/src/rclpy/graph.hpp @@ -22,6 +22,8 @@ #include #include +#include "node.hpp" + namespace py = pybind11; namespace rclpy @@ -29,11 +31,10 @@ namespace rclpy /// Get topic names and types for which a remote node has publishers. /** - * Raises ValueError if pynode is not a node capsule * Raises NodeNameNonExistentError if remote node was not found * Raises RCLError if there is an rcl error * - * \param[in] pynode Capsule pointing to the node. + * \param[in] node Node to get publisher topic names and types. * \param[in] no_demangle Whether to demangle topic names following ROS * conventions or not. * \param[in] node_name Name of the remote node to query. @@ -45,16 +46,15 @@ namespace rclpy */ py::list graph_get_publisher_names_and_types_by_node( - py::capsule pynode, bool no_demangle, + Node & node, bool no_demangle, std::string node_name, std::string node_namespace); /// Get topic names and types for which a remote node has subscribers. /** - * Raises ValueError if pynode is not a node capsule * Raises NodeNameNonExistentError if the remote node was not found * Raises RCLError if there is an rcl error * - * \param[in] pynode Capsule pointing to the node. + * \param[in] node node to get subscriber topic names and types * \param[in] no_demangle Whether to demangle topic names following ROS * conventions or not. * \param[in] node_name Name of the remote node to query. @@ -66,16 +66,15 @@ graph_get_publisher_names_and_types_by_node( */ py::list graph_get_subscriber_names_and_types_by_node( - py::capsule pynode, bool no_demangle, + Node & node, bool no_demangle, std::string node_name, std::string node_namespace); /// Get service names and types for which a remote node has servers. /** - * Raises ValueError if pynode is not a node capsule * Raises NodeNameNonExistentError if the remote node was not found * Raises RCLError if there is an rcl error * - * \param[in] pynode Capsule pointing to the node. + * \param[in] node Node to get service topic names and types * \param[in] node_name Name of the remote node to query. * \param[in] node_namespace Namespace of the remote node to query. * \return List of tuples, where the first element of each tuple is the service @@ -85,15 +84,14 @@ graph_get_subscriber_names_and_types_by_node( */ py::list graph_get_service_names_and_types_by_node( - py::capsule pynode, std::string node_name, std::string node_namespace); + Node & node, std::string node_name, std::string node_namespace); /// Get service names and types for which a remote node has clients. /** - * Raises ValueError if pynode is not a node capsule * Raises NodeNameNonExistentError if the remote node was not found * Raises RCLError if there is an rcl error * - * \param[in] pynode Capsule pointing to the node. + * \param[in] node node to get client topic names and types * \param[in] node_name Name of the remote node to query. * \param[in] node_namespace Namespace of the remote node to query. * \return List of tuples, where the first element of each tuple is the service @@ -103,14 +101,13 @@ graph_get_service_names_and_types_by_node( */ py::list graph_get_client_names_and_types_by_node( - py::capsule pynode, std::string node_name, std::string node_namespace); + Node & node, std::string node_name, std::string node_namespace); /// Get all topic names and types in the ROS graph. /** - * Raises ValueError if pynode is not a node capsule * Raises RCLError if there is an rcl error * - * \param[in] pynode Capsule pointing to the node to query the ROS graph. + * \param[in] node node to get topic names and types * \param[in] no_demangle Whether to demangle topic names following ROS * conventions or not. * \return List of tuples, where the first element of each tuple is the topic @@ -119,21 +116,20 @@ graph_get_client_names_and_types_by_node( * \see rcl_get_topic_names_and_types */ py::list -graph_get_topic_names_and_types(py::capsule pynode, bool no_demangle); +graph_get_topic_names_and_types(Node & node, bool no_demangle); /// Get all service names and types in the ROS graph. /** - * Raises ValueError if pynode is not a node capsule * Raises RCLError if there is an rcl error * - * \param[in] pynode Capsule pointing to the node to query the ROS graph. + * \param[in] node Node to get all topic service names and types * \return List of tuples, where the first element of each tuple is the service * name (string) and the second element is a list of service types (list of * strings). * \see rcl_get_service_names_and_types */ py::list -graph_get_service_names_and_types(py::capsule pynode); +graph_get_service_names_and_types(Node & node); /// Return a list of publishers on a given topic. /** @@ -143,7 +139,7 @@ graph_get_service_names_and_types(py::capsule pynode); * Raises NotImplementedError if the call is not supported by RMW * Raises RCLError if there is an rcl error * - * \param[in] pynode Capsule pointing to the node to get the namespace from. + * \param[in] node Node to get topic publisher info * \param[in] topic_name the topic name to get the publishers for. * \param[in] no_mangle if `true`, `topic_name` needs to be a valid middleware topic name, * otherwise it should be a valid ROS topic name. @@ -151,7 +147,7 @@ graph_get_service_names_and_types(py::capsule pynode); */ py::list graph_get_publishers_info_by_topic( - py::capsule pynode, const char * topic_name, bool no_mangle); + Node & node, const char * topic_name, bool no_mangle); /// Return a list of subscriptions on a given topic. /** @@ -161,7 +157,7 @@ graph_get_publishers_info_by_topic( * Raises NotImplementedError if the call is not supported by RMW * Raises RCLError if there is an rcl error * - * \param[in] pynode Capsule pointing to the node to get the namespace from. + * \param[in] node node to get topic subscriber info * \param[in] topic_name the topic name to get the subscriptions for. * \param[in] no_mangle if `true`, `topic_name` needs to be a valid middleware topic name, * otherwise it should be a valid ROS topic name. @@ -169,7 +165,7 @@ graph_get_publishers_info_by_topic( */ py::list graph_get_subscriptions_info_by_topic( - py::capsule pynode, const char * topic_name, bool no_mangle); + Node & node, const char * topic_name, bool no_mangle); } // namespace rclpy diff --git a/rclpy/src/rclpy/guard_condition.cpp b/rclpy/src/rclpy/guard_condition.cpp index 5c0d4a302..4c41cd4c8 100644 --- a/rclpy/src/rclpy/guard_condition.cpp +++ b/rclpy/src/rclpy/guard_condition.cpp @@ -31,14 +31,9 @@ namespace rclpy { -GuardCondition::GuardCondition(py::capsule pycontext) +GuardCondition::GuardCondition(Context & context) +: context_(context) { - auto context = static_cast( - rclpy_handle_get_pointer_from_capsule(pycontext.ptr(), "rcl_context_t")); - if (!context) { - throw py::error_already_set(); - } - rcl_guard_condition_ = std::shared_ptr( new rcl_guard_condition_t, [](rcl_guard_condition_t * guard_condition) @@ -58,8 +53,9 @@ GuardCondition::GuardCondition(py::capsule pycontext) *rcl_guard_condition_ = rcl_get_zero_initialized_guard_condition(); rcl_guard_condition_options_t gc_options = rcl_guard_condition_get_default_options(); - rcl_ret_t ret = rcl_guard_condition_init(rcl_guard_condition_.get(), context, gc_options); - if (ret != RCL_RET_OK) { + rcl_ret_t ret = rcl_guard_condition_init( + rcl_guard_condition_.get(), context.rcl_ptr(), gc_options); + if (RCL_RET_OK != ret) { throw RCLError("failed to create guard_condition"); } } @@ -68,6 +64,7 @@ void GuardCondition::destroy() { rcl_guard_condition_.reset(); + context_.destroy(); } void @@ -83,7 +80,7 @@ GuardCondition::trigger_guard_condition() void define_guard_condition(py::object module) { py::class_>(module, "GuardCondition") - .def(py::init()) + .def(py::init()) .def_property_readonly( "pointer", [](const GuardCondition & guard_condition) { return reinterpret_cast(guard_condition.rcl_ptr()); diff --git a/rclpy/src/rclpy/guard_condition.hpp b/rclpy/src/rclpy/guard_condition.hpp index ef71bbb81..d77f9f0a8 100644 --- a/rclpy/src/rclpy/guard_condition.hpp +++ b/rclpy/src/rclpy/guard_condition.hpp @@ -22,6 +22,7 @@ #include +#include "context.hpp" #include "destroyable.hpp" #include "rclpy_common/exceptions.hpp" #include "rclpy_common/handle.h" @@ -38,7 +39,7 @@ class GuardCondition : public Destroyable, public std::enable_shared_from_this rcl_guard_condition_; /// Handle destructor for guard condition diff --git a/rclpy/src/rclpy/init.cpp b/rclpy/src/rclpy/init.cpp deleted file mode 100644 index 73019815c..000000000 --- a/rclpy/src/rclpy/init.cpp +++ /dev/null @@ -1,154 +0,0 @@ -// Copyright 2021 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -// Include pybind11 before rclpy_common/handle.h includes Python.h -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include - -#include "rclpy_common/handle.h" - -#include "rclpy_common/exceptions.hpp" - -#include "init.hpp" - -namespace rclpy -{ -struct InitOptions -{ - explicit InitOptions(rcl_allocator_t allocator) - { - rcl_options = rcl_get_zero_initialized_init_options(); - rcl_ret_t ret = rcl_init_options_init(&rcl_options, allocator); - if (RCL_RET_OK != ret) { - throw RCLError("Failed to initialize init options"); - } - } - - ~InitOptions() - { - rcl_ret_t ret = rcl_init_options_fini(&rcl_options); - if (RCL_RET_OK != ret) { - RCUTILS_SAFE_FWRITE_TO_STDERR( - "[rclpy|" RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__LINE__) "]: " - "failed to fini rcl_init_options_t in destructor:"); - RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); - RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); - rcl_reset_error(); - } - } - - rcl_init_options_t rcl_options; -}; - -void -init(py::list pyargs, py::capsule pycontext, size_t domain_id) -{ - auto context = static_cast( - rclpy_handle_get_pointer_from_capsule(pycontext.ptr(), "rcl_context_t")); - if (!context) { - throw py::error_already_set(); - } - - // turn the arguments into an array of C-style strings - std::vector arg_c_values(pyargs.size()); - for (size_t i = 0; i < pyargs.size(); ++i) { - // CPython owns const char * memory - no need to free it - arg_c_values[i] = PyUnicode_AsUTF8(pyargs[i].ptr()); - if (!arg_c_values[i]) { - throw py::error_already_set(); - } - } - - InitOptions init_options(rcl_get_default_allocator()); - - // Set domain id - rcl_ret_t ret = rcl_init_options_set_domain_id(&init_options.rcl_options, domain_id); - if (RCL_RET_OK != ret) { - throw RCLError("failed to set domain id to init options"); - } - - if (arg_c_values.size() > static_cast(std::numeric_limits::max())) { - throw std::range_error("Too many cli arguments"); - } - int argc = static_cast(arg_c_values.size()); - const char ** argv = argc > 0 ? &(arg_c_values[0]) : nullptr; - ret = rcl_init(argc, argv, &init_options.rcl_options, context); - if (RCL_RET_OK != ret) { - throw RCLError("failed to initialize rcl"); - } - - throw_if_unparsed_ros_args(pyargs, context->global_arguments); -} - -void -shutdown(py::capsule pycontext) -{ - auto context = static_cast( - rclpy_handle_get_pointer_from_capsule(pycontext.ptr(), "rcl_context_t")); - if (!context) { - throw py::error_already_set(); - } - - rcl_ret_t ret = rcl_shutdown(context); - if (RCL_RET_OK != ret) { - throw RCLError("failed to shutdown"); - } -} - -void -throw_if_unparsed_ros_args(py::list pyargs, const rcl_arguments_t & rcl_args) -{ - int unparsed_ros_args_count = rcl_arguments_get_count_unparsed_ros(&rcl_args); - - if (unparsed_ros_args_count < 0) { - throw std::runtime_error("failed to count unparsed arguments"); - } - if (0 == unparsed_ros_args_count) { - return; - } - - rcl_allocator_t allocator = rcl_get_default_allocator(); - - int * unparsed_indices_c = nullptr; - rcl_ret_t ret = rcl_arguments_get_unparsed_ros(&rcl_args, allocator, &unparsed_indices_c); - if (RCL_RET_OK != ret) { - throw RCLError("failed to get unparsed arguments"); - } - - auto deallocator = [&](int ptr[]) {allocator.deallocate(ptr, allocator.state);}; - auto unparsed_indices = std::unique_ptr( - unparsed_indices_c, deallocator); - - py::list unparsed_args; - for (int i = 0; i < unparsed_ros_args_count; ++i) { - int index = unparsed_indices_c[i]; - if (index < 0 || static_cast(index) >= pyargs.size()) { - throw std::runtime_error("got invalid unparsed ROS arg index"); - } - unparsed_args.append(pyargs[index]); - } - - throw UnknownROSArgsError(static_cast(py::repr(unparsed_args))); -} -} // namespace rclpy diff --git a/rclpy/src/rclpy/init.hpp b/rclpy/src/rclpy/init.hpp deleted file mode 100644 index e5c4f829d..000000000 --- a/rclpy/src/rclpy/init.hpp +++ /dev/null @@ -1,50 +0,0 @@ -// Copyright 2021 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef RCLPY__INIT_HPP_ -#define RCLPY__INIT_HPP_ - -#include - -#include - -#include -#include - -namespace py = pybind11; - -namespace rclpy -{ -/// Initialize rcl with default options, ignoring parameters -/** - * Raises RCLError if rcl could not be initialized - * Raises UnknownROSArgsError if any CLI arguments could not be parsed - * Raises RuntimeError if an internal error happens - */ -void -init(py::list pyargs, py::capsule pycontext, size_t domain_id); - -/// Request shutdown of the client library -/** - * Raises RCLError if the library could not be shutdown - */ -void -shutdown(py::capsule pycontext); - -/// Throw UnparsedROSArgsError with a message saying which args are unparsed. -void -throw_if_unparsed_ros_args(py::list pyargs, const rcl_arguments_t & rcl_args); -} // namespace rclpy - -#endif // RCLPY__INIT_HPP_ diff --git a/rclpy/src/rclpy/logging.cpp b/rclpy/src/rclpy/logging.cpp index f95fb8503..f6d642149 100644 --- a/rclpy/src/rclpy/logging.cpp +++ b/rclpy/src/rclpy/logging.cpp @@ -62,18 +62,13 @@ rclpy_thread_safe_logging_output_handler( } void -logging_configure(py::object pycontext) +logging_configure(Context & context) { - auto context = static_cast( - rclpy_handle_get_pointer_from_capsule(pycontext.ptr(), "rcl_context_t")); - if (!context) { - throw py::error_already_set(); - } rcl_allocator_t allocator = rcl_get_default_allocator(); rclpy::LoggingGuard scoped_logging_guard; rcl_ret_t ret = rcl_logging_configure_with_output_handler( - &context->global_arguments, + &context.rcl_ptr()->global_arguments, &allocator, rclpy_thread_safe_logging_output_handler); if (RCL_RET_OK != ret) { diff --git a/rclpy/src/rclpy/logging.hpp b/rclpy/src/rclpy/logging.hpp index c5152df8f..f97c9cb34 100644 --- a/rclpy/src/rclpy/logging.hpp +++ b/rclpy/src/rclpy/logging.hpp @@ -19,6 +19,8 @@ #include +#include "context.hpp" + namespace py = pybind11; namespace rclpy @@ -40,7 +42,7 @@ class LoggingGuard * \param[in] pycontext a context instance to use to retrieve global CLI arguments */ void -logging_configure(py::object pycontext); +logging_configure(Context & _context); /// Finalize rcl logging void diff --git a/rclpy/src/rclpy/names.cpp b/rclpy/src/rclpy/names.cpp index 5f4fdf23a..e265af6ce 100644 --- a/rclpy/src/rclpy/names.cpp +++ b/rclpy/src/rclpy/names.cpp @@ -209,23 +209,17 @@ expand_topic_name(const char * topic, const char * node_name, const char * node_ } std::string -remap_topic_name(py::capsule pynode, const char * topic_name) +remap_topic_name(Node & node, const char * topic_name) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - // Get the node options - const rcl_node_options_t * node_options = rcl_node_get_options(node); + const rcl_node_options_t * node_options = rcl_node_get_options(node.rcl_ptr()); if (nullptr == node_options) { throw RCLError("failed to get node options"); } const rcl_arguments_t * global_args = nullptr; if (node_options->use_global_arguments) { - global_args = &(node->context->global_arguments); + global_args = &(node.rcl_ptr()->context->global_arguments); } char * output_cstr = nullptr; @@ -233,8 +227,8 @@ remap_topic_name(py::capsule pynode, const char * topic_name) &(node_options->arguments), global_args, topic_name, - rcl_node_get_name(node), - rcl_node_get_namespace(node), + rcl_node_get_name(node.rcl_ptr()), + rcl_node_get_namespace(node.rcl_ptr()), node_options->allocator, &output_cstr); if (RCL_RET_OK != ret) { @@ -256,22 +250,16 @@ remap_topic_name(py::capsule pynode, const char * topic_name) } std::string -resolve_name(py::capsule pynode, const char * topic_name, bool only_expand, bool is_service) +resolve_name(Node & node, const char * topic_name, bool only_expand, bool is_service) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - - const rcl_node_options_t * node_options = rcl_node_get_options(node); + const rcl_node_options_t * node_options = rcl_node_get_options(node.rcl_ptr()); if (nullptr == node_options) { throw RCLError("failed to get node options"); } char * output_cstr = nullptr; rcl_ret_t ret = rcl_node_resolve_name( - node, + node.rcl_ptr(), topic_name, node_options->allocator, is_service, diff --git a/rclpy/src/rclpy/names.hpp b/rclpy/src/rclpy/names.hpp index e79e33440..6ae90e822 100644 --- a/rclpy/src/rclpy/names.hpp +++ b/rclpy/src/rclpy/names.hpp @@ -19,6 +19,8 @@ #include +#include "node.hpp" + namespace py = pybind11; namespace rclpy @@ -94,26 +96,26 @@ expand_topic_name(const char * topic, const char * node_name, const char * node_ * Raises ValueError if the capsule is not the correct type * Raises RCLError if an unexpected error happens * - * \param[in] pynode Capsule pointing to the node + * \param[in] Node node to remap the topic name * \param[in] topic_name topic string to be remapped * \return remapped topic name */ std::string -remap_topic_name(py::capsule pynode, const char * topic_name); +remap_topic_name(Node & node, const char * topic_name); /// Expand and remap a topic name /** * Raises ValueError if the capsule is not the correct type * Raises RCLError if an unexpected error happens * - * \param[in] pynode Capsule pointing to the node + * \param[in] node node to expand and remap a topic name * \param[in] topic_name topic string to be remapped * \param[in] only_expand when `false`, remapping rules are ignored * \param[in] is_service `true` for service names, `false` for topic names * \return expanded and remapped topic name */ std::string -resolve_name(py::capsule pynode, const char * topic_name, bool only_expand, bool is_service); +resolve_name(Node & node, const char * topic_name, bool only_expand, bool is_service); } // namespace rclpy #endif // RCLPY__NAMES_HPP_ diff --git a/rclpy/src/rclpy/node.cpp b/rclpy/src/rclpy/node.cpp index 43d233012..c41138c92 100644 --- a/rclpy/src/rclpy/node.cpp +++ b/rclpy/src/rclpy/node.cpp @@ -15,6 +15,7 @@ // Include pybind11 before rclpy_common/handle.h includes Python.h #include +#include #include #include #include @@ -34,24 +35,16 @@ #include "rclpy_common/exceptions.hpp" #include "rclpy_common/handle.h" -#include "init.hpp" #include "logging.hpp" #include "node.hpp" #include "utils.hpp" - namespace rclpy { const char * -get_node_fully_qualified_name(py::capsule pynode) +Node::get_fully_qualified_name() { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - - const char * fully_qualified_node_name = rcl_node_get_fully_qualified_name(node); + const char * fully_qualified_node_name = rcl_node_get_fully_qualified_name(rcl_node_.get()); if (!fully_qualified_node_name) { throw RCLError("Fully qualified name not set"); } @@ -60,15 +53,9 @@ get_node_fully_qualified_name(py::capsule pynode) } const char * -get_node_logger_name(py::capsule pynode) +Node::logger_name() { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - - const char * node_logger_name = rcl_node_get_logger_name(node); + const char * node_logger_name = rcl_node_get_logger_name(rcl_node_.get()); if (!node_logger_name) { throw RCLError("Logger name not set"); } @@ -77,15 +64,9 @@ get_node_logger_name(py::capsule pynode) } const char * -get_node_name(py::capsule pynode) +Node::get_node_name() { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - - const char * node_name = rcl_node_get_name(node); + const char * node_name = rcl_node_get_name(rcl_node_.get()); if (!node_name) { throw RCLError("Node name not set"); } @@ -94,15 +75,9 @@ get_node_name(py::capsule pynode) } const char * -get_node_namespace(py::capsule pynode) +Node::get_namespace() { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - - const char * node_namespace = rcl_node_get_namespace(node); + const char * node_namespace = rcl_node_get_namespace(rcl_node_.get()); if (!node_namespace) { throw RCLError("Node namespace not set"); } @@ -111,16 +86,10 @@ get_node_namespace(py::capsule pynode) } size_t -get_count_publishers(py::capsule pynode, const char * topic_name) +Node::get_count_publishers(const char * topic_name) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - size_t count = 0; - rcl_ret_t ret = rcl_count_publishers(node, topic_name, &count); + rcl_ret_t ret = rcl_count_publishers(rcl_node_.get(), topic_name, &count); if (RCL_RET_OK != ret) { throw RCLError("Error in rcl_count_publishers"); } @@ -129,16 +98,10 @@ get_count_publishers(py::capsule pynode, const char * topic_name) } size_t -get_count_subscribers(py::capsule pynode, const char * topic_name) +Node::get_count_subscribers(const char * topic_name) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - size_t count = 0; - rcl_ret_t ret = rcl_count_subscribers(node, topic_name, &count); + rcl_ret_t ret = rcl_count_subscribers(rcl_node_.get(), topic_name, &count); if (RCL_RET_OK != ret) { throw RCLError("Error in rcl_count_subscribers"); } @@ -147,14 +110,8 @@ get_count_subscribers(py::capsule pynode, const char * topic_name) } py::list -get_node_names_impl(py::capsule pynode, bool get_enclaves) +Node::get_names_impl(bool get_enclaves) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - rcl_allocator_t allocator = rcl_get_default_allocator(); rcutils_string_array_t node_names = rcutils_get_zero_initialized_string_array(); rcutils_string_array_t node_namespaces = rcutils_get_zero_initialized_string_array(); @@ -163,10 +120,10 @@ get_node_names_impl(py::capsule pynode, bool get_enclaves) rcl_ret_t ret = RCL_RET_OK; if (get_enclaves) { ret = rcl_get_node_names_with_enclaves( - node, allocator, &node_names, &node_namespaces, &enclaves); + rcl_node_.get(), allocator, &node_names, &node_namespaces, &enclaves); } else { ret = rcl_get_node_names( - node, allocator, &node_names, &node_namespaces); + rcl_node_.get(), allocator, &node_names, &node_namespaces); } if (RCL_RET_OK != ret) { throw RCLError("Failed to get node names"); @@ -176,29 +133,32 @@ get_node_names_impl(py::capsule pynode, bool get_enclaves) { rcutils_ret_t fini_ret = rcutils_string_array_fini(&node_names); if (RCUTILS_RET_OK != fini_ret) { - RCUTILS_SAFE_FWRITE_TO_STDERR( - "[rclpy|" RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__LINE__) "]: " - "failed to fini node names during error handling: "); - RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); - RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); + // Warning should use line number of the current stack frame + int stack_level = 1; + PyErr_WarnFormat( + PyExc_RuntimeWarning, stack_level, + "[rclpy| %s : %s ]: failed to fini node names during error handling: %s", + RCUTILS_STRINGIFY(__FILE__), RCUTILS_STRINGIFY(__LINE__), rcl_get_error_string().str); rcl_reset_error(); } fini_ret = rcutils_string_array_fini(&node_namespaces); if (RCUTILS_RET_OK != fini_ret) { - RCUTILS_SAFE_FWRITE_TO_STDERR( - "[rclpy|" RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__LINE__) "]: " - "failed to fini node namespaces during error handling: "); - RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); - RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); + // Warning should use line number of the current stack frame + int stack_level = 1; + PyErr_WarnFormat( + PyExc_RuntimeWarning, stack_level, + "[rclpy| %s : %s ]: failed to fini node namespaces during error handling: %s", + RCUTILS_STRINGIFY(__FILE__), RCUTILS_STRINGIFY(__LINE__), rcl_get_error_string().str); rcl_reset_error(); } fini_ret = rcutils_string_array_fini(&enclaves); if (RCUTILS_RET_OK != fini_ret) { - RCUTILS_SAFE_FWRITE_TO_STDERR( - "[rclpy|" RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__LINE__) "]: " - "failed to fini enclaves string array during error handling: "); - RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); - RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); + // Warning should use line number of the current stack frame + int stack_level = 1; + PyErr_WarnFormat( + PyExc_RuntimeWarning, stack_level, + "[rclpy| %s : %s ]: failed to fini enclaves string array during error handling: %s", + RCUTILS_STRINGIFY(__FILE__), RCUTILS_STRINGIFY(__LINE__), rcl_get_error_string().str); rcl_reset_error(); } }); @@ -221,15 +181,15 @@ get_node_names_impl(py::capsule pynode, bool get_enclaves) } py::list -get_node_names_and_namespaces(py::capsule pynode) +Node::get_node_names_and_namespaces() { - return get_node_names_impl(pynode, false); + return get_names_impl(false); } py::list -get_node_names_and_namespaces_with_enclaves(py::capsule pynode) +Node::get_node_names_and_namespaces_with_enclaves() { - return get_node_names_impl(pynode, true); + return get_names_impl(true); } /// Create an rclpy.parameter.Parameter from an rcl_variant_t @@ -362,22 +322,16 @@ _parse_param_overrides( } py::dict -get_node_parameters(py::object pyparameter_cls, py::capsule pynode) +Node::get_parameters(py::object pyparameter_cls) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - py::dict params_by_node_name; py::object parameter_type_cls = pyparameter_cls.attr("Type"); - const rcl_node_options_t * node_options = rcl_node_get_options(node); + const rcl_node_options_t * node_options = rcl_node_get_options(rcl_node_.get()); if (node_options->use_global_arguments) { _parse_param_overrides( - &(node->context->global_arguments), pyparameter_cls, + &(rcl_node_.get()->context->global_arguments), pyparameter_cls, parameter_type_cls, params_by_node_name); } @@ -385,7 +339,7 @@ get_node_parameters(py::object pyparameter_cls, py::capsule pynode) &(node_options->arguments), pyparameter_cls, parameter_type_cls, params_by_node_name); - const char * node_fqn = rcl_node_get_fully_qualified_name(node); + const char * node_fqn = rcl_node_get_fully_qualified_name(rcl_node_.get()); if (!node_fqn) { throw RCLError("failed to get node fully qualified name"); } @@ -409,49 +363,22 @@ get_node_parameters(py::object pyparameter_cls, py::capsule pynode) return node_params; } -/// Handle destructor for node -static -void -_rclpy_destroy_node(void * p) +void Node::destroy() { - rclpy::LoggingGuard scoped_logging_guard; - auto node = static_cast(p); - if (!node) { - // Warning should use line number of the current stack frame - int stack_level = 1; - PyErr_WarnFormat( - PyExc_RuntimeWarning, stack_level, "_rclpy_destroy_node got a NULL pointer"); - return; - } - - rcl_ret_t ret = rcl_node_fini(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 node: %s", - rcl_get_error_string().str); - } - PyMem_Free(node); + rcl_node_.reset(); + context_.destroy(); } -py::capsule -create_node( +Node::Node( const char * node_name, const char * namespace_, - py::capsule pycontext, + Context & context, py::object pycli_args, bool use_global_arguments, bool enable_rosout) +: context_(context) { rcl_ret_t ret; - - auto context = static_cast( - rclpy_handle_get_pointer_from_capsule(pycontext.ptr(), "rcl_context_t")); - if (!context) { - throw py::error_already_set(); - } - rcl_arguments_t arguments = rcl_get_zero_initialized_arguments(); // turn the arguments into an array of C-style strings @@ -503,14 +430,23 @@ create_node( throw_if_unparsed_ros_args(pyargs, arguments); - auto deleter = [](rcl_node_t * ptr) {_rclpy_destroy_node(ptr);}; - auto node = std::unique_ptr( - static_cast(PyMem_Malloc(sizeof(rcl_node_t))), - deleter); - if (!node) { - throw std::bad_alloc(); - } - *node = rcl_get_zero_initialized_node(); + rcl_node_ = std::shared_ptr( + new rcl_node_t, + [](rcl_node_t * node) + { + rcl_ret_t ret = rcl_node_fini(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 node: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + delete node; + }); + + *rcl_node_ = rcl_get_zero_initialized_node(); rcl_node_options_t options = rcl_node_get_default_options(); options.use_global_arguments = use_global_arguments; options.arguments = arguments; @@ -518,7 +454,8 @@ create_node( { rclpy::LoggingGuard scoped_logging_guard; - ret = rcl_node_init(node.get(), node_name, namespace_, context, &options); + ret = rcl_node_init( + rcl_node_.get(), node_name, namespace_, context.rcl_ptr(), &options); } if (RCL_RET_BAD_ALLOC == ret) { @@ -534,22 +471,107 @@ create_node( if (RCL_RET_OK != ret) { throw RCLError("error creating node"); } +} + +py::list +Node::get_action_client_names_and_types_by_node( + const char * remote_node_name, const char * remote_node_namespace) +{ + rcl_names_and_types_t names_and_types = rcl_get_zero_initialized_names_and_types(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_ret_t ret = rcl_action_get_client_names_and_types_by_node( + rcl_node_.get(), + &allocator, + remote_node_name, + remote_node_namespace, + &names_and_types); + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed to get action client names and type"); + } + + return convert_to_py_names_and_types(&names_and_types); +} - PyObject * pynode_c = rclpy_create_handle_capsule(node.get(), "rcl_node_t", _rclpy_destroy_node); - if (!pynode_c) { - throw py::error_already_set(); +py::list +Node::get_action_server_names_and_types_by_node( + const char * remote_node_name, const char * remote_node_namespace) +{ + rcl_names_and_types_t names_and_types = rcl_get_zero_initialized_names_and_types(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_ret_t ret = rcl_action_get_server_names_and_types_by_node( + rcl_node_.get(), + &allocator, + remote_node_name, + remote_node_namespace, + &names_and_types); + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed to get action server names and type"); } - auto pynode = py::reinterpret_steal(pynode_c); - // pynode now owns the rcl_node_t - node.release(); - - auto node_handle = static_cast(pynode); - auto context_handle = static_cast(pycontext); - _rclpy_handle_add_dependency(node_handle, context_handle); - if (PyErr_Occurred()) { - throw py::error_already_set(); + + return convert_to_py_names_and_types(&names_and_types); +} + +py::list +Node::get_action_names_and_types() +{ + rcl_names_and_types_t names_and_types = rcl_get_zero_initialized_names_and_types(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_ret_t ret = rcl_action_get_names_and_types(rcl_node_.get(), &allocator, &names_and_types); + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed to get action names and type"); } - return pynode; + return convert_to_py_names_and_types(&names_and_types); +} + +void +define_node(py::object module) +{ + py::class_>(module, "Node") + .def(py::init()) + .def_property_readonly( + "pointer", [](const Node & node) { + return reinterpret_cast(node.rcl_ptr()); + }, + "Get the address of the entity as an integer") + .def( + "get_fully_qualified_name", &Node::get_fully_qualified_name, + "Get the fully qualified name of the node.") + .def( + "logger_name", &Node::logger_name, + "Get the name of the logger associated with a node.") + .def( + "get_node_name", &Node::get_node_name, + "Get the name of a node.") + .def( + "get_namespace", &Node::get_namespace, + "Get the namespace of a node.") + .def( + "get_count_publishers", &Node::get_count_publishers, + "Returns the count of all the publishers known for that topic in the entire ROS graph.") + .def( + "get_count_subscribers", &Node::get_count_subscribers, + "Returns the count of all the subscribers known for that topic in the entire ROS graph.") + .def( + "get_node_names_and_namespaces", &Node::get_node_names_and_namespaces, + "Get the list of nodes discovered by the provided node") + .def( + "get_node_names_and_namespaces_with_enclaves", + &Node::get_node_names_and_namespaces_with_enclaves, + "Get the list of nodes discovered by the provided node, with their respective enclaves.") + .def( + "get_action_client_names_and_types_by_node", + &Node::get_action_client_names_and_types_by_node, + "Get action client names and types by node.") + .def( + "get_action_server_names_and_types_by_node", + &Node::get_action_server_names_and_types_by_node, + "Get action server names and types by node.") + .def( + "get_action_names_and_types", &Node::get_action_names_and_types, + "Get action names and types.") + .def( + "get_parameters", &Node::get_parameters, + "Get a list of parameters for the current node"); } } // namespace rclpy diff --git a/rclpy/src/rclpy/node.hpp b/rclpy/src/rclpy/node.hpp index e09a463b3..fb6eeb289 100644 --- a/rclpy/src/rclpy/node.hpp +++ b/rclpy/src/rclpy/node.hpp @@ -17,157 +17,189 @@ #include +#include // rcl_names_and_types_t + +#include + +#include "context.hpp" +#include "destroyable.hpp" + namespace py = pybind11; namespace rclpy { -/// Get the fully qualified name of the node. -/** - * Raises ValueError if pynode is not a node capsule - * Raises RCLError if name is not set - * - * \param[in] pynode Capsule pointing to the node - * \return String containing the fully qualified name of the node - */ -const char * -get_node_fully_qualified_name(py::capsule pynode); - -/// Get the name of the logger associated with a node. -/** - * Raises ValueError if pynode is not a node capsule - * Raises RCLError if logger name is not set - * - * \param[in] pynode Capsule pointing to the node to get the logger name of - * \return logger_name - */ -const char * -get_node_logger_name(py::capsule pynode); - -/// Get the name of a node. -/** - * Raises ValueError if pynode is not a node capsule - * Raises RCLError if name is not set - * - * \param[in] pynode Capsule pointing to the node to get the name from - * \return name of the node - */ -const char * -get_node_name(py::capsule pynode); - -/// Get the namespace of a node. -/** - * Raises ValueError if pynode is not a node capsule - * Raises RCLError if namespace is not set - * - * \param[in] pynode Capsule pointing to the node to get the namespace from - * \return namespace - */ -const char * -get_node_namespace(py::capsule pynode); - -/// Returns the count of all the publishers known for that topic in the entire ROS graph. -/* - * Raises ValueError if pynode is not a node capsule - * Raises RCLError if an error occurs in rcl - * - * \param[in] pynode pynode Capsule pointing to a node - * \param[in] topic_name Name of the topic to count the number of publishers - * \return the count of all the publishers known for that topic in the entire ROS graph. - */ -size_t -get_count_publishers(py::capsule pynode, const char * topic_name); - -/// Returns the count of all the subscribers known for that topic in the entire ROS graph -/* - * - * Raises ValueError if pynode is not a node capsule - * Raises RCLError if an error occurs in rcl - * - * \param[in] pynode pynode Capsule pointing to a node - * \param[in] topic_name Name of the topic to count the number of subscribers - * \return the count of all the subscribers known for that topic in the entire ROS graph - */ -size_t -get_count_subscribers(py::capsule pynode, const char * topic_name); - -/// Get the list of nodes discovered by the provided node -/** - * Raises ValueError if pynode is not a node capsule - * Raises RCLError if the names are unavailable. - * - * \param[in] module the Python module this function is part of - * \param[in] pynode Capsule pointing to the node - * \param[in] get_enclaves specifies if the output includes the enclaves names - * or not - * \return Python list of tuples, containing: - * node name, node namespace, and - * enclave if `get_enclaves` is true. - */ -py::list -get_node_names_impl(py::capsule pynode, bool get_enclaves); - -/// Get the list of nodes discovered by the provided node -/** - * Raises ValueError if pynode is not a node capsule - * Raises RCLError if the names are unavailable. - * - * \param[in] pynode Capsule pointing to the node - * \return Python list of tuples where each tuple contains the two strings: - * the node name and node namespace - */ -py::list -get_node_names_and_namespaces(py::capsule pynode); - -/// Get the list of nodes discovered by the provided node, with their respective enclaves. -/** - * Raises ValueError if pynode is not a node capsule - * Raises RCLError if the names are unavailable. - * - * \param[in] pynode Capsule pointing to the node - * \return Python list of tuples where each tuple contains three strings: - * node name, node namespace, and enclave. - */ -py::list -get_node_names_and_namespaces_with_enclaves(py::capsule pynode); - -/// Get a list of parameters for the current node -/** - * Raises ValueError if the argument is not a node handle - * Raises RCLError if any rcl function call fails - * Raises AttributeError if pyparameter_cls doesn't have a 'Type' attribute - * Raises RuntimeError if assumptions about rcl structures are violated - * - * \param[in] pyparameter_cls The rclpy.parameter.Parameter class object. - * \param[in] node_capsule Capsule pointing to the node handle - * \return A dict mapping parameter names to rclpy.parameter.Parameter (may be empty). - */ -py::dict -get_node_parameters(py::object pyparameter_cls, py::capsule pynode); - -/// Create a node -/** - * Raises ValueError if the node name or namespace is invalid - * Raises RCLError if the node could not be initialized for an unexpected reason - * Raises RCLInvalidROSArgsError if the given CLI arguments could not be parsed - * Raises UnknownROSArgsError if there are unknown CLI arguments given - * Raises MemoryError if memory could not be allocated for the node - * - * \param[in] node_name name of the node to be created - * \param[in] namespace namespace for the node - * \param[in] pycontext Capsule for an rcl_context_t - * \param[in] pycli_args a sequence of command line arguments for just this node, or None - * \param[in] use_global_arguments if true then the node will also use cli arguments on pycontext - * \param[in] enable rosout if true then enable rosout logging - * \return Capsule of the pointer to the created rcl_node_t * structure - */ -py::capsule -create_node( - const char * node_name, - const char * namespace_, - py::capsule pycontext, - py::object pycli_args, - bool use_global_arguments, - bool enable_rosout); -} // namespace rclpy +class Node : public Destroyable, public std::enable_shared_from_this +{ +public: + /// Create a node + /** + * Raises ValueError if the node name or namespace is invalid + * Raises RCLError if the node could not be initialized for an unexpected reason + * Raises RCLInvalidROSArgsError if the given CLI arguments could not be parsed + * Raises UnknownROSArgsError if there are unknown CLI arguments given + * Raises MemoryError if memory could not be allocated for the node + * + * \param[in] node_name name of the node to be created + * \param[in] namespace namespace for the node + * \param[in] context Context + * \param[in] pycli_args a sequence of command line arguments for just this node, or None + * \param[in] use_global_arguments if true then the node will also use cli arguments on context + * \param[in] enable rosout if true then enable rosout logging + */ + Node( + const char * node_name, + const char * namespace_, + Context & context, + py::object pycli_args, + bool use_global_arguments, + bool enable_rosout); + + /// Get the fully qualified name of the node. + /** + * Raises RCLError if name is not set + * + * \return String containing the fully qualified name of the node + */ + const char * + get_fully_qualified_name(); + + /// Get the name of the logger associated with a node. + /** + * Raises RCLError if logger name is not set + * + * \return logger_name + */ + const char * + logger_name(); + + /// Get the name of a node. + /** + * Raises RCLError if name is not set + * + * \return name of the node + */ + const char * + get_node_name(); + + /// Get the namespace of a node. + /** + * Raises RCLError if namespace is not set + * + * \return namespace + */ + const char * + get_namespace(); + + /// Returns the count of all the publishers known for that topic in the entire ROS graph. + /* + * Raises RCLError if an error occurs in rcl + * + * \param[in] topic_name Name of the topic to count the number of publishers + * \return the count of all the publishers known for that topic in the entire ROS graph. + */ + size_t + get_count_publishers(const char * topic_name); + + /// Returns the count of all the subscribers known for that topic in the entire ROS graph + /* + * + * Raises RCLError if an error occurs in rcl + * + * \param[in] topic_name Name of the topic to count the number of subscribers + * \return the count of all the subscribers known for that topic in the entire ROS graph + */ + size_t + get_count_subscribers(const char * topic_name); + /// Get the list of nodes discovered by the provided node + /** + * Raises RCLError if the names are unavailable. + * + * \return Python list of tuples where each tuple contains the two strings: + * the node name and node namespace + */ + py::list + get_node_names_and_namespaces(); + + /// Get the list of nodes discovered by the provided node, with their respective enclaves. + /** + * Raises RCLError if the names are unavailable. + * + * \return Python list of tuples where each tuple contains three strings: + * node name, node namespace, and enclave. + */ + py::list + get_node_names_and_namespaces_with_enclaves(); + + /// Get a list of parameters for the current node + /** + * Raises RCLError if any rcl function call fails + * Raises AttributeError if pyparameter_cls doesn't have a 'Type' attribute + * Raises RuntimeError if assumptions about rcl structures are violated + * + * \param[in] pyparameter_cls The rclpy.parameter.Parameter class object. + * \return A dict mapping parameter names to rclpy.parameter.Parameter (may be empty). + */ + py::dict + get_parameters(py::object pyparameter_cls); + + /// Get action client names and types by node. + /* + * \param[in] remote_node_name the node name of the actions to return + * \param[in] remote_node_namespace the node namespace of the actions to return + * \return list of action client names and their types + */ + py::list + get_action_client_names_and_types_by_node( + const char * remote_node_name, const char * remote_node_namespace); + + /// Get action server names and types by node. + /* + * \param[in] remote_node_name the node name of the actions to return + * \param[in] remote_node_namespace the node namespace of the actions to return + * \return list of action server names and their types + */ + py::list + get_action_server_names_and_types_by_node( + const char * remote_node_name, const char * remote_node_namespace); + + /// Get action names and types. + /* + * \return list of action names and types + */ + py::list + get_action_names_and_types(); + + /// Get rcl_node_t pointer + rcl_node_t * + rcl_ptr() const + { + return rcl_node_.get(); + } + + /// Force an early destruction of this object + void + destroy() override; + +private: + /// Get the list of nodes discovered by the provided node + /** + * Raises RCLError if the names are unavailable. + * + * \param[in] get_enclaves specifies if the output includes the enclaves names + * or not + * \return Python list of tuples, containing: + * node name, node namespace, and + * enclave if `get_enclaves` is true. + */ + py::list + get_names_impl(bool get_enclaves); + + Context context_; + std::shared_ptr rcl_node_; +}; + +void +define_node(py::object module); +} // namespace rclpy #endif // RCLPY__NODE_HPP_ diff --git a/rclpy/src/rclpy/publisher.cpp b/rclpy/src/rclpy/publisher.cpp index c9caf916c..467d8f56f 100644 --- a/rclpy/src/rclpy/publisher.cpp +++ b/rclpy/src/rclpy/publisher.cpp @@ -21,7 +21,6 @@ #include #include "rclpy_common/common.h" -#include "rclpy_common/handle.h" #include "rclpy_common/exceptions.hpp" @@ -30,12 +29,10 @@ namespace rclpy { Publisher::Publisher( - py::capsule pynode, py::object pymsg_type, std::string topic, + Node & node, py::object pymsg_type, std::string topic, py::object pyqos_profile) -: node_handle_(std::make_shared(pynode)) +: node_(node) { - auto node = node_handle_->cast("rcl_node_t"); - auto msg_type = static_cast( rclpy_common_get_type_support(pymsg_type.ptr())); if (!msg_type) { @@ -50,11 +47,10 @@ Publisher::Publisher( rcl_publisher_ = std::shared_ptr( new rcl_publisher_t, - [this](rcl_publisher_t * publisher) + [node](rcl_publisher_t * publisher) { - auto node = node_handle_->cast_or_warn("rcl_node_t"); - - rcl_ret_t ret = rcl_publisher_fini(publisher, node); + // Intentionally capturing node by value so shared_ptr can be transfered to copies + rcl_ret_t ret = rcl_publisher_fini(publisher, node.rcl_ptr()); if (RCL_RET_OK != ret) { // Warning should use line number of the current stack frame int stack_level = 1; @@ -69,7 +65,7 @@ Publisher::Publisher( *rcl_publisher_ = rcl_get_zero_initialized_publisher(); rcl_ret_t ret = rcl_publisher_init( - rcl_publisher_.get(), node, msg_type, + rcl_publisher_.get(), node_.rcl_ptr(), msg_type, topic.c_str(), &publisher_ops); if (RCL_RET_OK != ret) { if (RCL_RET_TOPIC_NAME_INVALID == ret) { @@ -85,15 +81,13 @@ Publisher::Publisher( void Publisher::destroy() { rcl_publisher_.reset(); - node_handle_.reset(); + node_.destroy(); } const char * Publisher::get_logger_name() { - auto node = node_handle_->cast("rcl_node_t"); - - const char * node_logger_name = rcl_node_get_logger_name(node); + const char * node_logger_name = rcl_node_get_logger_name(node_.rcl_ptr()); if (!node_logger_name) { throw RCLError("Node logger name not set"); } @@ -158,7 +152,7 @@ void define_publisher(py::object module) { py::class_>(module, "Publisher") - .def(py::init()) + .def(py::init()) .def_property_readonly( "pointer", [](const Publisher & publisher) { return reinterpret_cast(publisher.rcl_ptr()); diff --git a/rclpy/src/rclpy/publisher.hpp b/rclpy/src/rclpy/publisher.hpp index ce91b9d4d..b40b9a2d6 100644 --- a/rclpy/src/rclpy/publisher.hpp +++ b/rclpy/src/rclpy/publisher.hpp @@ -24,7 +24,7 @@ #include #include "destroyable.hpp" -#include "handle.hpp" +#include "node.hpp" namespace py = pybind11; @@ -44,13 +44,13 @@ class Publisher : public Destroyable, public std::enable_shared_from_this node_handle_; + Node node_; std::shared_ptr rcl_publisher_; }; /// Define a pybind11 wrapper for an rclpy::Service diff --git a/rclpy/src/rclpy/qos_event.cpp b/rclpy/src/rclpy/qos_event.cpp index d22d24e06..10abd191f 100644 --- a/rclpy/src/rclpy/qos_event.cpp +++ b/rclpy/src/rclpy/qos_event.cpp @@ -21,6 +21,7 @@ #include #include +#include #include // NOLINT #include "rclpy_common/common.h" @@ -57,22 +58,20 @@ create_zero_initialized_event() void QoSEvent::destroy() { - grandparent_pub_handle_.reset(); - grandparent_sub_handle_.reset(); rcl_event_.reset(); + std::visit([](auto & t) {t.destroy();}, grandparent_); } QoSEvent::QoSEvent( rclpy::Subscription & subscription, rcl_subscription_event_type_t event_type) -: event_type_(event_type) +: event_type_(event_type), grandparent_(subscription) { - 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(), grandparent_sub_handle_->rcl_ptr(), event_type); + rcl_event_.get(), + std::get(grandparent_).rcl_ptr(), event_type); if (RCL_RET_BAD_ALLOC == ret) { rcl_reset_error(); throw std::bad_alloc(); @@ -87,15 +86,14 @@ QoSEvent::QoSEvent( QoSEvent::QoSEvent( rclpy::Publisher & publisher, rcl_publisher_event_type_t event_type) -: event_type_(event_type) +: event_type_(event_type), grandparent_(publisher) { - 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(), grandparent_pub_handle_->rcl_ptr(), event_type); + rcl_event_.get(), + std::get(grandparent_).rcl_ptr(), event_type); if (RCL_RET_BAD_ALLOC == ret) { rcl_reset_error(); throw std::bad_alloc(); diff --git a/rclpy/src/rclpy/qos_event.hpp b/rclpy/src/rclpy/qos_event.hpp index 1c218a220..7d7bbcd96 100644 --- a/rclpy/src/rclpy/qos_event.hpp +++ b/rclpy/src/rclpy/qos_event.hpp @@ -91,10 +91,9 @@ class QoSEvent : public Destroyable, public std::enable_shared_from_this grandparent_pub_handle_; - std::shared_ptr grandparent_sub_handle_; - std::shared_ptr rcl_event_; std::variant event_type_; + std::variant grandparent_; + std::shared_ptr rcl_event_; }; /// Define a pybind11 wrapper for an rclpy::QoSEvent diff --git a/rclpy/src/rclpy/service.cpp b/rclpy/src/rclpy/service.cpp index c8cbf6efb..2f4acf093 100644 --- a/rclpy/src/rclpy/service.cpp +++ b/rclpy/src/rclpy/service.cpp @@ -35,16 +35,14 @@ void Service::destroy() { rcl_service_.reset(); - node_handle_.reset(); + node_.destroy(); } Service::Service( - py::capsule pynode, py::object pysrv_type, std::string service_name, + Node & node, py::object pysrv_type, std::string service_name, py::object pyqos_profile) -: node_handle_(std::make_shared(pynode)) +: node_(node) { - auto node = node_handle_->cast("rcl_node_t"); - auto srv_type = static_cast( rclpy_common_get_type_support(pysrv_type.ptr())); if (!srv_type) { @@ -60,11 +58,10 @@ Service::Service( // Create a client rcl_service_ = std::shared_ptr( new rcl_service_t, - [this](rcl_service_t * service) + [node](rcl_service_t * service) { - auto node = node_handle_->cast_or_warn("rcl_node_t"); - - rcl_ret_t ret = rcl_service_fini(service, node); + // Intentionally capture node by copy so shared_ptr can be transfered to copies + rcl_ret_t ret = rcl_service_fini(service, node.rcl_ptr()); if (RCL_RET_OK != ret) { // Warning should use line number of the current stack frame int stack_level = 1; @@ -79,7 +76,7 @@ Service::Service( *rcl_service_ = rcl_get_zero_initialized_service(); rcl_ret_t ret = rcl_service_init( - rcl_service_.get(), node, srv_type, + rcl_service_.get(), node_.rcl_ptr(), srv_type, service_name.c_str(), &service_ops); if (RCL_RET_OK != ret) { if (ret == RCL_RET_SERVICE_NAME_INVALID) { @@ -120,8 +117,7 @@ Service::service_take_request(py::object pyrequest_type) rmw_service_info_t header; py::tuple result_tuple(2); - rcl_ret_t ret = rcl_take_request_with_info( - rcl_service_.get(), &header, taken_request.get()); + rcl_ret_t ret = rcl_take_request_with_info(rcl_service_.get(), &header, taken_request.get()); if (ret == RCL_RET_SERVICE_TAKE_FAILED) { result_tuple[0] = py::none(); result_tuple[1] = py::none(); @@ -140,7 +136,7 @@ void define_service(py::object module) { py::class_>(module, "Service") - .def(py::init()) + .def(py::init()) .def_property_readonly( "pointer", [](const Service & service) { return reinterpret_cast(service.rcl_ptr()); diff --git a/rclpy/src/rclpy/service.hpp b/rclpy/src/rclpy/service.hpp index cae0afba0..6d8b0f298 100644 --- a/rclpy/src/rclpy/service.hpp +++ b/rclpy/src/rclpy/service.hpp @@ -25,6 +25,7 @@ #include "destroyable.hpp" #include "handle.hpp" +#include "node.hpp" #include "rclpy_common/exceptions.hpp" #include "utils.hpp" @@ -45,14 +46,14 @@ class Service : public Destroyable, public std::enable_shared_from_this * Raises ValueError if the capsules are not the correct types * Raises RCLError if the service could not be created * - * \param[in] pynode Capsule pointing to the node to add the service to + * \param[in] node node to add the service to * \param[in] pysrv_type Service module associated with the service * \param[in] service_name Python object for the service name * \param[in] pyqos_profile QoSProfile Python object for this service * \return capsule containing the rcl_service_t */ Service( - py::capsule pynode, py::object pysrv_type, std::string service_name, + Node & node, py::object pysrv_type, std::string service_name, py::object pyqos_profile); ~Service() = default; @@ -93,8 +94,7 @@ class Service : public Destroyable, public std::enable_shared_from_this destroy() override; private: - // TODO(ahcorde) replace with std::shared_ptr when rclpy::Node exists - std::shared_ptr node_handle_; + Node node_; std::shared_ptr rcl_service_; }; diff --git a/rclpy/src/rclpy/subscription.cpp b/rclpy/src/rclpy/subscription.cpp index 41dfa2d52..2b2f11cff 100644 --- a/rclpy/src/rclpy/subscription.cpp +++ b/rclpy/src/rclpy/subscription.cpp @@ -23,7 +23,6 @@ #include #include "rclpy_common/common.h" -#include "rclpy_common/handle.h" #include "rclpy_common/exceptions.hpp" @@ -36,12 +35,10 @@ using pybind11::literals::operator""_a; namespace rclpy { Subscription::Subscription( - py::capsule pynode, py::object pymsg_type, std::string topic, + Node & node, py::object pymsg_type, std::string topic, py::object pyqos_profile) -: node_handle_(std::make_shared(pynode)) +: node_(node) { - auto node = node_handle_->cast("rcl_node_t"); - auto msg_type = static_cast( rclpy_common_get_type_support(pymsg_type.ptr())); if (!msg_type) { @@ -56,11 +53,10 @@ Subscription::Subscription( rcl_subscription_ = std::shared_ptr( new rcl_subscription_t, - [this](rcl_subscription_t * subscription) + [node](rcl_subscription_t * subscription) { - auto node = node_handle_->cast_or_warn("rcl_node_t"); - - rcl_ret_t ret = rcl_subscription_fini(subscription, node); + // Intentionally capture node by copy so shared_ptr can be transfered to copies + rcl_ret_t ret = rcl_subscription_fini(subscription, node.rcl_ptr()); if (RCL_RET_OK != ret) { // Warning should use line number of the current stack frame int stack_level = 1; @@ -75,7 +71,7 @@ Subscription::Subscription( *rcl_subscription_ = rcl_get_zero_initialized_subscription(); rcl_ret_t ret = rcl_subscription_init( - rcl_subscription_.get(), node, msg_type, + rcl_subscription_.get(), node_.rcl_ptr(), msg_type, topic.c_str(), &subscription_ops); if (ret != RCL_RET_OK) { if (ret == RCL_RET_TOPIC_NAME_INVALID) { @@ -91,7 +87,7 @@ Subscription::Subscription( void Subscription::destroy() { rcl_subscription_.reset(); - node_handle_.reset(); + node_.destroy(); } py::object @@ -144,9 +140,7 @@ Subscription::take_message(py::object pymsg_type, bool raw) const char * Subscription::get_logger_name() { - auto node = node_handle_->cast("rcl_node_t"); - - const char * node_logger_name = rcl_node_get_logger_name(node); + const char * node_logger_name = rcl_node_get_logger_name(node_.rcl_ptr()); if (!node_logger_name) { throw RCLError("Node logger name not set"); } @@ -168,7 +162,7 @@ void define_subscription(py::object module) { py::class_>(module, "Subscription") - .def(py::init()) + .def(py::init()) .def_property_readonly( "pointer", [](const Subscription & subscription) { return reinterpret_cast(subscription.rcl_ptr()); diff --git a/rclpy/src/rclpy/subscription.hpp b/rclpy/src/rclpy/subscription.hpp index bafabaf4e..21640a16e 100644 --- a/rclpy/src/rclpy/subscription.hpp +++ b/rclpy/src/rclpy/subscription.hpp @@ -24,7 +24,7 @@ #include #include "destroyable.hpp" -#include "handle.hpp" +#include "node.hpp" namespace py = pybind11; @@ -43,13 +43,13 @@ class Subscription : public Destroyable, public std::enable_shared_from_this node_handle_; + Node node_; std::shared_ptr rcl_subscription_; }; /// Define a pybind11 wrapper for an rclpy::Service diff --git a/rclpy/src/rclpy/timer.cpp b/rclpy/src/rclpy/timer.cpp index 997c3fb06..a424dd013 100644 --- a/rclpy/src/rclpy/timer.cpp +++ b/rclpy/src/rclpy/timer.cpp @@ -37,20 +37,14 @@ void Timer::destroy() { rcl_timer_.reset(); - clock_handle_.reset(); + clock_.destroy(); + context_.destroy(); } Timer::Timer( - Clock & rclcy_clock, py::capsule pycontext, int64_t period_nsec) + Clock & clock, Context & context, int64_t period_nsec) +: context_(context), clock_(clock) { - clock_handle_ = rclcy_clock.shared_from_this(); - - auto context = static_cast( - rclpy_handle_get_pointer_from_capsule(pycontext.ptr(), "rcl_context_t")); - if (!context) { - throw py::error_already_set(); - } - // Create a client rcl_timer_ = std::shared_ptr( new rcl_timer_t, @@ -72,7 +66,7 @@ Timer::Timer( rcl_allocator_t allocator = rcl_get_default_allocator(); rcl_ret_t ret = rcl_timer_init( - rcl_timer_.get(), clock_handle_->rcl_ptr(), context, + rcl_timer_.get(), clock_.rcl_ptr(), context.rcl_ptr(), period_nsec, NULL, allocator); if (RCL_RET_OK != ret) { @@ -169,7 +163,7 @@ void define_timer(py::object module) { py::class_>(module, "Timer") - .def(py::init()) + .def(py::init()) .def_property_readonly( "pointer", [](const Timer & timer) { return reinterpret_cast(timer.rcl_ptr()); diff --git a/rclpy/src/rclpy/timer.hpp b/rclpy/src/rclpy/timer.hpp index 00b2559e6..55c37b292 100644 --- a/rclpy/src/rclpy/timer.hpp +++ b/rclpy/src/rclpy/timer.hpp @@ -22,6 +22,7 @@ #include #include "clock.hpp" +#include "context.hpp" #include "destroyable.hpp" #include "handle.hpp" @@ -46,7 +47,7 @@ class Timer : public Destroyable, public std::enable_shared_from_this * \param[in] period_nsec the period of the timer in nanoseconds * \return a timer capsule */ - Timer(Clock & clock, py::capsule pycontext, int64_t period_nsec); + Timer(Clock & clock, Context & context, int64_t period_nsec); ~Timer() = default; @@ -133,7 +134,8 @@ class Timer : public Destroyable, public std::enable_shared_from_this void destroy() override; private: - std::shared_ptr clock_handle_; + Context context_; + Clock clock_; std::shared_ptr rcl_timer_; }; diff --git a/rclpy/src/rclpy/utils.cpp b/rclpy/src/rclpy/utils.cpp index 39adef07a..6f2cf2c0d 100644 --- a/rclpy/src/rclpy/utils.cpp +++ b/rclpy/src/rclpy/utils.cpp @@ -17,17 +17,18 @@ #include #include +#include #include #include #include +#include #include #include "rclpy_common/common.h" #include "rclpy_common/handle.h" #include "rclpy_common/exceptions.hpp" -#include "init.hpp" #include "utils.hpp" namespace rclpy @@ -225,4 +226,54 @@ remove_ros_args(py::object pycli_args) return result_args; } + +void +throw_if_unparsed_ros_args(py::list pyargs, const rcl_arguments_t & rcl_args) +{ + int unparsed_ros_args_count = rcl_arguments_get_count_unparsed_ros(&rcl_args); + + if (unparsed_ros_args_count < 0) { + throw std::runtime_error("failed to count unparsed arguments"); + } + if (0 == unparsed_ros_args_count) { + return; + } + + rcl_allocator_t allocator = rcl_get_default_allocator(); + + int * unparsed_indices_c = nullptr; + rcl_ret_t ret = rcl_arguments_get_unparsed_ros(&rcl_args, allocator, &unparsed_indices_c); + if (RCL_RET_OK != ret) { + throw RCLError("failed to get unparsed arguments"); + } + + auto deallocator = [&](int ptr[]) {allocator.deallocate(ptr, allocator.state);}; + auto unparsed_indices = std::unique_ptr( + unparsed_indices_c, deallocator); + + py::list unparsed_args; + for (int i = 0; i < unparsed_ros_args_count; ++i) { + int index = unparsed_indices_c[i]; + if (index < 0 || static_cast(index) >= pyargs.size()) { + throw std::runtime_error("got invalid unparsed ROS arg index"); + } + unparsed_args.append(pyargs[index]); + } + + throw UnknownROSArgsError(static_cast(py::repr(unparsed_args))); +} + +py::dict +rclpy_action_get_rmw_qos_profile(const char * rmw_profile) +{ + PyObject * pyqos_profile = NULL; + if (0 == strcmp(rmw_profile, "rcl_action_qos_profile_status_default")) { + pyqos_profile = rclpy_common_convert_to_qos_dict(&rcl_action_qos_profile_status_default); + } else { + std::string error_text = "Requested unknown rmw_qos_profile: "; + error_text += rmw_profile; + throw std::runtime_error(error_text); + } + return py::reinterpret_steal(pyqos_profile); +} } // namespace rclpy diff --git a/rclpy/src/rclpy/utils.hpp b/rclpy/src/rclpy/utils.hpp index 1520005a1..0f4ae104e 100644 --- a/rclpy/src/rclpy/utils.hpp +++ b/rclpy/src/rclpy/utils.hpp @@ -21,6 +21,7 @@ #include +#include "rclpy_common/common.h" #include "publisher.hpp" namespace py = pybind11; @@ -105,6 +106,21 @@ assert_liveliness(rclpy::Publisher * publisher); py::list remove_ros_args(py::object pycli_args); +/// Throw UnparsedROSArgsError with a message saying which args are unparsed. +void +throw_if_unparsed_ros_args(py::list pyargs, const rcl_arguments_t & rcl_args); + +/// Fetch a predefined qos_profile from rcl_action and convert it to a Python QoSProfile object. +/** + * Raises RuntimeError if the QoS profile is unknown. + * + * This function takes a string defining a rmw_qos_profile_t and returns the + * corresponding Python QoSProfile object. + * \param[in] rmw_profile String with the name of the profile to load. + * \return QoSProfile object. + */ +py::dict +rclpy_action_get_rmw_qos_profile(const char * rmw_profile); } // namespace rclpy #endif // RCLPY__UTILS_HPP_ diff --git a/rclpy/src/rclpy/wait_set.cpp b/rclpy/src/rclpy/wait_set.cpp index e901b8ec4..2e837be15 100644 --- a/rclpy/src/rclpy/wait_set.cpp +++ b/rclpy/src/rclpy/wait_set.cpp @@ -32,27 +32,6 @@ namespace rclpy { -WaitSet::WaitSet() -{ - // 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(); -} - WaitSet::WaitSet( size_t number_of_subscriptions, size_t number_of_guard_conditions, @@ -60,7 +39,8 @@ WaitSet::WaitSet( size_t number_of_clients, size_t number_of_services, size_t number_of_events, - py::capsule pycontext) + Context & context) +: context_(context) { // Create a client rcl_wait_set_ = std::shared_ptr( @@ -80,12 +60,6 @@ WaitSet::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")); - if (!context) { - throw py::error_already_set(); - } - rcl_ret_t ret = rcl_wait_set_init( rcl_wait_set_.get(), number_of_subscriptions, @@ -94,7 +68,7 @@ WaitSet::WaitSet( number_of_clients, number_of_services, number_of_events, - context, + context.rcl_ptr(), rcl_get_default_allocator()); if (RCL_RET_OK != ret) { throw RCLError("failed to initialize wait set"); @@ -105,6 +79,7 @@ void WaitSet::destroy() { rcl_wait_set_.reset(); + context_.destroy(); } void @@ -116,34 +91,6 @@ WaitSet::clear_entities() } } -size_t -WaitSet::add_entity(const std::string & entity_type, py::capsule pyentity) -{ - rcl_ret_t ret = RCL_RET_ERROR; - size_t index; - - 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) { - throw py::error_already_set(); - } - ret = rcl_wait_set_add_guard_condition(rcl_wait_set_.get(), guard_condition, &index); - } else { - std::string error_text{"'"}; - error_text += entity_type; - error_text += "' is not a known entity"; - throw std::runtime_error(error_text); - } - if (ret != RCL_RET_OK) { - std::string error_text{"failed to add'"}; - error_text += entity_type; - error_text += "' to waitset"; - throw RCLError(error_text); - } - return index; -} - size_t WaitSet::add_service(const Service & service) { @@ -312,8 +259,7 @@ WaitSet::wait(int64_t timeout) void define_waitset(py::object module) { py::class_>(module, "WaitSet") - .def(py::init()) - .def(py::init<>()) + .def(py::init()) .def_property_readonly( "pointer", [](const WaitSet & waitset) { return reinterpret_cast(waitset.rcl_ptr()); @@ -322,9 +268,6 @@ void define_waitset(py::object module) .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") diff --git a/rclpy/src/rclpy/wait_set.hpp b/rclpy/src/rclpy/wait_set.hpp index 2ba9efd8e..7d74cb1fc 100644 --- a/rclpy/src/rclpy/wait_set.hpp +++ b/rclpy/src/rclpy/wait_set.hpp @@ -21,6 +21,7 @@ #include #include "client.hpp" +#include "context.hpp" #include "destroyable.hpp" #include "guard_condition.hpp" #include "qos_event.hpp" @@ -57,7 +58,7 @@ class WaitSet : public Destroyable, public std::enable_shared_from_this size_t number_of_clients, size_t number_of_services, size_t number_of_events, - py::capsule pycontext); + Context & context); /// Clear all the pointers in the wait set /** @@ -66,18 +67,6 @@ class WaitSet : public Destroyable, public std::enable_shared_from_this 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 @@ -182,6 +171,7 @@ class WaitSet : public Destroyable, public std::enable_shared_from_this void destroy() override; private: + Context context_; std::shared_ptr rcl_wait_set_; }; diff --git a/rclpy/src/rclpy_common/include/rclpy_common/common.h b/rclpy/src/rclpy_common/include/rclpy_common/common.h index 606f5ad0e..db18e9586 100644 --- a/rclpy/src/rclpy_common/include/rclpy_common/common.h +++ b/rclpy/src/rclpy_common/include/rclpy_common/common.h @@ -32,16 +32,6 @@ typedef void destroy_ros_message_signature (void *); typedef bool convert_from_py_signature (PyObject *, void *); typedef PyObject * convert_to_py_signature (void *); -/// Finalize names and types struct with error setting. -/** - * \param[in] names_and_types The struct to finalize. - * \return `true` if finalized successfully, `false` otherwise. - * If `false`, then a Python error is set. - */ -RCLPY_COMMON_PUBLIC -bool -rclpy_names_and_types_fini(rcl_names_and_types_t * names_and_types); - /// Get the type support structure for a Python ROS message type. /** * \param[in] pymsg_type The Python ROS message type. @@ -51,17 +41,6 @@ RCLPY_COMMON_PUBLIC void * rclpy_common_get_type_support(PyObject * pymsg_type); -/// Convert a C rcl_names_and_types_t into a Python list. -/** - * \param names_and_types The names and types struct to convert. - * \return A PyList of PyTuples. The first element of each tuple is a string for the - * name and the second element is a list of strings for the types. - * \return `NULL` if there is an error. No Python error is set. - */ -RCLPY_COMMON_PUBLIC -PyObject * -rclpy_convert_to_py_names_and_types(rcl_names_and_types_t * topic_names_and_types); - /// Convert a C rmw_qos_profile_t into a Python dictionary with qos profile args. /** * \param[in] profile Pointer to a rmw_qos_profile_t to convert diff --git a/rclpy/src/rclpy_common/src/common.c b/rclpy/src/rclpy_common/src/common.c index e0788fdd8..e96319360 100644 --- a/rclpy/src/rclpy_common/src/common.c +++ b/rclpy/src/rclpy_common/src/common.c @@ -55,23 +55,6 @@ cleanup_rclpy_qos_profile(rclpy_qos_profile_t * profile) Py_XDECREF(profile->avoid_ros_namespace_conventions); } -bool -rclpy_names_and_types_fini(rcl_names_and_types_t * names_and_types) -{ - if (!names_and_types) { - return true; - } - rcl_ret_t ret = rcl_names_and_types_fini(names_and_types); - if (ret != RCL_RET_OK) { - PyErr_Format( - PyExc_RuntimeError, - "Failed to destroy rcl_names_and_types_t: %s", rcl_get_error_string().str); - rcl_reset_error(); - return false; - } - return true; -} - void * rclpy_common_get_type_support(PyObject * pymsg_type) { @@ -91,55 +74,6 @@ rclpy_common_get_type_support(PyObject * pymsg_type) return ts; } -PyObject * -rclpy_convert_to_py_names_and_types(rcl_names_and_types_t * names_and_types) -{ - if (!names_and_types) { - return NULL; - } - - PyObject * pynames_and_types = PyList_New(names_and_types->names.size); - if (!pynames_and_types) { - return NULL; - } - - size_t i; - for (i = 0; i < names_and_types->names.size; ++i) { - PyObject * pytuple = PyTuple_New(2); - if (!pytuple) { - Py_DECREF(pynames_and_types); - return NULL; - } - PyObject * pyname = PyUnicode_FromString(names_and_types->names.data[i]); - if (!pyname) { - Py_DECREF(pynames_and_types); - Py_DECREF(pytuple); - return NULL; - } - PyTuple_SET_ITEM(pytuple, 0, pyname); - PyObject * pytypes_list = PyList_New(names_and_types->types[i].size); - if (!pytypes_list) { - Py_DECREF(pynames_and_types); - Py_DECREF(pytuple); - return NULL; - } - size_t j; - for (j = 0; j < names_and_types->types[i].size; ++j) { - PyObject * pytype = PyUnicode_FromString(names_and_types->types[i].data[j]); - if (!pytype) { - Py_DECREF(pynames_and_types); - Py_DECREF(pytuple); - Py_DECREF(pytypes_list); - return NULL; - } - PyList_SET_ITEM(pytypes_list, j, pytype); - } - PyTuple_SET_ITEM(pytuple, 1, pytypes_list); - PyList_SET_ITEM(pynames_and_types, i, pytuple); - } - return pynames_and_types; -} - static PyObject * _convert_rmw_time_to_py_duration(const rmw_time_t * duration) diff --git a/rclpy/test/test_destruction.py b/rclpy/test/test_destruction.py index 14b93925f..e16d11fe7 100644 --- a/rclpy/test/test_destruction.py +++ b/rclpy/test/test_destruction.py @@ -38,8 +38,7 @@ def test_destroy_node_twice(): try: node = rclpy.create_node('test_node2', context=context) node.destroy_node() - with pytest.raises(InvalidHandle): - node.destroy_node() + node.destroy_node() finally: rclpy.shutdown(context=context) @@ -162,11 +161,8 @@ def test_destroy_node_asap(): try: node = rclpy.create_node('test_destroy_subscription_asap', context=context) - with node.handle: - node.destroy_node() - # handle valid because it's still being used - with node.handle: - pass + + node.destroy_node() with pytest.raises(InvalidHandle): # handle invalid because it was destroyed when no one was using it diff --git a/rclpy/test/test_handle.py b/rclpy/test/test_handle.py index 6c9cab2e8..99b016280 100644 --- a/rclpy/test/test_handle.py +++ b/rclpy/test/test_handle.py @@ -23,7 +23,7 @@ def test_handle_destroyed_immediately(): try: node = rclpy.create_node('test_handle_destroyed_immediately', context=context) - node.handle.destroy() + node.destroy_node() with pytest.raises(InvalidHandle): with node.handle: pass @@ -38,9 +38,7 @@ def test_handle_destroyed_when_not_used(): try: node = rclpy.create_node('test_handle_destroyed_when_not_used', context=context) with node.handle: - node.handle.destroy() - with node.handle: - pass + node.destroy_node() with pytest.raises(InvalidHandle): with node.handle: diff --git a/rclpy/test/test_qos_event.py b/rclpy/test/test_qos_event.py index 34a810468..a94b95865 100644 --- a/rclpy/test/test_qos_event.py +++ b/rclpy/test/test_qos_event.py @@ -12,6 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +import gc import unittest from unittest.mock import Mock @@ -50,6 +51,11 @@ def setUp(self): self.is_fastrtps = 'rmw_fastrtps' in get_rmw_implementation_identifier() def tearDown(self): + # These tests create a bunch of events by hand instead of using Node APIs, + # so they won't be cleaned up when calling `node.destroy_node()`, but they could still + # keep the node alive from one test to the next. + # Invoke the garbage collector to destroy them. + gc.collect() self.node.destroy_node() rclpy.shutdown(context=self.context) @@ -237,8 +243,8 @@ 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) - with self.context.handle as context_handle: - wait_set = _rclpy.WaitSet(0, 0, 0, 0, 0, 3, context_handle) + with self.context.handle: + wait_set = _rclpy.WaitSet(0, 0, 0, 0, 0, 3, self.context.handle) deadline_event_handle = self._create_event_handle( publisher, QoSPublisherEventType.RCL_PUBLISHER_OFFERED_DEADLINE_MISSED) @@ -309,8 +315,8 @@ 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) - with self.context.handle as context_handle: - wait_set = _rclpy.WaitSet(0, 0, 0, 0, 0, 3, context_handle) + with self.context.handle: + wait_set = _rclpy.WaitSet(0, 0, 0, 0, 0, 3, self.context.handle) deadline_event_handle = self._create_event_handle( subscription, QoSSubscriptionEventType.RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED) diff --git a/rclpy/test/test_subscription.py b/rclpy/test/test_subscription.py index e12b4e7a8..b3f994b71 100644 --- a/rclpy/test/test_subscription.py +++ b/rclpy/test/test_subscription.py @@ -16,6 +16,7 @@ import rclpy from rclpy.node import Node + from test_msgs.msg import Empty @@ -48,7 +49,13 @@ def make_mock_subscription(namespace, topic_name, cli_args=None): ('/example/topic', 'ns', '/example/topic'), ]) def test_get_subscription_topic_name(topic_name, namespace, expected): - sub = make_mock_subscription(namespace, topic_name) + node = Node('node_name', namespace=namespace, cli_args=None) + sub = node.create_subscription( + msg_type=Empty, + topic=topic_name, + callback=lambda _: None, + qos_profile=10, + ) assert sub.topic_name == expected @@ -61,5 +68,11 @@ def test_get_subscription_topic_name(topic_name, namespace, expected): '/ns/new_topic'), ]) def test_get_subscription_topic_name_after_remapping(topic_name, namespace, cli_args, expected): - sub = make_mock_subscription(namespace, topic_name, cli_args) + node = Node('node_name', namespace=namespace, cli_args=cli_args) + sub = node.create_subscription( + msg_type=Empty, + topic=topic_name, + callback=lambda _: None, + qos_profile=10, + ) assert sub.topic_name == expected diff --git a/rclpy/test/test_waitable.py b/rclpy/test/test_waitable.py index c7f422de0..f79841c51 100644 --- a/rclpy/test/test_waitable.py +++ b/rclpy/test/test_waitable.py @@ -41,9 +41,9 @@ class ClientWaitable(Waitable): def __init__(self, node): super().__init__(ReentrantCallbackGroup()) - with node.handle as node_capsule: + with node.handle: self.client = _rclpy.Client( - node_capsule, EmptySrv, 'test_client', QoSProfile(depth=10).get_c_qos_profile()) + node.handle, EmptySrv, 'test_client', QoSProfile(depth=10).get_c_qos_profile()) self.client_index = None self.client_is_ready = False @@ -84,9 +84,9 @@ class ServerWaitable(Waitable): def __init__(self, node): super().__init__(ReentrantCallbackGroup()) - with node.handle as node_capsule: + with node.handle: self.server = _rclpy.Service( - node_capsule, EmptySrv, 'test_server', QoSProfile(depth=10).get_c_qos_profile()) + node.handle, EmptySrv, 'test_server', QoSProfile(depth=10).get_c_qos_profile()) self.server_index = None self.server_is_ready = False @@ -129,9 +129,9 @@ def __init__(self, node): self._clock = Clock(clock_type=ClockType.STEADY_TIME) period_nanoseconds = 10000 - with self._clock.handle, node.context.handle as context_capsule: + with self._clock.handle, node.context.handle: self.timer = _rclpy.Timer( - self._clock.handle, context_capsule, period_nanoseconds) + self._clock.handle, node.context.handle, period_nanoseconds) self.timer_index = None self.timer_is_ready = False @@ -173,9 +173,9 @@ class SubscriptionWaitable(Waitable): def __init__(self, node): super().__init__(ReentrantCallbackGroup()) - with node.handle as node_capsule: + with node.handle: self.subscription = _rclpy.Subscription( - node_capsule, EmptyMsg, 'test_topic', QoSProfile(depth=10).get_c_qos_profile()) + node.handle, EmptyMsg, 'test_topic', QoSProfile(depth=10).get_c_qos_profile()) self.subscription_index = None self.subscription_is_ready = False @@ -219,8 +219,8 @@ class GuardConditionWaitable(Waitable): def __init__(self, node): super().__init__(ReentrantCallbackGroup()) - with node.context.handle as context_capsule: - self.guard_condition = _rclpy.GuardCondition(context_capsule) + with node.context.handle: + self.guard_condition = _rclpy.GuardCondition(node.context.handle) self.guard_condition_index = None self.guard_is_ready = False