def publisher( node: Node, message_type: MsgType, topic_name: str, values: dict, period: float, print_nth: int, times: int, wait_matching_subscriptions: int, qos_profile: QoSProfile, keep_alive: float, ) -> Optional[str]: """Initialize a node with a single publisher and run its publish loop (maybe only once).""" try: msg_module = get_message(message_type) except (AttributeError, ModuleNotFoundError, ValueError): raise RuntimeError('The passed message type is invalid') values_dictionary = yaml.safe_load(values) if not isinstance(values_dictionary, dict): return 'The passed value needs to be a dictionary in YAML format' pub = node.create_publisher(msg_module, topic_name, qos_profile) times_since_last_log = 0 while pub.get_subscription_count() < wait_matching_subscriptions: # Print a message reporting we're waiting each 1s, check condition each 100ms. if not times_since_last_log: print( f'Waiting for at least {wait_matching_subscriptions} matching subscription(s)...' ) times_since_last_log = (times_since_last_log + 1) % 10 time.sleep(0.1) msg = msg_module() try: set_message_fields(msg, values_dictionary) except Exception as e: return 'Failed to populate field: {0}'.format(e) print('publisher: beginning loop') count = 0 def timer_callback(): nonlocal count count += 1 if print_nth and count % print_nth == 0: print('publishing #%d: %r\n' % (count, msg)) pub.publish(msg) timer_callback() if times != 1: timer = node.create_timer(period, timer_callback) while times == 0 or count < times: rclpy.spin_once(node) # give some time for the messages to reach the wire before exiting time.sleep(keep_alive) node.destroy_timer(timer) else: # give some time for the messages to reach the wire before exiting time.sleep(keep_alive)
def publisher( node: Node, message_type: MsgType, topic_name: str, values: dict, period: float, print_nth: int, once: bool, qos_profile: QoSProfile, ) -> Optional[str]: """Initialize a node with a single publisher and run its publish loop (maybe only once).""" msg_module = get_message(message_type) values_dictionary = yaml.safe_load(values) if not isinstance(values_dictionary, dict): return 'The passed value needs to be a dictionary in YAML format' publisher_callbacks = PublisherEventCallbacks( incompatible_qos=handle_incompatible_qos_event) try: pub = node.create_publisher(msg_module, topic_name, qos_profile, event_callbacks=publisher_callbacks) except UnsupportedEventTypeError: pub = node.create_publisher(msg_module, topic_name, qos_profile) msg = msg_module() try: set_message_fields(msg, values_dictionary) except Exception as e: return 'Failed to populate field: {0}'.format(e) print('publisher: beginning loop') count = 0 def timer_callback(): nonlocal count count += 1 if print_nth and count % print_nth == 0: print('publishing #%d: %r\n' % (count, msg)) pub.publish(msg) timer = node.create_timer(period, timer_callback) if once: rclpy.spin_once(node) time.sleep( 0.1) # make sure the message reaches the wire before exiting else: rclpy.spin(node) node.destroy_timer(timer)
def publisher( node: Node, message_type: MsgType, topic_name: str, values: dict, period: float, print_nth: int, times: int, qos_profile: QoSProfile, keep_alive: float, ) -> Optional[str]: """Initialize a node with a single publisher and run its publish loop (maybe only once).""" msg_module = get_message(message_type) values_dictionary = yaml.safe_load(values) if not isinstance(values_dictionary, dict): return 'The passed value needs to be a dictionary in YAML format' pub = node.create_publisher(msg_module, topic_name, qos_profile) msg = msg_module() try: set_message_fields(msg, values_dictionary) except Exception as e: return 'Failed to populate field: {0}'.format(e) print('publisher: beginning loop') count = 0 def timer_callback(): nonlocal count count += 1 if print_nth and count % print_nth == 0: print('publishing #%d: %r\n' % (count, msg)) pub.publish(msg) timer = node.create_timer(period, timer_callback) while times == 0 or count < times: rclpy.spin_once(node) # give some time for the messages to reach the wire before exiting time.sleep(keep_alive) node.destroy_timer(timer)