class QoSEventHandler(Waitable): """Waitable type to handle QoS events.""" def __init__( self, *, callback_group: CallbackGroup, callback: Callable, event_type: IntEnum, parent_handle: 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 with parent_handle as parent_capsule: event_capsule = _rclpy.rclpy_create_event(event_type, parent_capsule) self._event_handle = Handle(event_capsule) 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 with self._parent_handle as parent_capsule, self._event_handle as event_capsule: return _rclpy.rclpy_take_event(event_capsule, parent_capsule, 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 await rclpy.executors.await_or_execute(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.""" with self._event_handle as event_capsule: self._event_index = _rclpy.rclpy_wait_set_add_entity( 'event', wait_set, event_capsule) def destroy(self): self._event_handle.destroy()
def create_publisher( self, msg_type, topic: str, *, qos_profile: QoSProfile = qos_profile_default) -> Publisher: """ Create a new 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. :return: The new publisher. """ # this line imports the typesupport for the message module if not already done check_for_type_support(msg_type) failed = False try: with self.handle as node_capsule: publisher_capsule = _rclpy.rclpy_create_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) publisher_handle.requires(self.handle) publisher = Publisher(publisher_handle, msg_type, topic, qos_profile) self.__publishers.append(publisher) return publisher
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 as clock_capsule: self.__handle = Handle( _rclpy.rclpy_create_timer(clock_capsule, self._context.handle, timer_period_ns)) self.__handle.requires(self._clock.handle) self.timer_period_ns = timer_period_ns self.callback = callback self.callback_group = callback_group # True when the callback is ready to fire but has not been "taken" by an executor self._executor_event = False
def create_subscription(self, msg_type, topic: str, callback: Callable[[MsgType], None], *, qos_profile: QoSProfile = qos_profile_default, callback_group: CallbackGroup = None, raw: bool = False) -> Subscription: """ Create a new subscription. :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 received by the 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 raw: If ``True``, then received messages will be stored in raw binary representation. """ if callback_group is None: callback_group = 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 try: with self.handle as capsule: subscription_capsule = _rclpy.rclpy_create_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) subscription_handle.requires(self.handle) subscription = Subscription(subscription_handle, msg_type, topic, callback, callback_group, qos_profile, raw) self.__subscriptions.append(subscription) callback_group.add_entity(subscription) return subscription
def __new__(cls, *, clock_type=ClockType.SYSTEM_TIME): if not isinstance(clock_type, ClockType): raise TypeError('Clock type must be a ClockType enum') if clock_type is ClockType.ROS_TIME: self = super().__new__(ROSClock) else: self = super().__new__(cls) self.__handle = Handle(_rclpy.rclpy_create_clock(clock_type)) self._clock_type = clock_type return self
def __init__(self, callback, callback_group, timer_period_ns, *, context=None): self._context = get_default_context() if context is None else context # TODO(sloretz) Allow passing clocks in via timer constructor self._clock = Clock(clock_type=ClockType.STEADY_TIME) with self._clock.handle as clock_capsule: self.__handle = Handle( _rclpy.rclpy_create_timer(clock_capsule, self._context.handle, timer_period_ns)) self.__handle.requires(self._clock.handle) self.timer_period_ns = timer_period_ns self.callback = callback self.callback_group = callback_group # True when the callback is ready to fire but has not been "taken" by an executor self._executor_event = False
def __init__(self, callback, callback_group, context=None): self._context = get_default_context() if context is None else context self.__handle = Handle( _rclpy.rclpy_create_guard_condition(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 self._executor_event = False # True when the executor sees this has been triggered but has not yet been handled self._executor_triggered = False
def __init__( self, *, callback_group: CallbackGroup, callback: Callable, event_type: IntEnum, parent_handle: 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 with parent_handle as parent_capsule: event_capsule = _rclpy.rclpy_create_event(event_type, parent_capsule) self._event_handle = Handle(event_capsule) self._ready_to_take_data = False self._event_index = None
def create_service(self, srv_type, srv_name: str, callback: Callable[[SrvTypeRequest, SrvTypeResponse], SrvTypeResponse], *, qos_profile: QoSProfile = qos_profile_services_default, callback_group: CallbackGroup = None) -> Service: """ Create a new service server. :param srv_type: The service type. :param srv_name: The name of the service. :param callback: A user-defined callback function that is called when a service request received by the server. :param qos_profile: The quality of service profile to apply the service server. :param callback_group: The callback group for the service server. If ``None``, then the nodes default callback group is used. """ if callback_group is None: callback_group = self.default_callback_group check_for_type_support(srv_type) failed = False try: with self.handle as node_capsule: service_capsule = _rclpy.rclpy_create_service( node_capsule, srv_type, srv_name, qos_profile.get_c_qos_profile()) except ValueError: failed = True if failed: self._validate_topic_or_service_name(srv_name, is_service=True) service_handle = Handle(service_capsule) service_handle.requires(self.handle) service = Service(service_handle, srv_type, srv_name, callback, callback_group, qos_profile) self.__services.append(service) callback_group.add_entity(service) return service
def create_client(self, srv_type, srv_name: str, *, qos_profile: QoSProfile = qos_profile_services_default, callback_group: CallbackGroup = None) -> Client: """ Create a new service client. :param srv_type: The service type. :param srv_name: The name of the service. :param qos_profile: The quality of service profile to apply the service client. :param callback_group: The callback group for the service client. If ``None``, then the nodes default callback group is used. """ if callback_group is None: callback_group = self.default_callback_group check_for_type_support(srv_type) failed = False try: with self.handle as node_capsule: client_capsule = _rclpy.rclpy_create_client( node_capsule, srv_type, srv_name, qos_profile.get_c_qos_profile()) except ValueError: failed = True if failed: self._validate_topic_or_service_name(srv_name, is_service=True) client_handle = Handle(client_capsule) client_handle.requires(self.handle) client = Client(self.context, client_handle, srv_type, srv_name, qos_profile, callback_group) self.__clients.append(client) callback_group.add_entity(client) return client
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 as clock_capsule: self.__handle = Handle( _rclpy.rclpy_create_timer(clock_capsule, self._context.handle, timer_period_ns)) self.__handle.requires(self._clock.handle) self.timer_period_ns = timer_period_ns self.callback = callback self.callback_group = callback_group # True when the callback is ready to fire but has not been "taken" by an executor self._executor_event = False @property def handle(self): return self.__handle def destroy(self): self.handle.destroy() @property def clock(self): return self._clock @property def timer_period_ns(self): with self.handle as capsule: val = _rclpy.rclpy_get_timer_period(capsule) self._timer_period_ns = val return val @timer_period_ns.setter def timer_period_ns(self, value): val = int(value) with self.handle as capsule: _rclpy.rclpy_change_timer_period(capsule, val) self._timer_period_ns = val def is_ready(self): with self.handle as capsule: return _rclpy.rclpy_is_timer_ready(capsule) def is_canceled(self): with self.handle as capsule: return _rclpy.rclpy_is_timer_canceled(capsule) def cancel(self): with self.handle as capsule: _rclpy.rclpy_cancel_timer(capsule) def reset(self): with self.handle as capsule: _rclpy.rclpy_reset_timer(capsule) def time_since_last_call(self): with self.handle as capsule: return _rclpy.rclpy_time_since_last_call(capsule) def time_until_next_call(self): with self.handle as capsule: return _rclpy.rclpy_time_until_next_call(capsule)
def _create_event_handle(self, parent_entity, event_type): with parent_entity.handle as parent_capsule: event_capsule = _rclpy.rclpy_create_event(event_type, parent_capsule) self.assertIsNotNone(event_capsule) return Handle(event_capsule)
def __init__( self, node_name: str, *, context: Context = None, cli_args: List[str] = None, namespace: str = None, use_global_arguments: bool = True, start_parameter_services: bool = True, initial_parameters: List[Parameter] = None, allow_undeclared_parameters: bool = False, automatically_declare_initial_parameters: bool = True) -> None: """ Constructor. :param node_name: A name to give to this node. Validated by :func:`validate_node_name`. :param context: The context to be associated with, or ``None`` for the default global context. :param cli_args: A list of strings of command line args to be used only by this node. :param namespace: The namespace to which relative topic and service names will be prefixed. Validated by :func:`validate_namespace`. :param use_global_arguments: ``False`` if the node should ignore process-wide command line args. :param start_parameter_services: ``False`` if the node should not create parameter services. :param initial_parameters: A list of parameters to be set during node creation. :param allow_undeclared_parameters: True if undeclared parameters are allowed. This flag affects the behavior of parameter-related operations. :param automatically_declare_initial_parameters: True if initial parameters have to be declared upon node creation, false otherwise. """ self.__handle = None self._context = get_default_context() if context is None else context self._parameters: dict = {} self.__publishers: List[Publisher] = [] self.__subscriptions: List[Subscription] = [] self.__clients: List[Client] = [] self.__services: List[Service] = [] self.__timers: List[WallTimer] = [] self.__guards: List[GuardCondition] = [] self.__waitables: List[Waitable] = [] self._default_callback_group = MutuallyExclusiveCallbackGroup() self._parameters_callback = None self._allow_undeclared_parameters = allow_undeclared_parameters self._initial_parameters = {} self._descriptors = {} namespace = namespace or '' if not self._context.ok(): raise NotInitializedException('cannot create node') try: self.__handle = Handle( _rclpy.rclpy_create_node(node_name, namespace, self._context.handle, cli_args, use_global_arguments)) except ValueError: # these will raise more specific errors if the name or namespace is bad validate_node_name(node_name) # emulate what rcl_node_init() does to accept '' and relative namespaces if not namespace: namespace = '/' if not namespace.startswith('/'): namespace = '/' + namespace 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)) # Clock that has support for ROS time. self._clock = ROSClock() self._time_source = TimeSource(node=self) self._time_source.attach_clock(self._clock) self.__executor_weakref = None self._parameter_event_publisher = self.create_publisher( ParameterEvent, 'parameter_events', qos_profile=qos_profile_parameter_events) with self.handle as capsule: self._initial_parameters = _rclpy.rclpy_get_node_parameters( Parameter, capsule) # 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 initial_parameters is not None: self._initial_parameters.update( {p.name: p for p in initial_parameters}) if automatically_declare_initial_parameters: self._parameters.update(self._initial_parameters) self._descriptors.update( {p: ParameterDescriptor() for p in self._parameters}) if start_parameter_services: self._parameter_service = ParameterService(self)