def test_time_source_not_using_sim_time(self): time_source = TimeSource(node=self.node) clock = ROSClock() time_source.attach_clock(clock) # When not using sim time, ROS time should look like system time now = clock.now() system_now = Clock(clock_type=ClockType.SYSTEM_TIME).now() assert (system_now.nanoseconds - now.nanoseconds) < 1e9 # Presence of clock publisher should not affect the clock self.publish_clock_messages() self.assertFalse(clock.ros_time_is_active) now = clock.now() system_now = Clock(clock_type=ClockType.SYSTEM_TIME).now() assert (system_now.nanoseconds - now.nanoseconds) < 1e9 # Whether or not an attached clock is using ROS time should be determined by the time # source managing it. self.assertFalse(time_source.ros_time_is_active) clock2 = ROSClock() clock2._set_ros_time_is_active(True) time_source.attach_clock(clock2) self.assertFalse(clock2.ros_time_is_active) assert time_source._clock_sub is None
def test_sleep_for_ros_time_enabled(default_context): clock = ROSClock() clock._set_ros_time_is_active(True) start_time = Time(seconds=1, clock_type=ClockType.ROS_TIME) sleep_duration = Duration(seconds=10) stop_time = start_time + sleep_duration clock.set_ros_time_override(start_time) retval = None def run(): nonlocal retval retval = clock.sleep_for(sleep_duration) t = threading.Thread(target=run) t.start() # wait for thread to get inside sleep_for call time.sleep(0.2) clock.set_ros_time_override(stop_time) # wait for thread to exit start = clock.now() t.join() stop = clock.now() assert stop - start < A_SMALL_AMOUNT_OF_TIME assert retval
def test_sleep_for_ros_time_toggled(default_context, ros_time_enabled): clock = ROSClock() clock._set_ros_time_is_active(not ros_time_enabled) retval = None def run(): nonlocal retval retval = clock.sleep_for(Duration(seconds=10)) t = threading.Thread(target=run) t.start() # wait for thread to get inside sleep_for call time.sleep(0.2) clock._set_ros_time_is_active(ros_time_enabled) # wait for thread to exit start = clock.now() t.join() stop = clock.now() assert stop - start < A_SMALL_AMOUNT_OF_TIME assert retval is False
def test_time_source_using_sim_time(self): time_source = TimeSource(node=self.node) clock = ROSClock() time_source.attach_clock(clock) # Setting ROS time active on a time source should also cause attached clocks' use of ROS # time to be set to active. self.assertFalse(time_source.ros_time_is_active) self.assertFalse(clock.ros_time_is_active) time_source.ros_time_is_active = True self.assertTrue(time_source.ros_time_is_active) self.assertTrue(clock.ros_time_is_active) # A subscriber should have been created assert time_source._clock_sub is not None # When using sim time, ROS time should look like the messages received on /clock self.publish_clock_messages() assert clock.now() > Time(seconds=0, clock_type=ClockType.ROS_TIME) assert clock.now() <= Time(seconds=5, clock_type=ClockType.ROS_TIME) # Check that attached clocks get the cached message clock2 = Clock(clock_type=ClockType.ROS_TIME) time_source.attach_clock(clock2) assert clock2.now() > Time(seconds=0, clock_type=ClockType.ROS_TIME) assert clock2.now() <= Time(seconds=5, clock_type=ClockType.ROS_TIME) # Check detaching the node time_source.detach_node() node2 = rclpy.create_node('TestTimeSource2', namespace='/rclpy') time_source.attach_node(node2) node2.destroy_node() assert time_source._node == node2 assert time_source._clock_sub is not None
class Node: def __init__( self, node_name, *, context=None, cli_args=None, namespace=None, use_global_arguments=True, start_parameter_services=True, initial_parameters=None ): self._handle = None self._context = get_default_context() if context is None else context self._parameters = {} self.publishers = [] self.subscriptions = [] self.clients = [] self.services = [] self.timers = [] self.guards = [] self.waitables = [] self._default_callback_group = MutuallyExclusiveCallbackGroup() self._parameters_callback = None namespace = namespace or '' if not self._context.ok(): raise NotInitializedException('cannot create node') try: self._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') self._logger = get_logger(_rclpy.rclpy_get_node_logger_name(self.handle)) # Clock that has support for ROS time. # TODO(dhood): use sim time if parameter has been set on the node. 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) node_parameters = _rclpy.rclpy_get_node_parameters(Parameter, self.handle) # 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: node_parameters.update({p.name: p for p in initial_parameters}) self.set_parameters_atomically(node_parameters.values()) if start_parameter_services: self._parameter_service = ParameterService(self) @property def executor(self): """Get the executor if the node has been added to one, else return None.""" if self.__executor_weakref: return self.__executor_weakref() @executor.setter def executor(self, new_executor): """Set or change the executor the node belongs to.""" current_executor = self.executor if current_executor is not None: current_executor.remove_node(self) if new_executor is None: self.__executor_weakref = None elif new_executor.add_node(self): self.__executor_weakref = weakref.ref(new_executor) @property def context(self): return self._context @property def default_callback_group(self): return self._default_callback_group @property def handle(self): return self._handle @handle.setter def handle(self, value): raise AttributeError('handle cannot be modified after node creation') def get_name(self): return _rclpy.rclpy_get_node_name(self.handle) def get_namespace(self): return _rclpy.rclpy_get_node_namespace(self.handle) def get_clock(self): return self._clock def get_logger(self): return self._logger def get_parameters(self, names): if not all(isinstance(name, str) for name in names): raise TypeError('All names must be instances of type str') return [self.get_parameter(name) for name in names] def get_parameter(self, name): if name not in self._parameters: return Parameter(name, Parameter.Type.NOT_SET, None) return self._parameters[name] def set_parameters(self, parameter_list): results = [] for param in parameter_list: if not isinstance(param, Parameter): raise TypeError("parameter must be instance of type '{}'".format(repr(Parameter))) results.append(self.set_parameters_atomically([param])) return results def set_parameters_atomically(self, parameter_list): result = None if self._parameters_callback: result = self._parameters_callback(parameter_list) else: result = SetParametersResult(successful=True) if result.successful: parameter_event = ParameterEvent() # Add fully qualified path of node to parameter event if self.get_namespace() == '/': parameter_event.node = self.get_namespace() + self.get_name() else: parameter_event.node = self.get_namespace() + '/' + self.get_name() for param in parameter_list: if Parameter.Type.NOT_SET == param.type_: if Parameter.Type.NOT_SET != self.get_parameter(param.name).type_: # Parameter deleted. (Parameter had value and new value is not set) parameter_event.deleted_parameters.append( param.to_parameter_msg()) # Delete any unset parameters regardless of their previous value. # We don't currently store NOT_SET parameters so this is an extra precaution. if param.name in self._parameters: del self._parameters[param.name] else: if Parameter.Type.NOT_SET == self.get_parameter(param.name).type_: # Parameter is new. (Parameter had no value and new value is set) parameter_event.new_parameters.append(param.to_parameter_msg()) else: # Parameter changed. (Parameter had a value and new value is set) parameter_event.changed_parameters.append( param.to_parameter_msg()) self._parameters[param.name] = param parameter_event.stamp = self._clock.now().to_msg() self._parameter_event_publisher.publish(parameter_event) return result def set_parameters_callback(self, callback): self._parameters_callback = callback def _validate_topic_or_service_name(self, topic_or_service_name, *, is_service=False): name = self.get_name() namespace = self.get_namespace() validate_node_name(name) validate_namespace(namespace) validate_topic_name(topic_or_service_name, is_service=is_service) expanded_topic_or_service_name = expand_topic_name(topic_or_service_name, name, namespace) validate_full_topic_name(expanded_topic_or_service_name, is_service=is_service) def add_waitable(self, waitable): """Add a class which itself is capable of add things to the wait set.""" self.waitables.append(waitable) def remove_waitable(self, waitable): """Remove a class which itself is capable of add things to the wait set.""" self.waitables.remove(waitable) def create_publisher(self, msg_type, topic, *, qos_profile=qos_profile_default): # this line imports the typesupport for the message module if not already done check_for_type_support(msg_type) failed = False try: publisher_handle = _rclpy.rclpy_create_publisher( self.handle, msg_type, topic, qos_profile.get_c_qos_profile()) except ValueError: failed = True if failed: self._validate_topic_or_service_name(topic) publisher = Publisher(publisher_handle, msg_type, topic, qos_profile, self.handle) self.publishers.append(publisher) return publisher def create_subscription( self, msg_type, topic, callback, *, qos_profile=qos_profile_default, callback_group=None, raw=False): 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: [subscription_handle, subscription_pointer] = _rclpy.rclpy_create_subscription( self.handle, msg_type, topic, qos_profile.get_c_qos_profile()) except ValueError: failed = True if failed: self._validate_topic_or_service_name(topic) subscription = Subscription( subscription_handle, subscription_pointer, msg_type, topic, callback, callback_group, qos_profile, self.handle, raw) self.subscriptions.append(subscription) callback_group.add_entity(subscription) return subscription def create_client( self, srv_type, srv_name, *, qos_profile=qos_profile_services_default, callback_group=None): if callback_group is None: callback_group = self.default_callback_group check_for_type_support(srv_type) failed = False try: [client_handle, client_pointer] = _rclpy.rclpy_create_client( self.handle, 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 = Client( self.handle, self.context, client_handle, client_pointer, srv_type, srv_name, qos_profile, callback_group) self.clients.append(client) callback_group.add_entity(client) return client def create_service( self, srv_type, srv_name, callback, *, qos_profile=qos_profile_services_default, callback_group=None): if callback_group is None: callback_group = self.default_callback_group check_for_type_support(srv_type) failed = False try: [service_handle, service_pointer] = _rclpy.rclpy_create_service( self.handle, 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 = Service( self.handle, service_handle, service_pointer, srv_type, srv_name, callback, callback_group, qos_profile) self.services.append(service) callback_group.add_entity(service) return service def create_timer(self, timer_period_sec, callback, callback_group=None): timer_period_nsec = int(float(timer_period_sec) * S_TO_NS) if callback_group is None: callback_group = self.default_callback_group timer = WallTimer(callback, callback_group, timer_period_nsec, context=self.context) self.timers.append(timer) callback_group.add_entity(timer) return timer def create_guard_condition(self, callback, callback_group=None): if callback_group is None: callback_group = self.default_callback_group guard = GuardCondition(callback, callback_group, context=self.context) self.guards.append(guard) callback_group.add_entity(guard) return guard def destroy_publisher(self, publisher): for pub in self.publishers: if pub.publisher_handle == publisher.publisher_handle: _rclpy.rclpy_destroy_node_entity(pub.publisher_handle, self.handle) self.publishers.remove(pub) return True return False def destroy_subscription(self, subscription): for sub in self.subscriptions: if sub.subscription_handle == subscription.subscription_handle: _rclpy.rclpy_destroy_node_entity(sub.subscription_handle, self.handle) self.subscriptions.remove(sub) return True return False def destroy_client(self, client): for cli in self.clients: if cli.client_handle == client.client_handle: _rclpy.rclpy_destroy_node_entity(cli.client_handle, self.handle) self.clients.remove(cli) return True return False def destroy_service(self, service): for srv in self.services: if srv.service_handle == service.service_handle: _rclpy.rclpy_destroy_node_entity(srv.service_handle, self.handle) self.services.remove(srv) return True return False def destroy_timer(self, timer): for tmr in self.timers: if tmr.timer_handle == timer.timer_handle: _rclpy.rclpy_destroy_entity(tmr.timer_handle) # TODO(sloretz) Store clocks on node and destroy them separately _rclpy.rclpy_destroy_entity(tmr.clock._clock_handle) self.timers.remove(tmr) return True return False def destroy_guard_condition(self, guard): for gc in self.guards: if gc.guard_handle == guard.guard_handle: _rclpy.rclpy_destroy_entity(gc.guard_handle) self.guards.remove(gc) return True return False def destroy_node(self): ret = True if self.handle is None: return ret # Drop extra reference to parameter event publisher. # It will be destroyed with other publishers below. self._parameter_event_publisher = None while self.publishers: pub = self.publishers.pop() _rclpy.rclpy_destroy_node_entity(pub.publisher_handle, self.handle) while self.subscriptions: sub = self.subscriptions.pop() _rclpy.rclpy_destroy_node_entity(sub.subscription_handle, self.handle) while self.clients: cli = self.clients.pop() _rclpy.rclpy_destroy_node_entity(cli.client_handle, self.handle) while self.services: srv = self.services.pop() _rclpy.rclpy_destroy_node_entity(srv.service_handle, self.handle) while self.timers: tmr = self.timers.pop() _rclpy.rclpy_destroy_entity(tmr.timer_handle) # TODO(sloretz) Store clocks on node and destroy them separately _rclpy.rclpy_destroy_entity(tmr.clock._clock_handle) while self.guards: gc = self.guards.pop() _rclpy.rclpy_destroy_entity(gc.guard_handle) _rclpy.rclpy_destroy_entity(self.handle) self._handle = None return ret def get_publisher_names_and_types_by_node(self, node_name, node_namespace, no_demangle=False): return _rclpy.rclpy_get_publisher_names_and_types_by_node( self.handle, no_demangle, node_name, node_namespace) def get_subscriber_names_and_types_by_node(self, node_name, node_namespace, no_demangle=False): return _rclpy.rclpy_get_subscriber_names_and_types_by_node( self.handle, no_demangle, node_name, node_namespace) def get_service_names_and_types_by_node(self, node_name, node_namespace): return _rclpy.rclpy_get_service_names_and_types_by_node( self.handle, node_name, node_namespace) def get_topic_names_and_types(self, no_demangle=False): return _rclpy.rclpy_get_topic_names_and_types(self.handle, no_demangle) def get_service_names_and_types(self): return _rclpy.rclpy_get_service_names_and_types(self.handle) def get_node_names(self): names_ns = _rclpy.rclpy_get_node_names_and_namespaces(self.handle) return [n[0] for n in names_ns] def get_node_names_and_namespaces(self): return _rclpy.rclpy_get_node_names_and_namespaces(self.handle) def _count_publishers_or_subscribers(self, topic_name, func): fq_topic_name = expand_topic_name(topic_name, self.get_name(), self.get_namespace()) validate_topic_name(fq_topic_name) return func(self.handle, fq_topic_name) def count_publishers(self, topic_name): """ Return the number of publishers on a given topic. `topic_name` may be a relative, private, or fully qualifed topic name. A relative or private topic is expanded using this node's namespace and name. The queried topic name is not remapped. :param topic_name: the topic_name on which to count the number of publishers. :type topic_name: str :return: the number of publishers on the topic. """ return self._count_publishers_or_subscribers(topic_name, _rclpy.rclpy_count_publishers) def count_subscribers(self, topic_name): """ Return the number of subscribers on a given topic. `topic_name` may be a relative, private, or fully qualifed topic name. A relative or private topic is expanded using this node's namespace and name. The queried topic name is not remapped. :param topic_name: the topic_name on which to count the number of subscribers. :type topic_name: str :return: the number of subscribers on the topic. """ return self._count_publishers_or_subscribers(topic_name, _rclpy.rclpy_count_subscribers) def __del__(self): self.destroy_node()
class Node: """ A Node in the ROS graph. A Node is the primary entrypoint in a ROS system for communication. It can be used to create ROS entities such as publishers, subscribers, services, etc. """ 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) @property def publishers(self) -> Iterator[Publisher]: """Get publishers that have been created on this node.""" yield from self.__publishers @property def subscriptions(self) -> Iterator[Subscription]: """Get subscriptions that have been created on this node.""" yield from self.__subscriptions @property def clients(self) -> Iterator[Client]: """Get clients that have been created on this node.""" yield from self.__clients @property def services(self) -> Iterator[Service]: """Get services that have been created on this node.""" yield from self.__services @property def timers(self) -> Iterator[WallTimer]: """Get timers that have been created on this node.""" yield from self.__timers @property def guards(self) -> Iterator[GuardCondition]: """Get guards that have been created on this node.""" yield from self.__guards @property def waitables(self) -> Iterator[Waitable]: """Get waitables that have been created on this node.""" yield from self.__waitables @property def executor(self) -> Optional[Executor]: """Get the executor if the node has been added to one, else return ``None``.""" if self.__executor_weakref: return self.__executor_weakref() return None @executor.setter def executor(self, new_executor: Executor) -> None: """Set or change the executor the node belongs to.""" current_executor = self.executor if current_executor == new_executor: return if current_executor is not None: current_executor.remove_node(self) if new_executor is None: self.__executor_weakref = None else: new_executor.add_node(self) self.__executor_weakref = weakref.ref(new_executor) @property def context(self) -> Context: """Get the context associated with the node.""" return self._context @property def default_callback_group(self) -> CallbackGroup: """ Get the default callback group. If no other callback group is provided when the a ROS entity is created with the node, then it is added to the default callback group. """ return self._default_callback_group @property def handle(self): """ Get the handle to the underlying `rcl_node_t`. Cannot be modified after node creation. :raises: AttributeError if modified after creation. """ return self.__handle @handle.setter def handle(self, value): raise AttributeError('handle cannot be modified after node creation') def get_name(self) -> str: """Get the name of the node.""" with self.handle as capsule: return _rclpy.rclpy_get_node_name(capsule) def get_namespace(self) -> str: """Get the namespace of the node.""" with self.handle as capsule: return _rclpy.rclpy_get_node_namespace(capsule) def get_clock(self) -> Clock: """Get the clock used by the node.""" return self._clock def get_logger(self): """Get the nodes logger.""" return self._logger def declare_parameter( self, name: str, value: ParameterValue = ParameterValue(), descriptor: ParameterDescriptor = ParameterDescriptor() ) -> Parameter: """ Declare and initialize a parameter. This method, if successful, will result in any callback registered with :func:`set_parameters_callback` to be called. :param name: Fully-qualified name of the parameter, including its namespace. :param value: Value of the parameter to declare. :param descriptor: Descriptor for the parameter to declare. :return: Parameter with the effectively assigned value. :raises: ParameterAlreadyDeclaredException if the parameter had already been declared. :raises: InvalidParameterException if the parameter name is invalid. :raises: InvalidParameterValueException if the registered callback rejects the parameter. """ return self.declare_parameters('', [(name, value, descriptor)])[0] def declare_parameters( self, namespace: str, parameters: List[Tuple[str, Optional[ParameterValue], Optional[ParameterDescriptor]]] ) -> List[Parameter]: """ Declare a list of parameters. This method, if successful, will result in any callback registered with :func:`set_parameters_callback` to be called once for each parameter. If one of those calls fail, an exception will be raised and the remaining parameters will not be declared. Parameters declared up to that point will not be undeclared. :param namespace: Namespace for parameters. :param parameters: Tuple with parameters to declare, with a name, value and descriptor. :return: Parameter list with the effectively assigned values for each of them. :raises: ParameterAlreadyDeclaredException if the parameter had already been declared. :raises: InvalidParameterException if the parameter name is invalid. :raises: InvalidParameterValueException if the registered callback rejects any parameter. """ parameter_list = [] descriptor_list = [] for parameter_tuple in parameters: name = parameter_tuple[0] assert isinstance(name, str) # Get value from initial parameters, of from tuple if it doesn't exist. if name in self._initial_parameters: value = self._initial_parameters[name].get_parameter_value() elif parameter_tuple[1] is None: value = ParameterValue() else: value = parameter_tuple[1] assert isinstance(value, ParameterValue) descriptor = parameter_tuple[2] if descriptor is None: descriptor = ParameterDescriptor() assert isinstance(descriptor, ParameterDescriptor) # Note(jubeira): declare_parameters verifies the name, but set_parameters doesn't. full_name = namespace + name validate_parameter_name(full_name) parameter_list.append( Parameter.from_parameter_msg( ParameterMsg(name=full_name, value=value))) descriptor_list.append(descriptor) parameters_already_declared = [ parameter.name for parameter in parameter_list if parameter.name in self._parameters ] if any(parameters_already_declared): raise ParameterAlreadyDeclaredException( parameters_already_declared) # Call the callback once for each of the parameters, using method that doesn't # check whether the parameter was declared beforehand or not. self._set_parameters(parameter_list, descriptor_list=descriptor_list, raise_on_failure=True) return self.get_parameters( [parameter.name for parameter in parameter_list]) def undeclare_parameter(self, name: str): """ Undeclare a previously declared parameter. This method will not cause a callback registered with :func:`set_parameters_callback` to be called. :param name: Fully-qualified name of the parameter, including its namespace. :raises: ParameterNotDeclaredException if parameter had not been declared before. :raises: ParameterImmutableException if the parameter was created as read-only. """ if self.has_parameter(name): if self._descriptors[name].read_only: raise ParameterImmutableException(name) else: del self._parameters[name] del self._descriptors[name] else: raise ParameterNotDeclaredException(name) def has_parameter(self, name: str) -> bool: """Return True if parameter is declared; False otherwise.""" return name in self._parameters def get_parameters(self, names: List[str]) -> List[Parameter]: """ Get a list of parameters. :param names: Fully-qualified names of the parameters to get, including their namespaces. :return: The values for the given parameter names. A default Parameter will be returned for undeclared parameters if undeclared parameters are allowed. :raises: ParameterNotDeclaredException if undeclared parameters are not allowed, and at least one parameter hadn't been declared beforehand. """ if not all(isinstance(name, str) for name in names): raise TypeError('All names must be instances of type str') return [self.get_parameter(name) for name in names] def get_parameter(self, name: str) -> Parameter: """ Get a parameter by name. :param name: Fully-qualified name of the parameter, including its namespace. :return: The values for the given parameter names. A default Parameter will be returned for an undeclared parameter if undeclared parameters are allowed. :raises: ParameterNotDeclaredException if undeclared parameters are not allowed, and the parameter hadn't been declared beforehand. """ if self.has_parameter(name): return self._parameters[name] elif self._allow_undeclared_parameters: # If undeclared parameters are allowed, the parameter might be in the initial set. # If that's the case, first set and then return. if name in self._initial_parameters: self._parameters.update({name: self._initial_parameters[name]}) self._descriptors.update({name: ParameterDescriptor()}) return self._parameters[name] return Parameter(name, Parameter.Type.NOT_SET, None) else: raise ParameterNotDeclaredException(name) def get_parameter_or( self, name: str, alternative_value: Optional[Parameter] = None) -> Parameter: """ Get a parameter or the alternative value. If the alternative value is None, a default Parameter with the given name and NOT_SET type will be returned. This method does not declare parameters in any case. :param name: Fully-qualified name of the parameter, including its namespace. :param alternative_value: Alternative parameter to get if it had not been declared before. :return: Requested parameter, or alternative value if it hadn't been declared before. """ if alternative_value is None: alternative_value = Parameter(name, Parameter.Type.NOT_SET) return self._parameters.get(name, alternative_value) def set_parameters( self, parameter_list: List[Parameter]) -> List[SetParametersResult]: """ Set parameters for the node, and return the result for the set action. If any parameter in the list was not declared beforehand and undeclared parameters are not allowed for the node, this method will raise a ParameterNotDeclaredException exception. Parameters are set in the order they are declared in the list. If setting a parameter fails due to not being declared, then the parameters which have already been set will stay set, and no attempt will be made to set the parameters which come after. If undeclared parameters are allowed, then all the parameters will be implicitly declared before being set even if they were not declared beforehand. If a callback was registered previously with :func:`set_parameters_callback`, it will be called prior to setting the parameters for the node, once for each parameter. If the callback prevents a parameter from being set, then it will be reflected in the returned result; no exceptions will be raised in this case. For each successfully set parameter, a :class:`ParameterEvent` message is published. If the value type of the parameter is NOT_SET, and the existing parameter type is something else, then the parameter will be implicitly undeclared. :param parameter_list: The list of parameters to set. :return: The result for each set action as a list. :raises: ParameterNotDeclaredException if undeclared parameters are not allowed, and at least one parameter in the list hadn't been declared beforehand. """ self._check_undeclared_parameters(parameter_list) return self._set_parameters(parameter_list) def _set_parameters(self, parameter_list: List[Parameter], descriptor_list: Optional[ List[ParameterDescriptor]] = None, raise_on_failure=False) -> List[SetParametersResult]: """ Set parameters for the node, and return the result for the set action. Method for internal usage; applies a setter method for each parameters in the list. By default it doesn't check if the parameters were declared, and both declares and sets the given list. If a callback was registered previously with :func:`set_parameters_callback`, it will be called prior to setting the parameters for the node, once for each parameter. If the callback doesn't succeed for a given parameter, it won't be set and either an unsuccessful result will be returned for that parameter, or an exception will be raised according to `raise_on_failure` flag. :param parameter_list: List of parameters to set. :return: The result for each set action as a list. :raises: InvalidParameterValueException if the user-defined callback rejects the parameter value and raise_on_failure flag is True. """ if descriptor_list is not None: assert len(descriptor_list) == len(parameter_list) results = [] for index, param in enumerate(parameter_list): result = self._set_parameters_atomically([param]) if raise_on_failure and not result.successful: raise InvalidParameterValueException(param.name, param.value) results.append(result) if descriptor_list is not None: self._descriptors[param.name] = descriptor_list[index] return results def set_parameters_atomically( self, parameter_list: List[Parameter]) -> SetParametersResult: """ Set the given parameters, all at one time, and then aggregate result. If any parameter in the list was not declared beforehand and undeclared parameters are not allowed for the node, this method will raise a ParameterNotDeclaredException exception. Parameters are set all at once. If setting a parameter fails due to not being declared, then no parameter will be set set. Either all of the parameters are set or none of them are set. If undeclared parameters are allowed, then all the parameters will be implicitly declared before being set even if they were not declared beforehand. If a callback was registered previously with :func:`set_parameters_callback`, it will be called prior to setting the parameters for the node only once for all parameters. If the callback prevents the parameters from being set, then it will be reflected in the returned result; no exceptions will be raised in this case. For each successfully set parameter, a :class:`ParameterEvent` message is published. If the value type of the parameter is NOT_SET, and the existing parameter type is something else, then the parameter will be implicitly undeclared. :param parameter_list: The list of parameters to set. :return: Aggregate result of setting all the parameters atomically. :raises: ParameterNotDeclaredException if undeclared parameters are not allowed, and at least one parameter in the list hadn't been declared beforehand. """ self._check_undeclared_parameters(parameter_list) return self._set_parameters_atomically(parameter_list) def _check_undeclared_parameters(self, parameter_list: List[Parameter]): """ Check if parameter list has correct types and was declared beforehand. :raises: ParameterNotDeclaredException if at least one parameter in the list was not declared beforehand. """ if not all( isinstance(parameter, Parameter) for parameter in parameter_list): raise TypeError("parameter must be instance of type '{}'".format( repr(Parameter))) undeclared_parameters = (param.name for param in parameter_list if param.name not in self._parameters) if (not self._allow_undeclared_parameters and any(undeclared_parameters)): raise ParameterNotDeclaredException(list(undeclared_parameters)) def _set_parameters_atomically( self, parameter_list: List[Parameter]) -> SetParametersResult: """ Set the given parameters, all at one time, and then aggregate result. This method does not check if the parameters were declared beforehand, and is intended for internal use of this class. If a callback was registered previously with :func:`set_parameters_callback`, it will be called prior to setting the parameters for the node only once for all parameters. If the callback prevents the parameters from being set, then it will be reflected in the returned result; no exceptions will be raised in this case. For each successfully set parameter, a :class:`ParameterEvent` message is published. If the value type of the parameter is NOT_SET, and the existing parameter type is something else, then the parameter will be implicitly undeclared. :param parameter_list: The list of parameters to set. :return: Aggregate result of setting all the parameters atomically. """ result = None if self._parameters_callback: result = self._parameters_callback(parameter_list) else: result = SetParametersResult(successful=True) if result.successful: parameter_event = ParameterEvent() # Add fully qualified path of node to parameter event if self.get_namespace() == '/': parameter_event.node = self.get_namespace() + self.get_name() else: parameter_event.node = self.get_namespace( ) + '/' + self.get_name() for param in parameter_list: if Parameter.Type.NOT_SET == param.type_: if Parameter.Type.NOT_SET != self.get_parameter_or( param.name).type_: # Parameter deleted. (Parameter had value and new value is not set) parameter_event.deleted_parameters.append( param.to_parameter_msg()) # Delete any unset parameters regardless of their previous value. # We don't currently store NOT_SET parameters so this is an extra precaution. if param.name in self._parameters: del self._parameters[param.name] if param.name in self._descriptors: del self._descriptors[param.name] else: if Parameter.Type.NOT_SET == self.get_parameter_or( param.name).type_: # Parameter is new. (Parameter had no value and new value is set) parameter_event.new_parameters.append( param.to_parameter_msg()) else: # Parameter changed. (Parameter had a value and new value is set) parameter_event.changed_parameters.append( param.to_parameter_msg()) self._parameters[param.name] = param parameter_event.stamp = self._clock.now().to_msg() self._parameter_event_publisher.publish(parameter_event) return result def describe_parameter(self, name: str) -> ParameterDescriptor: """ Get the parameter descriptor of a given parameter. :param name: Fully-qualified name of the parameter, including its namespace. :return: ParameterDescriptor corresponding to the parameter, or default ParameterDescriptor if parameter had not been declared before and undeclared parameters are allowed. :raises: ParameterNotDeclaredException if parameter had not been declared before and undeclared parameters are not allowed. """ try: return self._descriptors[name] except KeyError: if self._allow_undeclared_parameters: return ParameterDescriptor() else: raise ParameterNotDeclaredException(name) def describe_parameters(self, names: List[str]) -> List[ParameterDescriptor]: """ Get the parameter descriptors of a given list of parameters. :param name: List of fully-qualified names of the parameters to describe. :return: List of ParameterDescriptors corresponding to the given parameters. Default ParameterDescriptors shall be returned for parameters that had not been declared before if undeclared parameters are allowed. :raises: ParameterNotDeclaredException if at least one parameter had not been declared before and undeclared parameters are not allowed. """ parameter_descriptors = [] for name in names: parameter_descriptors.append(self.describe_parameter(name)) return parameter_descriptors def set_parameters_callback( self, callback: Callable[[List[Parameter]], SetParametersResult]) -> None: """ Register a set parameters callback. Calling this function with override any previously registered callback. :param callback: The function that is called whenever parameters are set for the node. """ self._parameters_callback = callback def _validate_topic_or_service_name(self, topic_or_service_name, *, is_service=False): name = self.get_name() namespace = self.get_namespace() validate_node_name(name) validate_namespace(namespace) validate_topic_name(topic_or_service_name, is_service=is_service) expanded_topic_or_service_name = expand_topic_name( topic_or_service_name, name, namespace) validate_full_topic_name(expanded_topic_or_service_name, is_service=is_service) def add_waitable(self, waitable: Waitable) -> None: """ Add a class that is capable of adding things to the wait set. :param waitable: An instance of a waitable that the node will add to the waitset. """ self.__waitables.append(waitable) def remove_waitable(self, waitable: Waitable) -> None: """ Remove a Waitable that was previously added to the node. :param waitable: The Waitable to remove. """ self.__waitables.remove(waitable) 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 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 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 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_timer(self, timer_period_sec: float, callback: Callable, callback_group: CallbackGroup = None) -> WallTimer: """ Create a new timer. The timer will be started and every ``timer_period_sec`` number of seconds the provided callback function will be called. :param timer_period_sec: The period (s) of the timer. :param callback: A user-defined callback function that is called when the timer expires. :param callback_group: The callback group for the timer. If ``None``, then the nodes default callback group is used. """ timer_period_nsec = int(float(timer_period_sec) * S_TO_NS) if callback_group is None: callback_group = self.default_callback_group timer = WallTimer(callback, callback_group, timer_period_nsec, context=self.context) timer.handle.requires(self.handle) self.__timers.append(timer) callback_group.add_entity(timer) return timer def create_guard_condition( self, callback: Callable, callback_group: CallbackGroup = None) -> GuardCondition: """Create a new guard condition.""" if callback_group is None: callback_group = self.default_callback_group guard = GuardCondition(callback, callback_group, context=self.context) guard.handle.requires(self.handle) self.__guards.append(guard) callback_group.add_entity(guard) return guard def destroy_publisher(self, publisher: Publisher) -> bool: """ Destroy a publisher created by the node. :return: ``True`` if successful, ``False`` otherwise. """ if publisher in self.__publishers: self.__publishers.remove(publisher) try: publisher.destroy() except InvalidHandle: return False return True return False def destroy_subscription(self, subscription: Subscription) -> bool: """ Destroy a subscription created by the node. :return: ``True`` if succesful, ``False`` otherwise. """ if subscription in self.__subscriptions: self.__subscriptions.remove(subscription) try: subscription.destroy() except InvalidHandle: return False return True return False def destroy_client(self, client: Client) -> bool: """ Destroy a service client created by the node. :return: ``True`` if successful, ``False`` otherwise. """ if client in self.__clients: self.__clients.remove(client) try: client.destroy() except InvalidHandle: return False return True return False def destroy_service(self, service: Service) -> bool: """ Destroy a service server created by the node. :return: ``True`` if successful, ``False`` otherwise. """ if service in self.__services: self.__services.remove(service) try: service.destroy() except InvalidHandle: return False return True return False def destroy_timer(self, timer: WallTimer) -> bool: """ Destroy a timer created by the node. :return: ``True`` if successful, ``False`` otherwise. """ if timer in self.__timers: self.__timers.remove(timer) try: timer.destroy() except InvalidHandle: return False return True return False def destroy_guard_condition(self, guard: GuardCondition) -> bool: """ Destroy a guard condition created by the node. :return: ``True`` if successful, ``False`` otherwise. """ if guard in self.__guards: self.__guards.remove(guard) try: guard.destroy() except InvalidHandle: return False return True return False def destroy_node(self) -> bool: """ Destroy the node. Frees resources used by the node, including any entities created by the following methods: * :func:`create_publisher` * :func:`create_subscription` * :func:`create_client` * :func:`create_service` * :func:`create_timer` * :func:`create_guard_condition` """ # Drop extra reference to parameter event publisher. # It will be destroyed with other publishers below. self._parameter_event_publisher = None self.__publishers.clear() self.__subscriptions.clear() self.__clients.clear() self.__services.clear() self.__timers.clear() self.__guards.clear() self.handle.destroy() def get_publisher_names_and_types_by_node( self, node_name: str, node_namespace: str, no_demangle: bool = False) -> List[Tuple[str, str]]: """ Get a list of discovered topics for publishers of a remote node. :param node_name: Name of a remote node to get publishers for. :param node_namespace: Namespace of the remote node. :param no_demangle: If ``True``, then topic names and types returned will not be demangled. :return: List of tuples containing two strings: the topic name and topic type. """ with self.handle as capsule: return _rclpy.rclpy_get_publisher_names_and_types_by_node( capsule, no_demangle, node_name, node_namespace) def get_subscriber_names_and_types_by_node( self, node_name: str, node_namespace: str, no_demangle: bool = False) -> List[Tuple[str, str]]: """ Get a list of discovered topics for subscriptions of a remote node. :param node_name: Name of a remote node to get subscriptions for. :param node_namespace: Namespace of the remote node. :param no_demangle: If ``True``, then topic names and types returned will not be demangled. :return: List of tuples containing two strings: the topic name and topic type. """ with self.handle as capsule: return _rclpy.rclpy_get_subscriber_names_and_types_by_node( capsule, no_demangle, node_name, node_namespace) def get_service_names_and_types_by_node( self, node_name: str, node_namespace: str) -> List[Tuple[str, str]]: """ Get a list of discovered service topics for a remote node. :param node_name: Name of a remote node to get services for. :param node_namespace: Namespace of the remote node. :return: List of tuples containing two strings: the service name and service type. """ with self.handle as capsule: return _rclpy.rclpy_get_service_names_and_types_by_node( capsule, node_name, node_namespace) def get_topic_names_and_types(self, no_demangle: bool = False ) -> List[Tuple[str, str]]: """ Get a list topic names and types for the node. :param no_demangle: If ``True``, then topic names and types returned will not be demangled. :return: List of tuples containing two strings: the topic name and topic type. """ with self.handle as capsule: return _rclpy.rclpy_get_topic_names_and_types(capsule, no_demangle) def get_service_names_and_types(self) -> List[Tuple[str, str]]: """ Get a list of service topics for the node. :return: List of tuples containing two strings: the service name and service type. """ with self.handle as capsule: return _rclpy.rclpy_get_service_names_and_types(capsule) def get_node_names(self) -> List[str]: """ Get a list of names for discovered nodes. :return: List of node names. """ with self.handle as capsule: names_ns = _rclpy.rclpy_get_node_names_and_namespaces(capsule) return [n[0] for n in names_ns] def get_node_names_and_namespaces(self) -> List[Tuple[str, str]]: """ Get a list of names and namespaces for discovered nodes. :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) def _count_publishers_or_subscribers(self, topic_name, func): fq_topic_name = expand_topic_name(topic_name, self.get_name(), self.get_namespace()) validate_topic_name(fq_topic_name) with self.handle as node_capsule: return func(node_capsule, fq_topic_name) def count_publishers(self, topic_name: str) -> int: """ Return the number of publishers on a given topic. `topic_name` may be a relative, private, or fully qualifed topic name. A relative or private topic is expanded using this node's namespace and name. The queried topic name is not remapped. :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) def count_subscribers(self, topic_name: str) -> int: """ Return the number of subscribers on a given topic. `topic_name` may be a relative, private, or fully qualifed topic name. A relative or private topic is expanded using this node's namespace and name. The queried topic name is not remapped. :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) def assert_liveliness(self) -> None: """ Manually assert that this Node is alive. If the QoS Liveliness policy is set to RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE, the application must call this at least as often as ``QoSProfile.liveliness_lease_duration``. """ _rclpy.rclpy_assert_liveliness(self.node_handle)
class Node: """ A Node in the ROS graph. A Node is the primary entrypoint in a ROS system for communication. It can be used to create ROS entities such as publishers, subscribers, services, etc. """ 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) -> 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. """ 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 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: node_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: node_parameters.update({p.name: p for p in initial_parameters}) self.set_parameters_atomically(node_parameters.values()) if start_parameter_services: self._parameter_service = ParameterService(self) @property def publishers(self) -> Iterator[Publisher]: """Get publishers that have been created on this node.""" yield from self.__publishers @property def subscriptions(self) -> Iterator[Subscription]: """Get subscriptions that have been created on this node.""" yield from self.__subscriptions @property def clients(self) -> Iterator[Client]: """Get clients that have been created on this node.""" yield from self.__clients @property def services(self) -> Iterator[Service]: """Get services that have been created on this node.""" yield from self.__services @property def timers(self) -> Iterator[WallTimer]: """Get timers that have been created on this node.""" yield from self.__timers @property def guards(self) -> Iterator[GuardCondition]: """Get guards that have been created on this node.""" yield from self.__guards @property def waitables(self) -> Iterator[Waitable]: """Get waitables that have been created on this node.""" yield from self.__waitables @property def executor(self) -> Optional[Executor]: """Get the executor if the node has been added to one, else return ``None``.""" if self.__executor_weakref: return self.__executor_weakref() return None @executor.setter def executor(self, new_executor: Executor) -> None: """Set or change the executor the node belongs to.""" current_executor = self.executor if current_executor == new_executor: return if current_executor is not None: current_executor.remove_node(self) if new_executor is None: self.__executor_weakref = None else: new_executor.add_node(self) self.__executor_weakref = weakref.ref(new_executor) @property def context(self) -> Context: """Get the context associated with the node.""" return self._context @property def default_callback_group(self) -> CallbackGroup: """ Get the default callback group. If no other callback group is provided when the a ROS entity is created with the node, then it is added to the default callback group. """ return self._default_callback_group @property def handle(self): """ Get the handle to the underlying `rcl_node_t`. Cannot be modified after node creation. :raises AttributeError: if modified after creation. """ return self.__handle @handle.setter def handle(self, value): raise AttributeError('handle cannot be modified after node creation') def get_name(self) -> str: """Get the name of the node.""" with self.handle as capsule: return _rclpy.rclpy_get_node_name(capsule) def get_namespace(self) -> str: """Get the namespace of the node.""" with self.handle as capsule: return _rclpy.rclpy_get_node_namespace(capsule) def get_clock(self) -> Clock: """Get the clock used by the node.""" return self._clock def get_logger(self): """Get the nodes logger.""" return self._logger def get_parameters(self, names: List[str]) -> List[Parameter]: """ Get a list of parameters. :param names: The names of the parameters to get. :return: The values for the given parameter names. """ if not all(isinstance(name, str) for name in names): raise TypeError('All names must be instances of type str') return [self.get_parameter(name) for name in names] def get_parameter(self, name: str) -> Parameter: """ Get a parameter by name. :param name: The name of the parameter. :return: The value of the parameter. """ if name not in self._parameters: return Parameter(name, Parameter.Type.NOT_SET, None) return self._parameters[name] def set_parameters( self, parameter_list: List[Parameter]) -> List[SetParametersResult]: """ Set parameters for the node. If a callback was registered previously with :func:`set_parameters_callback`, it will be called prior to setting the parameters for the node. For each successfully set parameter, a :class:`ParameterEvent` message is published. :param parameter_list: The list of parameters to set. :return: A list of SetParametersResult messages. """ results = [] for param in parameter_list: if not isinstance(param, Parameter): raise TypeError( "parameter must be instance of type '{}'".format( repr(Parameter))) results.append(self.set_parameters_atomically([param])) return results def set_parameters_atomically( self, parameter_list: List[Parameter]) -> SetParametersResult: """ Atomically set parameters for the node. If a callback was registered previously with :func:`set_parameters_callback`, it will be called prior to setting the parameters for the node. If the parameters are set successfully, a :class:`ParameterEvent` message is published. :param parameter_list: The list of parameters to set. """ result = None if self._parameters_callback: result = self._parameters_callback(parameter_list) else: result = SetParametersResult(successful=True) if result.successful: parameter_event = ParameterEvent() # Add fully qualified path of node to parameter event if self.get_namespace() == '/': parameter_event.node = self.get_namespace() + self.get_name() else: parameter_event.node = self.get_namespace( ) + '/' + self.get_name() for param in parameter_list: if Parameter.Type.NOT_SET == param.type_: if Parameter.Type.NOT_SET != self.get_parameter( param.name).type_: # Parameter deleted. (Parameter had value and new value is not set) parameter_event.deleted_parameters.append( param.to_parameter_msg()) # Delete any unset parameters regardless of their previous value. # We don't currently store NOT_SET parameters so this is an extra precaution. if param.name in self._parameters: del self._parameters[param.name] else: if Parameter.Type.NOT_SET == self.get_parameter( param.name).type_: # Parameter is new. (Parameter had no value and new value is set) parameter_event.new_parameters.append( param.to_parameter_msg()) else: # Parameter changed. (Parameter had a value and new value is set) parameter_event.changed_parameters.append( param.to_parameter_msg()) self._parameters[param.name] = param parameter_event.stamp = self._clock.now().to_msg() self._parameter_event_publisher.publish(parameter_event) return result def set_parameters_callback( self, callback: Callable[[List[Parameter]], SetParametersResult]) -> None: """ Register a set parameters callback. Calling this function with override any previously registered callback. :param callback: The function that is called whenever parameters are set for the node. """ self._parameters_callback = callback def _validate_topic_or_service_name(self, topic_or_service_name, *, is_service=False): name = self.get_name() namespace = self.get_namespace() validate_node_name(name) validate_namespace(namespace) validate_topic_name(topic_or_service_name, is_service=is_service) expanded_topic_or_service_name = expand_topic_name( topic_or_service_name, name, namespace) validate_full_topic_name(expanded_topic_or_service_name, is_service=is_service) def add_waitable(self, waitable: Waitable) -> None: """ Add a class that is capable of adding things to the wait set. :param waitable: An instance of a waitable that the node will add to the waitset. """ self.__waitables.append(waitable) def remove_waitable(self, waitable: Waitable) -> None: """ Remove a Waitable that was previously added to the node. :param waitable: The Waitable to remove. """ self.__waitables.remove(waitable) 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 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 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 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_timer(self, timer_period_sec: float, callback: Callable, callback_group: CallbackGroup = None) -> WallTimer: """ Create a new timer. The timer will be started and every ``timer_period_sec`` number of seconds the provided callback function will be called. :param timer_period_sec: The period (s) of the timer. :param callback: A user-defined callback function that is called when the timer expires. :param callback_group: The callback group for the timer. If ``None``, then the nodes default callback group is used. """ timer_period_nsec = int(float(timer_period_sec) * S_TO_NS) if callback_group is None: callback_group = self.default_callback_group timer = WallTimer(callback, callback_group, timer_period_nsec, context=self.context) timer.handle.requires(self.handle) self.__timers.append(timer) callback_group.add_entity(timer) return timer def create_guard_condition( self, callback: Callable, callback_group: CallbackGroup = None) -> GuardCondition: """Create a new guard condition.""" if callback_group is None: callback_group = self.default_callback_group guard = GuardCondition(callback, callback_group, context=self.context) guard.handle.requires(self.handle) self.__guards.append(guard) callback_group.add_entity(guard) return guard def destroy_publisher(self, publisher: Publisher) -> bool: """ Destroy a publisher created by the node. :return: ``True`` if successful, ``False`` otherwise. """ if publisher in self.__publishers: self.__publishers.remove(publisher) try: publisher.destroy() except InvalidHandle: return False return True return False def destroy_subscription(self, subscription: Subscription) -> bool: """ Destroy a subscription created by the node. :return: ``True`` if succesful, ``False`` otherwise. """ if subscription in self.__subscriptions: self.__subscriptions.remove(subscription) try: subscription.destroy() except InvalidHandle: return False return True return False def destroy_client(self, client: Client) -> bool: """ Destroy a service client created by the node. :return: ``True`` if successful, ``False`` otherwise. """ if client in self.__clients: self.__clients.remove(client) try: client.destroy() except InvalidHandle: return False return True return False def destroy_service(self, service: Service) -> bool: """ Destroy a service server created by the node. :return: ``True`` if successful, ``False`` otherwise. """ if service in self.__services: self.__services.remove(service) try: service.destroy() except InvalidHandle: return False return True return False def destroy_timer(self, timer: WallTimer) -> bool: """ Destroy a timer created by the node. :return: ``True`` if successful, ``False`` otherwise. """ if timer in self.__timers: self.__timers.remove(timer) try: timer.destroy() except InvalidHandle: return False return True return False def destroy_guard_condition(self, guard: GuardCondition) -> bool: """ Destroy a guard condition created by the node. :return: ``True`` if successful, ``False`` otherwise. """ if guard in self.__guards: self.__guards.remove(guard) try: guard.destroy() except InvalidHandle: return False return True return False def destroy_node(self) -> bool: """ Destroy the node. Frees resources used by the node, including any entities created by the following methods: * :func:`create_publisher` * :func:`create_subscription` * :func:`create_client` * :func:`create_service` * :func:`create_timer` * :func:`create_guard_condition` """ # Drop extra reference to parameter event publisher. # It will be destroyed with other publishers below. self._parameter_event_publisher = None self.__publishers.clear() self.__subscriptions.clear() self.__clients.clear() self.__services.clear() self.__timers.clear() self.__guards.clear() self.handle.destroy() def get_publisher_names_and_types_by_node( self, node_name: str, node_namespace: str, no_demangle: bool = False) -> List[Tuple[str, str]]: """ Get a list of discovered topics for publishers of a remote node. :param node_name: Name of a remote node to get publishers for. :param node_namespace: Namespace of the remote node. :param no_demangle: If ``True``, then topic names and types returned will not be demangled. :return: List of tuples containing two strings: the topic name and topic type. """ with self.handle as capsule: return _rclpy.rclpy_get_publisher_names_and_types_by_node( capsule, no_demangle, node_name, node_namespace) def get_subscriber_names_and_types_by_node( self, node_name: str, node_namespace: str, no_demangle: bool = False) -> List[Tuple[str, str]]: """ Get a list of discovered topics for subscriptions of a remote node. :param node_name: Name of a remote node to get subscriptions for. :param node_namespace: Namespace of the remote node. :param no_demangle: If ``True``, then topic names and types returned will not be demangled. :return: List of tuples containing two strings: the topic name and topic type. """ with self.handle as capsule: return _rclpy.rclpy_get_subscriber_names_and_types_by_node( capsule, no_demangle, node_name, node_namespace) def get_service_names_and_types_by_node( self, node_name: str, node_namespace: str) -> List[Tuple[str, str]]: """ Get a list of discovered service topics for a remote node. :param node_name: Name of a remote node to get services for. :param node_namespace: Namespace of the remote node. :return: List of tuples containing two strings: the service name and service type. """ with self.handle as capsule: return _rclpy.rclpy_get_service_names_and_types_by_node( capsule, node_name, node_namespace) def get_topic_names_and_types(self, no_demangle: bool = False ) -> List[Tuple[str, str]]: """ Get a list topic names and types for the node. :param no_demangle: If ``True``, then topic names and types returned will not be demangled. :return: List of tuples containing two strings: the topic name and topic type. """ with self.handle as capsule: return _rclpy.rclpy_get_topic_names_and_types(capsule, no_demangle) def get_service_names_and_types(self) -> List[Tuple[str, str]]: """ Get a list of service topics for the node. :return: List of tuples containing two strings: the service name and service type. """ with self.handle as capsule: return _rclpy.rclpy_get_service_names_and_types(capsule) def get_node_names(self) -> List[str]: """ Get a list of names for discovered nodes. :return: List of node names. """ with self.handle as capsule: names_ns = _rclpy.rclpy_get_node_names_and_namespaces(capsule) return [n[0] for n in names_ns] def get_node_names_and_namespaces(self) -> List[Tuple[str, str]]: """ Get a list of names and namespaces for discovered nodes. :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) def _count_publishers_or_subscribers(self, topic_name, func): fq_topic_name = expand_topic_name(topic_name, self.get_name(), self.get_namespace()) validate_topic_name(fq_topic_name) with self.handle as node_capsule: return func(node_capsule, fq_topic_name) def count_publishers(self, topic_name: str) -> int: """ Return the number of publishers on a given topic. `topic_name` may be a relative, private, or fully qualifed topic name. A relative or private topic is expanded using this node's namespace and name. The queried topic name is not remapped. :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) def count_subscribers(self, topic_name: str) -> int: """ Return the number of subscribers on a given topic. `topic_name` may be a relative, private, or fully qualifed topic name. A relative or private topic is expanded using this node's namespace and name. The queried topic name is not remapped. :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)
def set_initial_state(self, server, path, initial_states, initial_userdata=smach.UserData(), timeout=None): """Set the initial state of a smach server. @type server: string @param server: The name of the introspection server to which this client should connect. @type path: string @param path: The path to the target container in the state machine. @type initial_states: list of string @param inital_state: The state the target container should take when it starts. This is as list of at least one state label. @type initial_userdata: UserData @param initial_userdata: The userdata to inject into the target container. @type timeout: rclpy.time.Duration @param timeout: Timeout for this call. If this is set to None, it will not block, and the initial state may not be set before the target state machine goes active. """ # Construct initial state command initial_status_msg = SmachContainerInitialStatusCmd( path=path, initial_states=initial_states, local_data=bytearray(pickle.dumps(initial_userdata._data, 2))) # A status message to receive confirmation that the state was set properly msg_response = SmachContainerStatus() # Define a local callback to just stuff a local message def local_cb(msg_response, msg): self.get_logger().debug("Received status response: " + str(msg)) msg_response.path = msg.path msg_response.initial_states = msg.initial_states msg_response.local_data = msg.local_data # Create a subscriber to verify the request went through state_sub = self.create_subscription(SmachContainerStatus, server + STATUS_TOPIC, partial(local_cb, msg_response), 1) # Create a publisher to send the command self.get_logger().debug("Sending initial state command: " + str(initial_status_msg.path) + " on topic '" + server + INIT_TOPIC + "'") init_pub = self.create_publisher(SmachContainerInitialStatusCmd, server + INIT_TOPIC, 1) init_pub.publish(initial_status_msg) clock = ROSClock() rate = self.create_rate(4, clock) start_time = clock.now() # Block until we get a new state back if timeout is not None: while clock.now() - start_time < timeout: # Send the initial state command init_pub.publish(initial_status_msg) # Filter messages that are from other containers if msg_response.path == path: # Check if the heartbeat came back to match state_match = all([ s in msg_response.initial_states for s in initial_states ]) local_data = smach.UserData() local_data._data = pickle.loads(msg_response.local_data) ud_match = all([\ (key in local_data and local_data._data[key] == initial_userdata._data[key])\ for key in initial_userdata._data]) self.get_logger().debug("STATE MATCH: " + str(state_match) + ", UD_MATCH: " + str(ud_match)) if state_match and ud_match: return True rate.sleep() return False