Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 9 additions & 3 deletions ros2topic/ros2topic/verb/pub.py
Original file line number Diff line number Diff line change
Expand Up @@ -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')
Expand All @@ -108,7 +112,8 @@ def main(args):
1. / args.rate,
args.print,
times,
qos_profile)
qos_profile,
args.keep_alive)


def publisher(
Expand All @@ -120,6 +125,7 @@ def publisher(
print_nth: int,
times: int,
qos_profile: QoSProfile,
keep_alive: float = 0.1,
) -> Optional[str]:
"""Initialize a node with a single publisher and run its publish loop (maybe only once)."""
msg_module = get_message(message_type)
Expand Down Expand Up @@ -149,7 +155,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)
7 changes: 5 additions & 2 deletions ros2topic/test/fixtures/listener_node.py
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@

import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_system_default
from ros2topic.api import qos_profile_from_short_keys

from std_msgs.msg import String

Expand All @@ -25,8 +25,11 @@ class ListenerNode(Node):

def __init__(self):
super().__init__('listener')
qos_profile = qos_profile_from_short_keys(
'system_default', durability='transient_local',
reliability='reliable')
self.sub = self.create_subscription(
String, 'chatter', self.callback, qos_profile_system_default
String, 'chatter', self.callback, qos_profile
)

def callback(self, msg):
Expand Down
16 changes: 15 additions & 1 deletion ros2topic/test/test_cli.py
Original file line number Diff line number Diff line change
Expand Up @@ -559,7 +559,15 @@ def test_topic_echo_no_publisher(self):

def test_topic_pub(self):
with self.launch_topic_command(
arguments=['pub', '/chit_chatter', 'std_msgs/msg/String', '{data: foo}'],
arguments=[
'pub',
'--keep-alive', '3', # seconds
'--qos-durability', 'transient_local',
'--qos-reliability', 'reliable',
'/chit_chatter',
'std_msgs/msg/String',
'{data: foo}'
],
) as topic_command:
assert topic_command.wait_for_output(functools.partial(
launch_testing.tools.expect_output, expected_lines=[
Expand All @@ -579,6 +587,9 @@ def test_topic_pub_once(self):
with self.launch_topic_command(
arguments=[
'pub', '--once',
'--keep-alive', '3', # seconds
'--qos-durability', 'transient_local',
'--qos-reliability', 'reliable',
'/chit_chatter',
'std_msgs/msg/String',
'{data: bar}'
Expand All @@ -604,6 +615,9 @@ def test_topic_pub_print_every_two(self):
arguments=[
'pub',
'-p', '2',
'--keep-alive', '3', # seconds
'--qos-durability', 'transient_local',
'--qos-reliability', 'reliable',
'/chit_chatter',
'std_msgs/msg/String',
'{data: fizz}'
Expand Down