diff --git a/ros2topic/ros2topic/verb/pub.py b/ros2topic/ros2topic/verb/pub.py index 011cc02c4..53ea91b12 100644 --- a/ros2topic/ros2topic/verb/pub.py +++ b/ros2topic/ros2topic/verb/pub.py @@ -83,6 +83,10 @@ def add_arguments(self, parser, cli_name): group.add_argument( '-t', '--times', type=nonnegative_int, default=0, help='Publish this number of times and then exit') + parser.add_argument( + '--keep-alive', metavar='N', type=positive_float, default=0.1, + help='Keep publishing node alive for N seconds after the last msg ' + '(default: 0.1)') parser.add_argument( '-n', '--node-name', help='Name of the created publishing node') @@ -109,7 +113,8 @@ def main(args): 1. / args.rate, args.print, times, - qos_profile) + qos_profile, + args.keep_alive) def publisher( @@ -121,6 +126,7 @@ def publisher( 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) @@ -150,7 +156,7 @@ def timer_callback(): while times == 0 or count < times: rclpy.spin_once(node) - if times == 1: - time.sleep(0.1) # make sure the message reaches the wire before exiting + # give some time for the messages to reach the wire before exiting + time.sleep(keep_alive) node.destroy_timer(timer)