From c69de7d541a2c08caf8eb9900462e309e863a81d Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Thu, 25 Apr 2019 17:11:19 -0700 Subject: [PATCH] Refactor to new subscription handle type Signed-off-by: Emerson Knapp --- rclpy/rclpy/qos_event.py | 28 +++++++++++++----------- rclpy/rclpy/subscription.py | 3 ++- rclpy/src/rclpy/_rclpy_qos_event.c | 4 ++-- rclpy/test/test_qos_event.py | 35 +++++++++++++++++------------- 4 files changed, 39 insertions(+), 31 deletions(-) diff --git a/rclpy/rclpy/qos_event.py b/rclpy/rclpy/qos_event.py index 0fce11698..b100cdce2 100644 --- a/rclpy/rclpy/qos_event.py +++ b/rclpy/rclpy/qos_event.py @@ -18,6 +18,7 @@ from typing import NamedTuple from rclpy.callback_groups import CallbackGroup +from rclpy.handle import Handle from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.waitable import NumberOfEntities from rclpy.waitable import Waitable @@ -167,21 +168,22 @@ def __init__( self.liveliness = liveliness def create_event_handlers( - self, callback_group: CallbackGroup, subscription_handle + self, callback_group: CallbackGroup, subscription_handle: Handle ) -> List[QoSEventHandler]: event_handlers = [] - if self.deadline: - event_handlers.append(QoSEventHandler( - callback_group=callback_group, - callback=self.deadline, - event_type=QoSSubscriptionEventType.RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED, - parent_handle=subscription_handle)) - if self.liveliness: - event_handlers.append(QoSEventHandler( - callback_group=callback_group, - callback=self.liveliness, - event_type=QoSSubscriptionEventType.RCL_SUBSCRIPTION_LIVELINESS_CHANGED, - parent_handle=subscription_handle)) + with subscription_handle as subscription_capsule: + if self.deadline: + event_handlers.append(QoSEventHandler( + callback_group=callback_group, + callback=self.deadline, + event_type=QoSSubscriptionEventType.RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED, + parent_handle=subscription_capsule)) + if self.liveliness: + event_handlers.append(QoSEventHandler( + callback_group=callback_group, + callback=self.liveliness, + event_type=QoSSubscriptionEventType.RCL_SUBSCRIPTION_LIVELINESS_CHANGED, + parent_handle=subscription_capsule)) return event_handlers diff --git a/rclpy/rclpy/subscription.py b/rclpy/rclpy/subscription.py index 64432a451..730870497 100644 --- a/rclpy/rclpy/subscription.py +++ b/rclpy/rclpy/subscription.py @@ -16,6 +16,7 @@ from typing import TypeVar from rclpy.callback_groups import CallbackGroup +from rclpy.handle import Handle from rclpy.qos import QoSProfile from rclpy.qos_event import SubscriptionEventCallbacks @@ -28,7 +29,7 @@ class Subscription: def __init__( self, - subscription_handle, + subscription_handle: Handle, msg_type: MsgType, topic: str, callback: Callable, diff --git a/rclpy/src/rclpy/_rclpy_qos_event.c b/rclpy/src/rclpy/_rclpy_qos_event.c index 4eac52666..697387ac0 100644 --- a/rclpy/src/rclpy/_rclpy_qos_event.c +++ b/rclpy/src/rclpy/_rclpy_qos_event.c @@ -49,7 +49,7 @@ _destroy_event_capsule(PyObject * pycapsule) bool _is_pycapsule_rcl_subscription(PyObject * pycapsule) { - return PyCapsule_IsValid(pycapsule, "rcl_subscription_t"); + return PyCapsule_IsValid(pycapsule, "rclpy_subscription_t"); } bool @@ -67,7 +67,7 @@ _is_pycapsule_rcl_event(PyObject * pycapsule) rcl_subscription_t * _pycapsule_to_rcl_subscription(PyObject * pycapsule) { - return (rcl_subscription_t *)PyCapsule_GetPointer(pycapsule, "rcl_subscription_t"); + return (rcl_subscription_t *)PyCapsule_GetPointer(pycapsule, "rclpy_subscription_t"); } rcl_publisher_t * diff --git a/rclpy/test/test_qos_event.py b/rclpy/test/test_qos_event.py index b9e3f776c..6591512a7 100644 --- a/rclpy/test/test_qos_event.py +++ b/rclpy/test/test_qos_event.py @@ -100,7 +100,7 @@ def test_subscription_constructor(self): self.assertEqual(len(subscription.event_handlers), 2) self.node.destroy_subscription(subscription) - def test_event_create_destroy(self): + def test_publisher_event_create_destroy(self): # Publisher event types publisher = self.node.create_publisher(EmptyMsg, 'test_topic') @@ -118,21 +118,24 @@ def test_event_create_destroy(self): self.node.destroy_publisher(publisher) + def test_subscription_event_create_destroy(self): # Subscription event types message_callback = Mock() subscription = self.node.create_subscription(EmptyMsg, 'test_topic', message_callback) - event_handle = _rclpy.rclpy_create_event( - QoSSubscriptionEventType.RCL_SUBSCRIPTION_LIVELINESS_CHANGED, - subscription.subscription_handle) - self.assertIsNotNone(event_handle) - _rclpy.rclpy_destroy_entity(event_handle) + with subscription.handle as subscription_capsule: + # self.assertFalse(subscription_capsule) + event_handle = _rclpy.rclpy_create_event( + QoSSubscriptionEventType.RCL_SUBSCRIPTION_LIVELINESS_CHANGED, + subscription_capsule) + self.assertIsNotNone(event_handle) + _rclpy.rclpy_destroy_entity(event_handle) - event_handle = _rclpy.rclpy_create_event( - QoSSubscriptionEventType.RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED, - subscription.subscription_handle) - self.assertIsNotNone(event_handle) - _rclpy.rclpy_destroy_entity(event_handle) + event_handle = _rclpy.rclpy_create_event( + QoSSubscriptionEventType.RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED, + subscription_capsule) + self.assertIsNotNone(event_handle) + _rclpy.rclpy_destroy_entity(event_handle) self.node.destroy_subscription(subscription) @@ -190,16 +193,17 @@ def test_call_subscription_rclpy_event_apis(self): wait_set = _rclpy.rclpy_get_zero_initialized_wait_set() _rclpy.rclpy_wait_set_init(wait_set, 0, 0, 0, 0, 0, 2, self.context.handle) + subscription_capsule = subscription.handle._get_capsule() deadline_event_handle = _rclpy.rclpy_create_event( QoSSubscriptionEventType.RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED, - subscription.subscription_handle) + subscription_capsule) deadline_event_index = _rclpy.rclpy_wait_set_add_entity( 'event', wait_set, deadline_event_handle) self.assertIsNotNone(deadline_event_index) liveliness_event_handle = _rclpy.rclpy_create_event( QoSSubscriptionEventType.RCL_SUBSCRIPTION_LIVELINESS_CHANGED, - subscription.subscription_handle) + subscription_capsule) liveliness_event_index = _rclpy.rclpy_wait_set_add_entity( 'event', wait_set, liveliness_event_handle) self.assertIsNotNone(liveliness_event_index) @@ -214,7 +218,7 @@ def test_call_subscription_rclpy_event_apis(self): # Tests data conversion utilities in C side event_data = _rclpy.rclpy_take_event( deadline_event_handle, - subscription.subscription_handle, + subscription_capsule, QoSSubscriptionEventType.RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED) self.assertIsInstance(event_data, QoSRequestedDeadlineMissedInfo) self.assertEqual(event_data.total_count, 0) @@ -222,7 +226,7 @@ def test_call_subscription_rclpy_event_apis(self): event_data = _rclpy.rclpy_take_event( liveliness_event_handle, - subscription.subscription_handle, + subscription_capsule, QoSSubscriptionEventType.RCL_SUBSCRIPTION_LIVELINESS_CHANGED) self.assertIsInstance(event_data, QoSLivelinessChangedInfo) self.assertEqual(event_data.alive_count, 0) @@ -230,4 +234,5 @@ def test_call_subscription_rclpy_event_apis(self): self.assertEqual(event_data.not_alive_count, 0) self.assertEqual(event_data.not_alive_count_change, 0) + subscription.handle._return_capsule() self.node.destroy_subscription(subscription)