def _getQoSProfile(self): profile = QoSProfile(depth=10) profile.reliability = QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT profile.durability = QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE profile.liveliness = QoSLivelinessPolicy.RMW_QOS_POLICY_LIVELINESS_AUTOMATIC profile.deadline = Duration() profile.lifespan = Duration() return profile
def _getQoSProfile(self): profile = QoSProfile( history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_ALL) profile.reliability = QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE profile.durability = QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL profile.liveliness = QoSLivelinessPolicy.RMW_QOS_POLICY_LIVELINESS_AUTOMATIC profile.deadline = Duration() profile.lifespan = Duration() return profile
def __init__(self, topic, node_handle, msg_type=None, latched_client_id=None, queue_size=100): """ Register a publisher on the specified topic. Keyword arguments: topic -- the name of the topic to register the publisher to node_handle -- Handle to a rclpy node to create the publisher. msg_type -- (optional) the type to register the publisher as. If not provided, an attempt will be made to infer the topic type latch -- (optional) if a client requested this publisher to be latched, provide the client_id of that client here Throws: TopicNotEstablishedException -- if no msg_type was specified by the caller and the topic is not yet established, so a topic type cannot be inferred TypeConflictException -- if the msg_type was specified by the caller and the topic is established, and the established type is different to the user-specified msg_type """ # First check to see if the topic is already established topics_names_and_types = dict(node_handle.get_topic_names_and_types()) topic_type = topics_names_and_types.get(topic) # If it's not established and no type was specified, exception if msg_type is None and topic_type is None: raise TopicNotEstablishedException(topic) # topic_type is a list of types or None at this point; only one type is supported. if topic_type is not None: if len(topic_type) > 1: node_handle.get_logger().warning( 'More than one topic type detected: {}'.format(topic_type)) topic_type = topic_type[0] # Use the established topic type if none was specified if msg_type is None: msg_type = topic_type # Load the message class, propagating any exceptions from bad msg types msg_class = ros_loader.get_message_class(msg_type) # Make sure the specified msg type and established msg type are same msg_type_string = msg_class_type_repr(msg_class) if topic_type is not None and topic_type != msg_type_string: raise TypeConflictException(topic, topic_type, msg_type_string) # Create the publisher and associated member variables self.clients = {} self.latched_client_id = latched_client_id self.topic = topic self.node_handle = node_handle self.msg_class = msg_class # Adding a lifespan solves the problem of late-joining subscribers # without the need of a custom message publisher implementation. publisher_qos = QoSProfile(depth=queue_size, durability=QoSDurabilityPolicy. RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) # For latched clients, no lifespan has to be specified (i.e. latch forever). # Otherwise we want to keep the messages for a second to prevent late-joining subscribers from # missing messages. if latched_client_id is None: publisher_qos.lifespan = Duration(seconds=1) else: publisher_qos.depth = 1 self.publisher = node_handle.create_publisher( msg_class, topic, qos_profile=publisher_qos)