diff --git a/rclpy/CMakeLists.txt b/rclpy/CMakeLists.txt index 722703c76..6f07bf4a4 100644 --- a/rclpy/CMakeLists.txt +++ b/rclpy/CMakeLists.txt @@ -137,6 +137,17 @@ ament_target_dependencies(rclpy_logging "rcutils" ) +# Signal handling library +add_library( + rclpy_signal_handler + SHARED src/rclpy/_rclpy_signal_handler.c +) +configure_python_c_extension_library(rclpy_signal_handler) +ament_target_dependencies(rclpy_signal_handler + "rcl" + "rcutils" +) + if(NOT WIN32) ament_environment_hooks( "${ament_cmake_package_templates_ENVIRONMENT_HOOK_LIBRARY_PATH}" diff --git a/rclpy/rclpy/action/client.py b/rclpy/rclpy/action/client.py index 7c8b47902..6244ef89b 100644 --- a/rclpy/rclpy/action/client.py +++ b/rclpy/rclpy/action/client.py @@ -349,7 +349,13 @@ def send_goal(self, goal, **kwargs): :type goal: action_type.Goal :return: The result response. :rtype: action_type.Result + :raises: TypeError if the type of the passed goal isn't an instance of + the Goal type of the provided action when the service was + constructed. """ + if not isinstance(goal, self._action_type.Goal): + raise TypeError() + event = threading.Event() def unblock(future): @@ -386,7 +392,13 @@ def send_goal_async(self, goal, feedback_callback=None, goal_uuid=None): :return: a Future instance to a goal handle that completes when the goal request has been accepted or rejected. :rtype: :class:`rclpy.task.Future` instance + :raises: TypeError if the type of the passed goal isn't an instance of + the Goal type of the provided action when the service was + constructed. """ + if not isinstance(goal, self._action_type.Goal): + raise TypeError() + 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 diff --git a/rclpy/rclpy/action/server.py b/rclpy/rclpy/action/server.py index 4dabe78a0..453f8d1dd 100644 --- a/rclpy/rclpy/action/server.py +++ b/rclpy/rclpy/action/server.py @@ -134,6 +134,9 @@ def execute(self, execute_callback=None): self._action_server.notify_execute(self, execute_callback) def publish_feedback(self, feedback): + if not isinstance(feedback, self._action_server.action_type.Feedback): + raise TypeError() + with self._lock: # Ignore for already destructed goal handles if self._handle is None: diff --git a/rclpy/rclpy/client.py b/rclpy/rclpy/client.py index 056faff38..183999acc 100644 --- a/rclpy/rclpy/client.py +++ b/rclpy/rclpy/client.py @@ -79,7 +79,13 @@ def call(self, request: SrvTypeRequest) -> SrvTypeResponse: :param request: The service request. :return: The service response. + :raises: TypeError if the type of the passed request isn't an instance + of the Request type of the provided service when the client was + constructed. """ + if not isinstance(request, self.srv_type.Request): + raise TypeError() + event = threading.Event() def unblock(future): @@ -116,7 +122,13 @@ def call_async(self, request: SrvTypeRequest) -> Future: :param request: The service request. :return: A future that completes when the request does. + :raises: TypeError if the type of the passed request isn't an instance + of the Request type of the provided service when the client was + constructed. """ + if not isinstance(request, self.srv_type.Request): + raise TypeError() + sequence_number = _rclpy.rclpy_send_request(self.client_handle, request) if sequence_number in self._pending_requests: raise RuntimeError('Sequence (%r) conflicts with pending request' % sequence_number) diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py index 820398d04..c2eb491f1 100644 --- a/rclpy/rclpy/executors.py +++ b/rclpy/rclpy/executors.py @@ -37,6 +37,7 @@ from rclpy.guard_condition import GuardCondition from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.service import Service +from rclpy.signals import SignalHandlerGuardCondition from rclpy.subscription import Subscription from rclpy.task import Future from rclpy.task import Task @@ -46,13 +47,6 @@ from rclpy.waitable import NumberOfEntities from rclpy.waitable import Waitable -# TODO(wjwwood): make _rclpy_wait(...) thread-safe -# Executor.spin_once() ends up calling _rclpy_wait(...), which right now is -# not thread-safe, no matter if different wait sets are used or not. -# See, for example, https://github.com/ros2/rclpy/issues/192 -g_wait_set_spinning_lock = Lock() -g_wait_set_spinning = False - # For documentation purposes # TODO(jacobperron): Make all entities implement the 'Waitable' interface for better type checking WaitableEntityType = TypeVar('WaitableEntityType') @@ -167,6 +161,7 @@ def __init__(self, *, context: Context = None) -> None: self._cb_iter = None self._last_args = None self._last_kwargs = None + self._sigint_gc = SignalHandlerGuardCondition(context) @property def context(self) -> Context: @@ -214,6 +209,9 @@ def shutdown(self, timeout_sec: float = None) -> bool: if self._guard_condition: _rclpy.rclpy_destroy_entity(self._guard_condition) self._guard_condition = None + if self._sigint_gc: + self._sigint_gc.destroy() + self._sigint_gc = None self._cb_iter = None self._last_args = None self._last_kwargs = None @@ -222,6 +220,8 @@ def shutdown(self, timeout_sec: float = None) -> bool: def __del__(self): if self._guard_condition is not None: _rclpy.rclpy_destroy_entity(self._guard_condition) + if self._sigint_gc is not None: + self._sigint_gc.destroy() def add_node(self, node: 'Node') -> bool: """ @@ -480,6 +480,7 @@ def _wait_for_ready_callbacks( entity_count.num_timers, entity_count.num_clients, entity_count.num_services, + entity_count.num_events, self._context.handle) entities = { @@ -497,26 +498,23 @@ def _wait_for_ready_callbacks( ) for waitable in waitables: waitable.add_to_wait_set(wait_set) - (sigint_gc, sigint_gc_handle) = \ - _rclpy.rclpy_get_sigint_guard_condition(self._context.handle) - try: - _rclpy.rclpy_wait_set_add_entity('guard_condition', wait_set, sigint_gc) - _rclpy.rclpy_wait_set_add_entity( - 'guard_condition', wait_set, self._guard_condition) - - # Wait for something to become ready - _rclpy.rclpy_wait(wait_set, timeout_nsec) - if self._is_shutdown: - raise ShutdownException() - - # 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) - finally: - _rclpy.rclpy_destroy_entity(sigint_gc) + + sigint_gc = self._sigint_gc.guard_handle + _rclpy.rclpy_wait_set_add_entity('guard_condition', wait_set, sigint_gc) + _rclpy.rclpy_wait_set_add_entity( + 'guard_condition', wait_set, self._guard_condition) + + # Wait for something to become ready + _rclpy.rclpy_wait(wait_set, timeout_nsec) + if self._is_shutdown: + raise ShutdownException() + + # 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) # Mark all guards as triggered before yielding since they're auto-taken for gc in guards: @@ -595,33 +593,21 @@ def wait_for_ready_callbacks(self, *args, **kwargs) -> Tuple[Task, WaitableEntit .. Including the docstring for the hidden function for reference .. automethod:: _wait_for_ready_callbacks """ - global g_wait_set_spinning_lock - global g_wait_set_spinning - with g_wait_set_spinning_lock: - if g_wait_set_spinning: - raise RuntimeError( - 'Executor.wait_for_ready_callbacks() called concurrently in multiple threads') - g_wait_set_spinning = True - - try: - # if an old generator is done, this var makes the loop get a new one before returning - got_generator = False - while not got_generator: - if self._cb_iter is None or self._last_args != args or self._last_kwargs != kwargs: - # Create a new generator - self._last_args = args - self._last_kwargs = kwargs - self._cb_iter = self._wait_for_ready_callbacks(*args, **kwargs) - got_generator = True + # if an old generator is done, this var makes the loop get a new one before returning + got_generator = False + while not got_generator: + if self._cb_iter is None or self._last_args != args or self._last_kwargs != kwargs: + # Create a new generator + self._last_args = args + self._last_kwargs = kwargs + self._cb_iter = self._wait_for_ready_callbacks(*args, **kwargs) + got_generator = True - try: - return next(self._cb_iter) - except StopIteration: - # Generator ran out of work - self._cb_iter = None - finally: - with g_wait_set_spinning_lock: - g_wait_set_spinning = False + try: + return next(self._cb_iter) + except StopIteration: + # Generator ran out of work + self._cb_iter = None class SingleThreadedExecutor(Executor): diff --git a/rclpy/rclpy/impl/implementation_singleton.py b/rclpy/rclpy/impl/implementation_singleton.py index 70a04132b..965bd2fc6 100644 --- a/rclpy/rclpy/impl/implementation_singleton.py +++ b/rclpy/rclpy/impl/implementation_singleton.py @@ -31,3 +31,4 @@ rclpy_implementation = _import('._rclpy') rclpy_action_implementation = _import('._rclpy_action') rclpy_logging_implementation = _import('._rclpy_logging') +rclpy_signal_handler_implementation = _import('._rclpy_signal_handler') diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index 4a9d867f4..f82adc443 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -42,6 +42,8 @@ from rclpy.qos import qos_profile_parameter_events from rclpy.qos import qos_profile_services_default from rclpy.qos import QoSProfile +from rclpy.qos_event import PublisherEventCallbacks +from rclpy.qos_event import SubscriptionEventCallbacks from rclpy.service import Service from rclpy.subscription import Subscription from rclpy.time_source import TimeSource @@ -348,7 +350,9 @@ def create_publisher( msg_type, topic: str, *, - qos_profile: QoSProfile = qos_profile_default + qos_profile: QoSProfile = qos_profile_default, + callback_group: Optional[CallbackGroup] = None, + event_callbacks: Optional[PublisherEventCallbacks] = None, ) -> Publisher: """ Create a new publisher. @@ -356,8 +360,13 @@ def create_publisher( :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. + :param callback_group: The callback group for the publisher's event handlers. + If ``None``, then the node's default callback group is used. + :param event_callbacks: User-defined callbacks for middleware events. :return: The new publisher. """ + callback_group = callback_group or self.default_callback_group + # this line imports the typesupport for the message module if not already done check_for_type_support(msg_type) failed = False @@ -368,8 +377,16 @@ def create_publisher( failed = True if failed: self._validate_topic_or_service_name(topic) - publisher = Publisher(publisher_handle, msg_type, topic, qos_profile, self.handle) + + publisher = Publisher( + publisher_handle, msg_type, topic, qos_profile, self.handle, + event_callbacks=event_callbacks or PublisherEventCallbacks(), + callback_group=callback_group) self.publishers.append(publisher) + + for event_callback in publisher.event_handlers: + self.add_waitable(event_callback) + return publisher def create_subscription( @@ -379,7 +396,8 @@ def create_subscription( callback: Callable[[MsgType], None], *, qos_profile: QoSProfile = qos_profile_default, - callback_group: CallbackGroup = None, + callback_group: Optional[CallbackGroup] = None, + event_callbacks: Optional[SubscriptionEventCallbacks] = None, raw: bool = False ) -> Subscription: """ @@ -392,11 +410,12 @@ def create_subscription( :param qos_profile: The quality of service profile to apply to the subscription. :param callback_group: The callback group for the subscription. If ``None``, then the nodes default callback group is used. + :param event_callbacks: User-defined callbacks for middleware events. :param raw: If ``True``, then received messages will be stored in raw binary representation. """ - if callback_group is None: - callback_group = self.default_callback_group + callback_group = callback_group or self.default_callback_group + # this line imports the typesupport for the message module if not already done check_for_type_support(msg_type) failed = False @@ -410,9 +429,14 @@ def create_subscription( subscription = Subscription( subscription_handle, subscription_pointer, msg_type, - topic, callback, callback_group, qos_profile, self.handle, raw) + topic, callback, callback_group, qos_profile, self.handle, raw, + event_callbacks=event_callbacks or SubscriptionEventCallbacks()) self.subscriptions.append(subscription) callback_group.add_entity(subscription) + + for event_handler in subscription.event_handlers: + self.add_waitable(event_handler) + return subscription def create_client( @@ -535,6 +559,11 @@ def create_guard_condition( callback_group.add_entity(guard) return guard + def _cleanup_publisher(self, publisher: Publisher) -> None: + """Free all resources used by the publisher.""" + publisher.event_handlers = [] + _rclpy.rclpy_destroy_node_entity(publisher.publisher_handle, self.handle) + def destroy_publisher(self, publisher: Publisher) -> bool: """ Destroy a publisher created by the node. @@ -543,11 +572,16 @@ def destroy_publisher(self, publisher: Publisher) -> bool: """ for pub in self.publishers: if pub.publisher_handle == publisher.publisher_handle: - _rclpy.rclpy_destroy_node_entity(pub.publisher_handle, self.handle) + self._cleanup_publisher(pub) self.publishers.remove(pub) return True return False + def _cleanup_subscription(self, subscription: Subscription) -> None: + """Free all resources used by the subscription.""" + subscription.event_handlers = [] + _rclpy.rclpy_destroy_node_entity(subscription.subscription_handle, self.handle) + def destroy_subscription(self, subscription: Subscription) -> bool: """ Destroy a subscription created by the node. @@ -556,7 +590,7 @@ def destroy_subscription(self, subscription: Subscription) -> bool: """ for sub in self.subscriptions: if sub.subscription_handle == subscription.subscription_handle: - _rclpy.rclpy_destroy_node_entity(sub.subscription_handle, self.handle) + self._cleanup_subscription(sub) self.subscriptions.remove(sub) return True return False @@ -640,10 +674,10 @@ def destroy_node(self) -> bool: while self.publishers: pub = self.publishers.pop() - _rclpy.rclpy_destroy_node_entity(pub.publisher_handle, self.handle) + self._cleanup_publisher(pub) while self.subscriptions: sub = self.subscriptions.pop() - _rclpy.rclpy_destroy_node_entity(sub.subscription_handle, self.handle) + self._cleanup_subscription(sub) while self.clients: cli = self.clients.pop() _rclpy.rclpy_destroy_node_entity(cli.client_handle, self.handle) diff --git a/rclpy/rclpy/publisher.py b/rclpy/rclpy/publisher.py index 85e8f66b1..914ed15c1 100644 --- a/rclpy/rclpy/publisher.py +++ b/rclpy/rclpy/publisher.py @@ -14,8 +14,10 @@ from typing import TypeVar +from rclpy.callback_groups import CallbackGroup from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.qos import QoSProfile +from rclpy.qos_event import PublisherEventCallbacks MsgType = TypeVar('MsgType') @@ -28,7 +30,9 @@ def __init__( msg_type: MsgType, topic: str, qos_profile: QoSProfile, - node_handle + node_handle, + event_callbacks: PublisherEventCallbacks, + callback_group: CallbackGroup, ) -> None: """ Create a container for a ROS publisher. @@ -51,12 +55,20 @@ def __init__( self.topic = topic self.qos_profile = qos_profile self.node_handle = node_handle + self.callback_group = callback_group + + self.event_callbacks = event_callbacks + self.event_handlers = event_callbacks.create_event_handlers( + callback_group, publisher_handle) def publish(self, msg: MsgType) -> None: """ Send a message to the topic for the publisher. - :param msg: The ROS message to publish. The message must be the same type as the type - provided when the publisher was constructed. + :param msg: The ROS message to publish. + :raises: TypeError if the type of the passed message isn't an instance + of the provided type when the publisher was constructed. """ + if not isinstance(msg, self.msg_type): + raise TypeError() _rclpy.rclpy_publish(self.publisher_handle, msg) diff --git a/rclpy/rclpy/qos_event.py b/rclpy/rclpy/qos_event.py new file mode 100644 index 000000000..0fce11698 --- /dev/null +++ b/rclpy/rclpy/qos_event.py @@ -0,0 +1,224 @@ +# 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. + +from enum import IntEnum +from typing import Callable +from typing import List +from typing import NamedTuple + +from rclpy.callback_groups import CallbackGroup +from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy +from rclpy.waitable import NumberOfEntities +from rclpy.waitable import Waitable + + +class QoSPublisherEventType(IntEnum): + """ + Enum for types of QoS events that a Publisher can receive. + + This enum matches the one defined in rcl/event.h + """ + + RCL_PUBLISHER_OFFERED_DEADLINE_MISSED = 0 + RCL_PUBLISHER_LIVELINESS_LOST = 1 + + +class QoSSubscriptionEventType(IntEnum): + """ + Enum for types of QoS events that a Subscription can receive. + + This enum matches the one defined in rcl/event.h + """ + + RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED = 0 + RCL_SUBSCRIPTION_LIVELINESS_CHANGED = 1 + + +""" +Payload type for Subscription Deadline callback. + +Mirrors rmw_requested_deadline_missed_status_t from rmw/types.h +""" +QoSRequestedDeadlineMissedInfo = NamedTuple( + 'QoSRequestedDeadlineMissedInfo', [ + ('total_count', 'int'), + ('total_count_change', 'int'), + ]) + +""" +Payload type for Subscription Liveliness callback. + +Mirrors rmw_liveliness_changed_status_t from rmw/types.h +""" +QoSLivelinessChangedInfo = NamedTuple( + 'QoSLivelinessChangedInfo', [ + ('alive_count', 'int'), + ('not_alive_count', 'int'), + ('alive_count_change', 'int'), + ('not_alive_count_change', 'int'), + ]) + +""" +Payload type for Publisher Deadline callback. + +Mirrors rmw_offered_deadline_missed_status_t from rmw/types.h +""" +QoSOfferedDeadlineMissedInfo = NamedTuple( + 'QoSOfferedDeadlineMissedInfo', [ + ('total_count', 'int'), + ('total_count_change', 'int'), + ]) + +""" +Payload type for Publisher Liveliness callback. + +Mirrors rmw_liveliness_lost_status_t from rmw/types.h +""" +QoSLivelinessLostInfo = NamedTuple( + 'QoSLivelinessLostInfo', [ + ('total_count', 'int'), + ('total_count_change', 'int'), + ]) + + +class QoSEventHandler(Waitable): + """Waitable type to handle QoS events.""" + + def __init__( + self, + *, + callback_group, + callback, + event_type, + parent_handle, + ): + # Waitable init adds self to callback_group + super().__init__(callback_group) + self.event_type = event_type + self.callback = callback + + self._parent_handle = parent_handle + self._event_handle = _rclpy.rclpy_create_event(event_type, parent_handle) + self._ready_to_take_data = False + self._event_index = None + + # Start Waitable API + 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): + self._ready_to_take_data = True + return self._ready_to_take_data + + def take_data(self): + """Take stuff from lower level so the wait set doesn't immediately wake again.""" + if self._ready_to_take_data: + self._ready_to_take_data = False + return _rclpy.rclpy_take_event( + self._event_handle, self._parent_handle, self.event_type) + return None + + async def execute(self, taken_data): + """Execute work after data has been taken from a ready wait set.""" + if not taken_data: + return + self.callback(taken_data) + + def get_num_entities(self): + """Return number of each type of entity used.""" + return NumberOfEntities(num_events=1) + + def add_to_wait_set(self, wait_set): + """Add entites to wait set.""" + self._event_index = _rclpy.rclpy_wait_set_add_entity('event', wait_set, self._event_handle) + # End Waitable API + + +class SubscriptionEventCallbacks: + """Container to provide middleware event callbacks for a Subscription.""" + + def __init__( + self, + *, + deadline: Callable[[QoSRequestedDeadlineMissedInfo], None] = None, + liveliness: Callable[[QoSLivelinessChangedInfo], None] = None, + ) -> None: + """ + Constructor. + + :param deadline: A user-defined callback that is called when a topic misses our + requested Deadline. + :param liveliness: A user-defined callback that is called when the Liveliness of + a Publisher on subscribed topic changes. + """ + self.deadline = deadline + self.liveliness = liveliness + + def create_event_handlers( + self, callback_group: CallbackGroup, subscription_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)) + return event_handlers + + +class PublisherEventCallbacks: + """Container to provide middleware event callbacks for a Publisher.""" + + def __init__( + self, + *, + deadline: Callable[[QoSOfferedDeadlineMissedInfo], None] = None, + liveliness: Callable[[QoSLivelinessLostInfo], None] = None + ) -> None: + """ + Constructor. + + :param deadline: A user-defined callback that is called when the Publisher misses + its offered Deadline. + :param liveliness: A user-defined callback that is called when this Publisher + fails to signal its Liveliness and is reported as not-alive. + """ + self.deadline = deadline + self.liveliness = liveliness + + def create_event_handlers( + self, callback_group: CallbackGroup, publisher_handle + ) -> List[QoSEventHandler]: + event_handlers = [] + if self.deadline: + event_handlers.append(QoSEventHandler( + callback_group=callback_group, + callback=self.deadline, + event_type=QoSPublisherEventType.RCL_PUBLISHER_OFFERED_DEADLINE_MISSED, + parent_handle=publisher_handle)) + 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)) + return event_handlers diff --git a/rclpy/rclpy/service.py b/rclpy/rclpy/service.py index c37fbe0ab..8d5bd905d 100644 --- a/rclpy/rclpy/service.py +++ b/rclpy/rclpy/service.py @@ -71,5 +71,10 @@ def send_response(self, response: SrvTypeResponse, header) -> None: :param response: The service response. :param header: Capsule pointing to the service header from the original request. + :raises: TypeError if the type of the passed response isn't an instance + of the Response type of the provided service when the service was + constructed. """ + if not isinstance(response, self.srv_type.Response): + raise TypeError() _rclpy.rclpy_send_response(self.service_handle, response, header) diff --git a/rclpy/rclpy/signals.py b/rclpy/rclpy/signals.py new file mode 100644 index 000000000..90e0ea439 --- /dev/null +++ b/rclpy/rclpy/signals.py @@ -0,0 +1,36 @@ +# 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. + +from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy +from rclpy.impl.implementation_singleton import rclpy_signal_handler_implementation as _signals +from rclpy.utilities import get_default_context + + +class SignalHandlerGuardCondition: + + def __init__(self, context=None): + if context is None: + context = get_default_context() + self.guard_handle, _ = _rclpy.rclpy_create_guard_condition(context.handle) + _signals.rclpy_register_sigint_guard_condition(self.guard_handle) + + def __del__(self): + self.destroy() + + def destroy(self): + if self.guard_handle is None: + return + _signals.rclpy_unregister_sigint_guard_condition(self.guard_handle) + _rclpy.rclpy_destroy_entity(self.guard_handle) + self.guard_handle = None diff --git a/rclpy/rclpy/subscription.py b/rclpy/rclpy/subscription.py index a5ee66b8a..a877850cc 100644 --- a/rclpy/rclpy/subscription.py +++ b/rclpy/rclpy/subscription.py @@ -17,6 +17,8 @@ from rclpy.callback_groups import CallbackGroup from rclpy.qos import QoSProfile +from rclpy.qos_event import SubscriptionEventCallbacks + # For documentation only MsgType = TypeVar('MsgType') @@ -34,7 +36,8 @@ def __init__( callback_group: CallbackGroup, qos_profile: QoSProfile, node_handle, - raw: bool + raw: bool, + event_callbacks: SubscriptionEventCallbacks, ) -> None: """ Create a container for a ROS subscription. @@ -68,3 +71,7 @@ def __init__( self._executor_event = False self.qos_profile = qos_profile self.raw = raw + + self.event_callbacks = event_callbacks + self.event_handlers = event_callbacks.create_event_handlers( + callback_group, subscription_handle) diff --git a/rclpy/rclpy/waitable.py b/rclpy/rclpy/waitable.py index 030da2461..a42211ed4 100644 --- a/rclpy/rclpy/waitable.py +++ b/rclpy/rclpy/waitable.py @@ -20,14 +20,19 @@ class NumberOfEntities: 'num_guard_conditions', 'num_timers', 'num_clients', - 'num_services'] + 'num_services', + 'num_events'] - def __init__(self, num_subs=0, num_gcs=0, num_timers=0, num_clients=0, num_services=0): + def __init__( + self, num_subs=0, num_gcs=0, num_timers=0, + num_clients=0, num_services=0, num_events=0 + ): self.num_subscriptions = num_subs self.num_guard_conditions = num_gcs self.num_timers = num_timers self.num_clients = num_clients self.num_services = num_services + self.num_events = num_events def __add__(self, other): result = self.__class__() @@ -38,9 +43,10 @@ def __add__(self, other): return result def __repr__(self): - return '<{0}({1}, {2}, {3}, {4}, {5})>'.format( - self.__class__.__name__, self.num_subscriptions, self.num_guard_conditions, - self.num_timers, self.num_clients, self.num_services) + return '<{0}({1}, {2}, {3}, {4}, {5}, {6})>'.format( + self.__class__.__name__, self.num_subscriptions, + self.num_guard_conditions, self.num_timers, self.num_clients, + self.num_services, self.num_events) class Waitable: diff --git a/rclpy/src/rclpy/_rclpy.c b/rclpy/src/rclpy/_rclpy.c index 09670e284..2406cfe96 100644 --- a/rclpy/src/rclpy/_rclpy.c +++ b/rclpy/src/rclpy/_rclpy.c @@ -36,34 +36,8 @@ #include #include -#include - #include "rclpy_common/common.h" - -static rcl_guard_condition_t * g_sigint_gc_handle; - -#ifdef _WIN32 -_crt_signal_t g_original_signal_handler = NULL; -#else -sig_t g_original_signal_handler = NULL; -#endif // _WIN32 - -/// Catch signals -static void catch_function(int signo) -{ - (void) signo; - if (NULL != g_sigint_gc_handle) { - rcl_ret_t ret = rcl_trigger_guard_condition(g_sigint_gc_handle); - if (ret != RCL_RET_OK) { - PyErr_Format(PyExc_RuntimeError, - "Failed to trigger guard_condition: %s", rcl_get_error_string().str); - rcl_reset_error(); - } - } - if (NULL != g_original_signal_handler) { - g_original_signal_handler(signo); - } -} +#include "./_rclpy_qos_event.c" void _rclpy_context_capsule_destructor(PyObject * capsule) @@ -124,80 +98,6 @@ rclpy_create_context(PyObject * Py_UNUSED(self), PyObject * Py_UNUSED(args)) return PyCapsule_New(context, "rcl_context_t", _rclpy_context_capsule_destructor); } -/// Create a sigint guard condition -/** - * A successful call will return a list with two elements: - * - * - a Capsule with the pointer of the created rcl_guard_condition_t * structure - * - an integer representing the memory address of the rcl_guard_condition_t - * - * Raises RuntimeError if initializing the guard condition fails - * - * \return a list with the capsule and memory location, or - * \return NULL on failure - */ -static PyObject * -rclpy_get_sigint_guard_condition(PyObject * Py_UNUSED(self), PyObject * args) -{ - PyObject * pycontext; - - if (!PyArg_ParseTuple(args, "O", &pycontext)) { - return NULL; - } - - rcl_context_t * context = (rcl_context_t *)PyCapsule_GetPointer(pycontext, "rcl_context_t"); - if (NULL == context) { - return NULL; - } - - rcl_guard_condition_t * sigint_gc = - (rcl_guard_condition_t *)PyMem_Malloc(sizeof(rcl_guard_condition_t)); - if (!sigint_gc) { - PyErr_Format(PyExc_MemoryError, "Failed to allocate memory for sigint guard condition"); - return NULL; - } - *sigint_gc = rcl_get_zero_initialized_guard_condition(); - rcl_guard_condition_options_t sigint_gc_options = rcl_guard_condition_get_default_options(); - - rcl_ret_t ret = rcl_guard_condition_init(sigint_gc, context, sigint_gc_options); - if (ret != RCL_RET_OK) { - PyErr_Format(PyExc_RuntimeError, - "Failed to create guard_condition: %s", rcl_get_error_string().str); - rcl_reset_error(); - PyMem_Free(sigint_gc); - return NULL; - } - - PyObject * pylist = PyList_New(2); - if (!pylist) { - ret = rcl_guard_condition_fini(sigint_gc); - PyMem_Free(sigint_gc); - return NULL; - } - - PyObject * pysigint_gc = PyCapsule_New(sigint_gc, "rcl_guard_condition_t", NULL); - if (!pysigint_gc) { - ret = rcl_guard_condition_fini(sigint_gc); - PyMem_Free(sigint_gc); - Py_DECREF(pylist); - return NULL; - } - - PyObject * pysigint_gc_impl_reference = PyLong_FromUnsignedLongLong((uint64_t)&sigint_gc->impl); - if (!pysigint_gc_impl_reference) { - ret = rcl_guard_condition_fini(sigint_gc); - PyMem_Free(sigint_gc); - Py_DECREF(pylist); - Py_DECREF(pysigint_gc); - return NULL; - } - - g_sigint_gc_handle = sigint_gc; - PyList_SET_ITEM(pylist, 0, pysigint_gc); - PyList_SET_ITEM(pylist, 1, pysigint_gc_impl_reference); - return pylist; -} - /// Create a general purpose guard condition /** * A successful call will return a list with two elements: @@ -604,9 +504,6 @@ rclpy_init(PyObject * Py_UNUSED(self), PyObject * args) } Py_DECREF(pyseqlist); - // Register our signal handler that will forward to the original one. - g_original_signal_handler = signal(SIGINT, catch_function); - if (PyErr_Occurred()) { return NULL; } @@ -2405,11 +2302,12 @@ rclpy_destroy_entity(PyObject * Py_UNUSED(self), PyObject * args) } else if (PyCapsule_IsValid(pyentity, "rcl_guard_condition_t")) { rcl_guard_condition_t * guard_condition = (rcl_guard_condition_t *)PyCapsule_GetPointer( pyentity, "rcl_guard_condition_t"); - if (g_sigint_gc_handle == guard_condition) { - g_sigint_gc_handle = NULL; - } ret = rcl_guard_condition_fini(guard_condition); PyMem_Free(guard_condition); + } else if (PyCapsule_IsValid(pyentity, "rcl_event_t")) { + PyCapsule_SetDestructor(pyentity, NULL); + _destroy_event_capsule(pyentity); + ret = RCL_RET_OK; } else { ret = RCL_RET_ERROR; // to avoid a linter warning PyErr_Format(PyExc_RuntimeError, "'%s' is not a known entity", PyCapsule_GetName(pyentity)); @@ -2482,12 +2380,13 @@ rclpy_wait_set_init(PyObject * Py_UNUSED(self), PyObject * args) unsigned PY_LONG_LONG number_of_timers; unsigned PY_LONG_LONG number_of_clients; unsigned PY_LONG_LONG number_of_services; + unsigned PY_LONG_LONG number_of_events; PyObject * pycontext; if (!PyArg_ParseTuple( - args, "OKKKKKO", &pywait_set, &number_of_subscriptions, + args, "OKKKKKKO", &pywait_set, &number_of_subscriptions, &number_of_guard_conditions, &number_of_timers, - &number_of_clients, &number_of_services, &pycontext)) + &number_of_clients, &number_of_services, &number_of_events, &pycontext)) { return NULL; } @@ -2503,8 +2402,15 @@ rclpy_wait_set_init(PyObject * Py_UNUSED(self), PyObject * args) } rcl_ret_t ret = rcl_wait_set_init( - wait_set, number_of_subscriptions, number_of_guard_conditions, number_of_timers, - number_of_clients, number_of_services, context, rcl_get_default_allocator()); + wait_set, + number_of_subscriptions, + number_of_guard_conditions, + number_of_timers, + number_of_clients, + number_of_services, + number_of_events, + context, + rcl_get_default_allocator()); if (ret != RCL_RET_OK) { PyErr_Format(PyExc_RuntimeError, "Failed to initialize wait set: %s", rcl_get_error_string().str); @@ -2589,6 +2495,9 @@ rclpy_wait_set_add_entity(PyObject * Py_UNUSED(self), PyObject * args) rcl_guard_condition_t * guard_condition = (rcl_guard_condition_t *)PyCapsule_GetPointer(pyentity, "rcl_guard_condition_t"); ret = rcl_wait_set_add_guard_condition(wait_set, guard_condition, &index); + } else if (0 == strcmp(entity_type, "event")) { + rcl_event_t * event = (rcl_event_t *)PyCapsule_GetPointer(pyentity, "rcl_event_t"); + ret = rcl_wait_set_add_event(wait_set, event, &index); } else { ret = RCL_RET_ERROR; // to avoid a linter warning PyErr_Format(PyExc_RuntimeError, @@ -2654,6 +2563,9 @@ rclpy_wait_set_is_ready(PyObject * Py_UNUSED(self), PyObject * args) } else if (0 == strcmp(entity_type, "guard_condition")) { entities = (void *)wait_set->guard_conditions; num_entities = wait_set->size_of_guard_conditions; + } else if (0 == strcmp(entity_type, "event")) { + entities = (void *)wait_set->events; + num_entities = wait_set->size_of_events; } else { PyErr_Format(PyExc_RuntimeError, "'%s' is not a known entity", entity_type); @@ -3129,10 +3041,6 @@ rclpy_shutdown(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - // Restore the original signal handler. - signal(SIGINT, g_original_signal_handler); - g_original_signal_handler = NULL; - Py_RETURN_NONE; } @@ -4627,6 +4535,10 @@ static PyMethodDef rclpy_methods[] = { "rclpy_create_timer", rclpy_create_timer, METH_VARARGS, "Create a Timer." }, + { + "rclpy_create_event", rclpy_create_event, METH_VARARGS, + "Create an Event." + }, { "rclpy_get_sigint_guard_condition", rclpy_get_sigint_guard_condition, METH_VARARGS, @@ -4767,6 +4679,10 @@ static PyMethodDef rclpy_methods[] = { "rclpy_take_response", rclpy_take_response, METH_VARARGS, "rclpy_take_response." }, + { + "rclpy_take_event", rclpy_take_event, METH_VARARGS, + "Get the pending data for a ready QoS Event." + }, { "rclpy_ok", rclpy_ok, METH_VARARGS, diff --git a/rclpy/src/rclpy/_rclpy_qos_event.c b/rclpy/src/rclpy/_rclpy_qos_event.c new file mode 100644 index 000000000..4eac52666 --- /dev/null +++ b/rclpy/src/rclpy/_rclpy_qos_event.c @@ -0,0 +1,334 @@ +// 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 + +typedef union _qos_event_callback_data { + // Subscription events + rmw_requested_deadline_missed_status_t requested_deadline_missed; + rmw_liveliness_changed_status_t liveliness_changed; + // Publisher events + rmw_offered_deadline_missed_status_t offered_deadline_missed; + rmw_liveliness_lost_status_t liveliness_lost; +} _qos_event_callback_data_t; + +typedef PyObject * (* _qos_event_data_filler_function)(_qos_event_callback_data_t *); + +bool +_check_rcl_return(rcl_ret_t ret, const char * error_msg) +{ + if (ret != RCL_RET_OK) { + PyErr_Format(PyExc_RuntimeError, + "%s: %s", error_msg, rcl_get_error_string().str); + rcl_reset_error(); + return false; + } + return true; +} + +void +_destroy_event_capsule(PyObject * pycapsule) +{ + rcl_event_t * event = (rcl_event_t *)PyCapsule_GetPointer(pycapsule, "rcl_event_t"); + rcl_ret_t ret = rcl_event_fini(event); + PyMem_Free(event); + _check_rcl_return(ret, "Failed to fini 'rcl_event_t'"); +} + +bool +_is_pycapsule_rcl_subscription(PyObject * pycapsule) +{ + return PyCapsule_IsValid(pycapsule, "rcl_subscription_t"); +} + +bool +_is_pycapsule_rcl_publisher(PyObject * pycapsule) +{ + return PyCapsule_IsValid(pycapsule, "rcl_publisher_t"); +} + +bool +_is_pycapsule_rcl_event(PyObject * pycapsule) +{ + return PyCapsule_IsValid(pycapsule, "rcl_event_t"); +} + +rcl_subscription_t * +_pycapsule_to_rcl_subscription(PyObject * pycapsule) +{ + return (rcl_subscription_t *)PyCapsule_GetPointer(pycapsule, "rcl_subscription_t"); +} + +rcl_publisher_t * +_pycapsule_to_rcl_publisher(PyObject * pycapsule) +{ + return (rcl_publisher_t *)PyCapsule_GetPointer(pycapsule, "rcl_publisher_t"); +} + +rcl_event_t * +_pycapsule_to_rcl_event(PyObject * pycapsule) +{ + return (rcl_event_t *)PyCapsule_GetPointer(pycapsule, "rcl_event_t"); +} + +rcl_event_t * +_new_zero_initialized_rcl_event() +{ + rcl_event_t * event = (rcl_event_t *)PyMem_Malloc(sizeof(rcl_event_t)); + if (!event) { + PyErr_Format(PyExc_MemoryError, "Failed to allocate memory for event"); + return NULL; + } + *event = rcl_get_zero_initialized_event(); + return event; +} + + +PyObject * _create_py_qos_event(const char * class_name, PyObject * args) +{ + PyObject * pyqos_event_module = NULL; + PyObject * pyqos_event_class = NULL; + PyObject * pyqos_event = NULL; + + pyqos_event_module = PyImport_ImportModule("rclpy.qos_event"); + if (!pyqos_event_module) { + goto cleanup; + } + + pyqos_event_class = PyObject_GetAttrString(pyqos_event_module, class_name); + if (!pyqos_event_class) { + goto cleanup; + } + + pyqos_event = PyObject_CallObject(pyqos_event_class, args); + +cleanup: + Py_XDECREF(pyqos_event_module); + Py_XDECREF(pyqos_event_class); + Py_XDECREF(args); + + return pyqos_event; +} + +PyObject * +_requested_deadline_missed_to_py_object(_qos_event_callback_data_t * data) +{ + rmw_requested_deadline_missed_status_t * actual_data = &data->requested_deadline_missed; + PyObject * args = Py_BuildValue( + "ii", + actual_data->total_count, + actual_data->total_count_change); + if (!args) { + return NULL; + } + return _create_py_qos_event("QoSRequestedDeadlineMissedInfo", args); +} + +PyObject * +_liveliness_changed_to_py_object(_qos_event_callback_data_t * data) +{ + rmw_liveliness_changed_status_t * actual_data = &data->liveliness_changed; + PyObject * args = Py_BuildValue( + "iiii", + actual_data->alive_count, + actual_data->not_alive_count, + actual_data->alive_count_change, + actual_data->not_alive_count_change); + if (!args) { + return NULL; + } + return _create_py_qos_event("QoSLivelinessChangedInfo", args); +} + +PyObject * +_offered_deadline_missed_to_py_object(_qos_event_callback_data_t * data) +{ + rmw_offered_deadline_missed_status_t * actual_data = &data->offered_deadline_missed; + PyObject * args = Py_BuildValue( + "ii", + actual_data->total_count, + actual_data->total_count_change); + if (!args) { + return NULL; + } + return _create_py_qos_event("QoSOfferedDeadlineMissedInfo", args); +} + +PyObject * +_liveliness_lost_to_py_object(_qos_event_callback_data_t * data) +{ + rmw_liveliness_lost_status_t * actual_data = &data->liveliness_lost; + PyObject * args = Py_BuildValue( + "ii", + actual_data->total_count, + actual_data->total_count_change); + if (!args) { + return NULL; + } + return _create_py_qos_event("QoSLivelinessLostInfo", args); +} + + +_qos_event_data_filler_function +_get_qos_event_data_filler_function_for(PyObject * pyparent, uint32_t event_type) +{ + if (_is_pycapsule_rcl_subscription(pyparent)) { + switch (event_type) { + case RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED: + return &_requested_deadline_missed_to_py_object; + case RCL_SUBSCRIPTION_LIVELINESS_CHANGED: + return &_liveliness_changed_to_py_object; + default: + PyErr_Format(PyExc_ValueError, + "Event type %d for Subscriptions not understood by rclpy.", event_type); + } + } else if (_is_pycapsule_rcl_publisher(pyparent)) { + switch (event_type) { + case RCL_PUBLISHER_OFFERED_DEADLINE_MISSED: + return &_offered_deadline_missed_to_py_object; + case RCL_PUBLISHER_LIVELINESS_LOST: + return &_liveliness_lost_to_py_object; + default: + PyErr_Format(PyExc_ValueError, + "Event type %d for Publishers not understood by rclpy.", event_type); + } + } else { + PyErr_Format(PyExc_TypeError, + "Parent handle was not a valid Publisher or Subscription."); + } + return NULL; +} + +/// Create an event object for QoS event handling. +/** + * This function will create an event handle for the given Subscription or Publisher parent. + * + * Raises MemoryError if the event can't be allocated + * Raises RuntimeError on initialization failure + * Raises TypeError if the capsules are not the correct types + * + * \param[in] pyevent_type Enum value of + * rcl_publisher_event_type_t or rcl_subscription_event_type_t, chosen by the type of pyparent + * \param[in] pyparent Capsule containing the parent Publisher or Subscription + * \return capsule containing rcl_event_t + * \return NULL on failure + */ +static PyObject * +rclpy_create_event(PyObject * Py_UNUSED(self), PyObject * args) +{ + unsigned PY_LONG_LONG pyevent_type; + PyObject * pyparent = NULL; + + rcl_ret_t ret; + rcl_subscription_t * subscription = NULL; + rcl_publisher_t * publisher = NULL; + rcl_event_t * event = NULL; + + PyObject * pyevent = NULL; + + if (!PyArg_ParseTuple(args, "KO", &pyevent_type, &pyparent)) { + return NULL; + } + + if (_is_pycapsule_rcl_subscription(pyparent)) { + subscription = _pycapsule_to_rcl_subscription(pyparent); + } else if (_is_pycapsule_rcl_publisher(pyparent)) { + publisher = _pycapsule_to_rcl_publisher(pyparent); + } else { + PyErr_Format(PyExc_TypeError, "Event parent was not a valid Publisher or Subscription."); + return NULL; + } + + event = _new_zero_initialized_rcl_event(); + if (!event) { + return NULL; + } + + if (subscription) { + ret = rcl_subscription_event_init(event, subscription, pyevent_type); + } else { + ret = rcl_publisher_event_init(event, publisher, pyevent_type); + } + if (!_check_rcl_return(ret, "Failed to initialize event")) { + PyMem_Free(event); + return NULL; + } + + pyevent = PyCapsule_New(event, "rcl_event_t", _destroy_event_capsule); + if (!pyevent) { + ret = rcl_event_fini(event); + PyMem_Free(event); + _check_rcl_return(ret, "Failed to fini 'rcl_event_t'"); + return NULL; + } + + return pyevent; +} + +/// Get a pending QoS event's data +/** + * After having determined that a middleware event is ready, get the callback payload. + * + * Raises RuntimeError on failure to take the event from the middleware + * Raises TypeError if the capsules are not the correct types + * Raises ValueError on unknown event_type argument + * + * \param[in] pyevent Event handle from rclpy_create_event + * \param[in] pyevent_type Enum value of + * rcl_publisher_event_type_t or rcl_subscription_event_type_t, chosen by the type of pyparent + * \param[in] pyparent Capsule containing the parent Publisher or Subscription + * \return Python object from rclpy.qos_event containing callback data + * \return NULL on failure + */ +static PyObject * +rclpy_take_event(PyObject * Py_UNUSED(self), PyObject * args) +{ + // Arguments + PyObject * pyevent = NULL; + PyObject * pyparent = NULL; + unsigned PY_LONG_LONG pyevent_type; + + // Type conversion + rcl_ret_t ret; + rcl_event_t * event = NULL; + PyObject * pyqos_event = NULL; + _qos_event_callback_data_t event_data; + _qos_event_data_filler_function event_filler = NULL; + + if (!PyArg_ParseTuple(args, "OOK", &pyevent, &pyparent, &pyevent_type)) { + return NULL; + } + + if (!_is_pycapsule_rcl_event(pyevent)) { + PyErr_Format(PyExc_TypeError, "Capsule was not a valid rcl_event_t"); + return NULL; + } + event = _pycapsule_to_rcl_event(pyevent); + + event_filler = _get_qos_event_data_filler_function_for(pyparent, pyevent_type); + if (!event_filler) { + return NULL; + } + + ret = rcl_take_event(event, &event_data); + if (!_check_rcl_return(ret, "Failed to take event")) { + return NULL; + } + + pyqos_event = event_filler(&event_data); + if (!pyqos_event) { + return NULL; + } + return pyqos_event; +} diff --git a/rclpy/src/rclpy/_rclpy_signal_handler.c b/rclpy/src/rclpy/_rclpy_signal_handler.c new file mode 100644 index 000000000..e01a74279 --- /dev/null +++ b/rclpy/src/rclpy/_rclpy_signal_handler.c @@ -0,0 +1,316 @@ +// 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 + +#ifdef _WIN32 + #define SIGNAL_HANDLER_T _crt_signal_t +#else + #define SIGNAL_HANDLER_T sig_t +#endif // _WIN32 + +/// Global reference to original signal handler for chaining purposes +SIGNAL_HANDLER_T g_original_signal_handler = NULL; + +typedef _Atomic (rcl_guard_condition_t **) atomic_rcl_guard_condition_ptrptr_t; + +/// Global reference to guard conditions +/// End with sentinel value instead of count to avoid mismatch if signal +/// interrupts while adding or removing from the list +atomic_rcl_guard_condition_ptrptr_t g_guard_conditions; + +/// Warn if getting g_guard_conditions could deadlock the signal handler +/// \return 0 if no exception is raised, -1 if an exception was raised +static int +check_signal_safety() +{ + static bool did_warn = false; + if (!did_warn && !atomic_is_lock_free(&g_guard_conditions)) { + did_warn = true; + const char * deadlock_msg = + "Global guard condition list access is not lock-free on this platform." + "The program may deadlock when receiving SIGINT."; + return PyErr_WarnEx(PyExc_ResourceWarning, deadlock_msg, 1); + } + return 0; +} + +// Forward declaration +static void catch_function(int signo); + +/// Restore the original signal handler when ours was registered +static void +restore_original_signal_handler() +{ + const SIGNAL_HANDLER_T current_handler = signal(SIGINT, g_original_signal_handler); + if (current_handler != catch_function) { + // Oops, someone else must have registered a signal handler that chains to us + // put it back so it continues to work + signal(SIGINT, current_handler); + return; + } + // Got ourself out of the chain + g_original_signal_handler = NULL; +} + +/// Register our signal handler and store the current +static void +register_signal_handler() +{ + if (NULL != g_original_signal_handler) { + // We must already be registered + return; + } + g_original_signal_handler = signal(SIGINT, catch_function); +} + +/// Call the original signal handler if there was one +static void +call_original_signal_handler(int signo) +{ + if (NULL != g_original_signal_handler) { + g_original_signal_handler(signo); + } +} + +/// Catch signals +/** + * This triggers guard conditions when a signal is received. + * These wake executors currently blocked in `rcl_wait`. + */ +static void catch_function(int signo) +{ + rcl_guard_condition_t ** guard_conditions; + rcutils_atomic_load(&g_guard_conditions, guard_conditions); + if (NULL == guard_conditions || NULL == guard_conditions[0]) { + call_original_signal_handler(signo); + // There may have been another signal handler chaining to this one when we last tried to + // restore the old signal handler. + // Try to restore the old handler again. + restore_original_signal_handler(); + return; + } + + rcl_guard_condition_t ** pgc = guard_conditions; + // Trigger python signal handlers + while (NULL != *pgc) { + rcl_ret_t ret = rcl_trigger_guard_condition(*pgc); + if (ret != RCL_RET_OK) { + // TODO(sloretz) find signal safe way to tell the world an error occurred + rcl_reset_error(); + } + ++pgc; + } + + // Chain signal handlers + call_original_signal_handler(signo); +} + +/// Register a guard condition to be triggered when SIGINT is received. +/** + * Raises ValueError if the argument is not a guard condition handle + * Raises ValueError if the argument was already registered + * + * \param[in] pygc a guard condition pycapsule + * \return None + */ +static PyObject * +rclpy_register_sigint_guard_condition(PyObject * Py_UNUSED(self), PyObject * args) +{ + // Expect a pycapsule with a guard condition + PyObject * pygc; + if (!PyArg_ParseTuple(args, "O", &pygc)) { + return NULL; + } + + if (0 != check_signal_safety()) { + // exception raised + return NULL; + } + + rcl_guard_condition_t * gc = (rcl_guard_condition_t *)PyCapsule_GetPointer( + pygc, "rcl_guard_condition_t"); + if (!gc) { + return NULL; + } + + rcl_guard_condition_t ** guard_conditions; + rcutils_atomic_load(&g_guard_conditions, guard_conditions); + + // Figure out how big the list currently is + size_t count_gcs = 0; + if (NULL != guard_conditions) { + while (NULL != guard_conditions[count_gcs]) { + if (gc == guard_conditions[count_gcs]) { + PyErr_Format(PyExc_ValueError, "Guard condition was already registered"); + return NULL; + } + ++count_gcs; + } + } + + // Current size of guard condition list: count_gcs + 1 (sentinel value) + // Allocate space for one more guard condition: count_cs + 1 (new gc) + 1 (sentinel value) + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_guard_condition_t ** new_gcs = + allocator.allocate(sizeof(rcl_guard_condition_t *) * (count_gcs + 2), allocator.state); + + // populate the new guard condition list, ending with a sentinel of NULL + for (size_t i = 0; i < count_gcs; ++i) { + new_gcs[i] = guard_conditions[i]; + } + new_gcs[count_gcs] = gc; + new_gcs[count_gcs + 1] = NULL; + + // Swap the lists and free the old + rcl_guard_condition_t ** old_gcs; + rcutils_atomic_exchange(&g_guard_conditions, old_gcs, new_gcs); + if (NULL != old_gcs) { + allocator.deallocate(old_gcs, allocator.state); + } + + // make sure our signal handler is registered + register_signal_handler(); + + Py_RETURN_NONE; +} + +/// Unregister a guard condition so it is not triggered when SIGINT is received. +/** + * Raises ValueError if the argument is not a guard condition handle + * Raises ValueError if the argument was not registered + * + * \param[in] pygc a guard condition pycapsule + * \return None + */ +static PyObject * +rclpy_unregister_sigint_guard_condition(PyObject * Py_UNUSED(self), PyObject * args) +{ + // Expect a pycapsule with a guard condition + PyObject * pygc; + if (!PyArg_ParseTuple(args, "O", &pygc)) { + return NULL; + } + + rcl_guard_condition_t * gc = (rcl_guard_condition_t *)PyCapsule_GetPointer( + pygc, "rcl_guard_condition_t"); + if (!gc) { + return NULL; + } + + rcl_guard_condition_t ** guard_conditions; + rcutils_atomic_load(&g_guard_conditions, guard_conditions); + + // Figure out how big the list currently is + size_t count_gcs = 0; + bool found_gc = false; + // assumes guard condition was only added to list once + size_t found_index = 0; + + if (NULL != guard_conditions) { + while (NULL != guard_conditions[count_gcs]) { + if (gc == guard_conditions[count_gcs]) { + found_gc = true; + found_index = count_gcs; + } + ++count_gcs; + } + } + + if (!found_gc) { + PyErr_Format(PyExc_ValueError, "Guard condition was not registered"); + return NULL; + } + + rcl_allocator_t allocator = rcl_get_default_allocator(); + + if (count_gcs == 1) { + // Just delete the list if there are no guard conditions left + rcl_guard_condition_t ** old_gcs; + rcutils_atomic_exchange(&g_guard_conditions, old_gcs, NULL); + allocator.deallocate(old_gcs, allocator.state); + restore_original_signal_handler(); + } else { + // Create space for one less guard condition + // current list size: count_gcs + 1 (sentinel) + // new list size: count_gcs - 1 (removing a guard condition) + 1 (sentinel) + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_guard_condition_t ** new_gcs = + allocator.allocate(sizeof(rcl_guard_condition_t *) * (count_gcs), allocator.state); + + // Put remaining guard conditions in the list, ending with a sentinel of NULL + for (size_t i = 0; i < found_index; ++i) { + new_gcs[i] = guard_conditions[i]; + } + // one less guard condition + --count_gcs; + for (size_t i = found_index; i < count_gcs; ++i) { + new_gcs[i] = guard_conditions[i + 1]; + } + // Put sentinel at end + new_gcs[count_gcs] = NULL; + + // Replace guard condition list + rcl_guard_condition_t ** old_gcs; + rcutils_atomic_exchange(&g_guard_conditions, old_gcs, new_gcs); + allocator.deallocate(old_gcs, allocator.state); + } + + Py_RETURN_NONE; +} + +/// Define the public methods of this module +static PyMethodDef rclpy_signal_handler_methods[] = { + { + "rclpy_register_sigint_guard_condition", rclpy_register_sigint_guard_condition, + METH_VARARGS, + "Register a guard condition to be called on SIGINT." + }, + { + "rclpy_unregister_sigint_guard_condition", rclpy_unregister_sigint_guard_condition, + METH_VARARGS, + "Stop triggering a guard condition when SIGINT occurs." + }, + {NULL, NULL, 0, NULL} /* sentinel */ +}; + +PyDoc_STRVAR(rclpy_signal_handler__doc__, + "RCLPY module for handling signals."); + +/// Define the Python module +static struct PyModuleDef _rclpy_signal_handler_module = { + PyModuleDef_HEAD_INIT, + "_rclpy_signal_handler", + rclpy_signal_handler__doc__, + -1, /* -1 means that the module keeps state in global variables */ + rclpy_signal_handler_methods, + NULL, + NULL, + NULL, + NULL +}; + +/// Init function of this module +PyMODINIT_FUNC PyInit__rclpy_signal_handler(void) +{ + atomic_init(&g_guard_conditions, NULL); + return PyModule_Create(&_rclpy_signal_handler_module); +} diff --git a/rclpy/test/action/test_client.py b/rclpy/test/action/test_client.py index bc9ab9466..b7b664465 100644 --- a/rclpy/test/action/test_client.py +++ b/rclpy/test/action/test_client.py @@ -327,6 +327,16 @@ def test_get_result_async(self): finally: ac.destroy() + def test_different_type_raises(self): + ac = ActionClient(self.node, Fibonacci, 'fibonacci') + try: + with self.assertRaises(TypeError): + ac.send_goal('different goal type') + with self.assertRaises(TypeError): + ac.send_goal_async('different goal type') + finally: + ac.destroy() + if __name__ == '__main__': unittest.main() diff --git a/rclpy/test/action/test_server.py b/rclpy/test/action/test_server.py index 4642454b5..ca5d29366 100644 --- a/rclpy/test/action/test_server.py +++ b/rclpy/test/action/test_server.py @@ -618,6 +618,40 @@ def execute_with_feedback(goal_handle): self.mock_action_client.feedback_msg.feedback.sequence.tolist()) action_server.destroy() + def test_different_feedback_type_raises(self): + + def execute_with_feedback(goal_handle): + try: + goal_handle.publish_feedback('different feedback type') + except TypeError: + feedback = Fibonacci.Feedback() + feedback.sequence = [1, 1, 2, 3] + goal_handle.publish_feedback(feedback) + goal_handle.succeed() + return Fibonacci.Result() + + action_server = ActionServer( + self.node, + Fibonacci, + 'fibonacci', + execute_callback=execute_with_feedback, + ) + + try: + goal_msg = Fibonacci.Impl.SendGoalService.Request() + goal_msg.goal_id = UUID(uuid=list(uuid.uuid4().bytes)) + goal_future = self.mock_action_client.send_goal(goal_msg) + + rclpy.spin_until_future_complete( + self.node, goal_future, self.executor) + + feedback_msg = self.mock_action_client.feedback_msg + self.assertIsNotNone(feedback_msg) + self.assertEqual( + [1, 1, 2, 3], feedback_msg.feedback.sequence.tolist()) + finally: + action_server.destroy() + if __name__ == '__main__': unittest.main() diff --git a/rclpy/test/test_client.py b/rclpy/test/test_client.py index 49d8b1bfc..4f8e60fca 100644 --- a/rclpy/test/test_client.py +++ b/rclpy/test/test_client.py @@ -90,6 +90,25 @@ def test_concurrent_calls_to_service(self): self.node.destroy_client(cli) self.node.destroy_service(srv) + def test_different_type_raises(self): + cli = self.node.create_client(GetParameters, 'get/parameters') + srv = self.node.create_service( + GetParameters, 'get/parameters', + lambda request, response: 'different response type') + try: + with self.assertRaises(TypeError): + cli.call('different request type') + with self.assertRaises(TypeError): + cli.call_async('different request type') + self.assertTrue(cli.wait_for_service(timeout_sec=20)) + future = cli.call_async(GetParameters.Request()) + executor = rclpy.executors.SingleThreadedExecutor(context=self.context) + with self.assertRaises(TypeError): + rclpy.spin_until_future_complete(self.node, future, executor=executor) + finally: + self.node.destroy_client(cli) + self.node.destroy_service(srv) + if __name__ == '__main__': unittest.main() diff --git a/rclpy/test/test_messages.py b/rclpy/test/test_messages.py index 4e35a37a3..a3fcbed5d 100644 --- a/rclpy/test/test_messages.py +++ b/rclpy/test/test_messages.py @@ -45,3 +45,8 @@ def test_invalid_string_raises(self): pub = self.node.create_publisher(Primitives, 'chatter') with self.assertRaises(UnicodeEncodeError): pub.publish(msg) + + def test_different_type_raises(self): + pub = self.node.create_publisher(Primitives, 'chatter') + with self.assertRaises(TypeError): + pub.publish('different message type') diff --git a/rclpy/test/test_qos_event.py b/rclpy/test/test_qos_event.py new file mode 100644 index 000000000..b9e3f776c --- /dev/null +++ b/rclpy/test/test_qos_event.py @@ -0,0 +1,233 @@ +# 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. + +import unittest +from unittest.mock import Mock + +import rclpy +from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy +from rclpy.qos_event import PublisherEventCallbacks +from rclpy.qos_event import QoSLivelinessChangedInfo +from rclpy.qos_event import QoSLivelinessLostInfo +from rclpy.qos_event import QoSOfferedDeadlineMissedInfo +from rclpy.qos_event import QoSPublisherEventType +from rclpy.qos_event import QoSRequestedDeadlineMissedInfo +from rclpy.qos_event import QoSSubscriptionEventType +from rclpy.qos_event import SubscriptionEventCallbacks + +from test_msgs.msg import Empty as EmptyMsg + + +class TestQoSEvent(unittest.TestCase): + + @classmethod + def setUpClass(cls): + cls.context = rclpy.context.Context() + rclpy.init(context=cls.context) + cls.node = rclpy.create_node('TestQoSEvent', namespace='/rclpy/test', context=cls.context) + + @classmethod + def tearDownClass(cls): + cls.node.destroy_node() + rclpy.shutdown(context=cls.context) + + def test_publisher_constructor(self): + callbacks = PublisherEventCallbacks() + liveliness_callback = Mock() + deadline_callback = Mock() + + # No arg + publisher = self.node.create_publisher(EmptyMsg, 'test_topic') + self.assertEqual(len(publisher.event_handlers), 0) + self.node.destroy_publisher(publisher) + + # Arg with no callbacks + publisher = self.node.create_publisher(EmptyMsg, 'test_topic', event_callbacks=callbacks) + self.assertEqual(len(publisher.event_handlers), 0) + self.node.destroy_publisher(publisher) + + # Arg with one of the callbacks + callbacks.deadline = deadline_callback + publisher = self.node.create_publisher(EmptyMsg, 'test_topic', event_callbacks=callbacks) + self.assertEqual(len(publisher.event_handlers), 1) + self.node.destroy_publisher(publisher) + + # Arg with both callbacks + callbacks.liveliness = liveliness_callback + publisher = self.node.create_publisher(EmptyMsg, 'test_topic', event_callbacks=callbacks) + self.assertEqual(len(publisher.event_handlers), 2) + self.node.destroy_publisher(publisher) + + def test_subscription_constructor(self): + callbacks = SubscriptionEventCallbacks() + liveliness_callback = Mock() + deadline_callback = Mock() + message_callback = Mock() + + # No arg + subscription = self.node.create_subscription(EmptyMsg, 'test_topic', message_callback) + self.assertEqual(len(subscription.event_handlers), 0) + self.node.destroy_subscription(subscription) + + # Arg with no callbacks + subscription = self.node.create_subscription( + EmptyMsg, 'test_topic', message_callback, event_callbacks=callbacks) + self.assertEqual(len(subscription.event_handlers), 0) + self.node.destroy_subscription(subscription) + + # Arg with one of the callbacks + callbacks.deadline = deadline_callback + subscription = self.node.create_subscription( + EmptyMsg, 'test_topic', message_callback, event_callbacks=callbacks) + self.assertEqual(len(subscription.event_handlers), 1) + self.node.destroy_subscription(subscription) + + # Arg with both callbacks + callbacks.liveliness = liveliness_callback + subscription = self.node.create_subscription( + EmptyMsg, 'test_topic', message_callback, event_callbacks=callbacks) + self.assertEqual(len(subscription.event_handlers), 2) + self.node.destroy_subscription(subscription) + + def test_event_create_destroy(self): + # Publisher event types + publisher = self.node.create_publisher(EmptyMsg, 'test_topic') + + event_handle = _rclpy.rclpy_create_event( + QoSPublisherEventType.RCL_PUBLISHER_OFFERED_DEADLINE_MISSED, + publisher.publisher_handle) + self.assertIsNotNone(event_handle) + _rclpy.rclpy_destroy_entity(event_handle) + + event_handle = _rclpy.rclpy_create_event( + QoSPublisherEventType.RCL_PUBLISHER_LIVELINESS_LOST, + publisher.publisher_handle) + self.assertIsNotNone(event_handle) + _rclpy.rclpy_destroy_entity(event_handle) + + self.node.destroy_publisher(publisher) + + # 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) + + 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) + + self.node.destroy_subscription(subscription) + + 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, 'test_topic') + 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) + + deadline_event_handle = _rclpy.rclpy_create_event( + QoSPublisherEventType.RCL_PUBLISHER_OFFERED_DEADLINE_MISSED, + publisher.publisher_handle) + 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( + QoSPublisherEventType.RCL_PUBLISHER_LIVELINESS_LOST, + publisher.publisher_handle) + liveliness_event_index = _rclpy.rclpy_wait_set_add_entity( + 'event', wait_set, liveliness_event_handle) + self.assertIsNotNone(liveliness_event_index) + + # 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)) + + # Calling take data even though not ready should provide me an empty initialized message + # Tests data conversion utilities in C side + event_data = _rclpy.rclpy_take_event( + deadline_event_handle, + publisher.publisher_handle, + QoSPublisherEventType.RCL_PUBLISHER_OFFERED_DEADLINE_MISSED) + self.assertIsInstance(event_data, QoSOfferedDeadlineMissedInfo) + self.assertEqual(event_data.total_count, 0) + self.assertEqual(event_data.total_count_change, 0) + + event_data = _rclpy.rclpy_take_event( + liveliness_event_handle, + publisher.publisher_handle, + QoSPublisherEventType.RCL_PUBLISHER_LIVELINESS_LOST) + self.assertIsInstance(event_data, QoSLivelinessLostInfo) + self.assertEqual(event_data.total_count, 0) + self.assertEqual(event_data.total_count_change, 0) + + self.node.destroy_publisher(publisher) + + 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, 'test_topic', Mock()) + 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) + + deadline_event_handle = _rclpy.rclpy_create_event( + QoSSubscriptionEventType.RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED, + subscription.subscription_handle) + 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) + liveliness_event_index = _rclpy.rclpy_wait_set_add_entity( + 'event', wait_set, liveliness_event_handle) + self.assertIsNotNone(liveliness_event_index) + + # 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)) + + # Calling take data even though not ready should provide me an empty initialized message + # Tests data conversion utilities in C side + event_data = _rclpy.rclpy_take_event( + deadline_event_handle, + subscription.subscription_handle, + QoSSubscriptionEventType.RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED) + self.assertIsInstance(event_data, QoSRequestedDeadlineMissedInfo) + self.assertEqual(event_data.total_count, 0) + self.assertEqual(event_data.total_count_change, 0) + + event_data = _rclpy.rclpy_take_event( + liveliness_event_handle, + subscription.subscription_handle, + QoSSubscriptionEventType.RCL_SUBSCRIPTION_LIVELINESS_CHANGED) + self.assertIsInstance(event_data, QoSLivelinessChangedInfo) + self.assertEqual(event_data.alive_count, 0) + self.assertEqual(event_data.alive_count_change, 0) + self.assertEqual(event_data.not_alive_count, 0) + self.assertEqual(event_data.not_alive_count_change, 0) + + self.node.destroy_subscription(subscription) diff --git a/rclpy/test/test_waitable.py b/rclpy/test/test_waitable.py index fb3596df8..fc2ce2428 100644 --- a/rclpy/test/test_waitable.py +++ b/rclpy/test/test_waitable.py @@ -380,21 +380,23 @@ def test_waitable_with_mutually_exclusive_callback_group(self): class TestNumberOfEntities(unittest.TestCase): def test_add(self): - n1 = NumberOfEntities(1, 2, 3, 4, 5) - n2 = NumberOfEntities(10, 20, 30, 40, 50) + n1 = NumberOfEntities(1, 2, 3, 4, 5, 6) + n2 = NumberOfEntities(10, 20, 30, 40, 50, 60) n = n1 + n2 assert n.num_subscriptions == 11 assert n.num_guard_conditions == 22 assert n.num_timers == 33 assert n.num_clients == 44 assert n.num_services == 55 + assert n.num_events == 66 def test_add_assign(self): - n1 = NumberOfEntities(1, 2, 3, 4, 5) - n2 = NumberOfEntities(10, 20, 30, 40, 50) + n1 = NumberOfEntities(1, 2, 3, 4, 5, 6) + n2 = NumberOfEntities(10, 20, 30, 40, 50, 60) n1 += n2 assert n1.num_subscriptions == 11 assert n1.num_guard_conditions == 22 assert n1.num_timers == 33 assert n1.num_clients == 44 assert n1.num_services == 55 + assert n1.num_events == 66