From c9d666f21e15afc57a4095439af5eea3c3745272 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Wed, 28 Aug 2024 17:50:37 -0700 Subject: [PATCH 1/4] `ros2 topic pub --yaml-file` support. Signed-off-by: Tomoya Fujita --- ros2topic/ros2topic/verb/pub.py | 71 +++++++++++++++++++++++++++------ 1 file changed, 58 insertions(+), 13 deletions(-) diff --git a/ros2topic/ros2topic/verb/pub.py b/ros2topic/ros2topic/verb/pub.py index fce959a7b..3b78ce9d7 100644 --- a/ros2topic/ros2topic/verb/pub.py +++ b/ros2topic/ros2topic/verb/pub.py @@ -98,6 +98,10 @@ def add_arguments(self, parser, cli_name): '--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( + '--yaml-file', type=str, default=None, + help='Yaml file that has message contents, prevails . ' + 'e.g STDOUT from ros2 topic echo ') parser.add_argument( '-n', '--node-name', help='Name of the created publishing node') @@ -120,10 +124,12 @@ def main(args): if args.once: times = 1 - if args.stdin: - values = collect_stdin() - else: - values = args.values + values = None + if not args.yaml_file: + if args.stdin: + values = collect_stdin() + else: + values = args.values with DirectNode(args, node_name=args.node_name) as node: return publisher( @@ -131,6 +137,7 @@ def main(args): args.message_type, args.topic_name, values, + args.yaml_file, 1. / args.rate, args.print, times, @@ -146,6 +153,7 @@ def publisher( message_type: MsgType, topic_name: str, values: dict, + yaml_file: str, period: float, print_nth: int, times: int, @@ -159,9 +167,14 @@ def publisher( 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' + + msg_reader = None + if yaml_file: + msg_reader = read_msg_from_yaml(yaml_file) + else: + 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) @@ -184,15 +197,38 @@ def publisher( total_wait_time += DEFAULT_WAIT_TIME msg = msg_module() - try: - timestamp_fields = set_message_fields( - msg, values_dictionary, expand_header_auto=True, expand_time_now=True) - except Exception as e: - return 'Failed to populate field: {0}'.format(e) + timestamp_fields = None + + if not msg_reader: + # Set the static message from specified values once + try: + timestamp_fields = set_message_fields( + msg, values_dictionary, expand_header_auto=True, expand_time_now=True) + except Exception as e: + return 'Failed to populate field: {0}'.format(e) + print('publisher: beginning loop') count = 0 + more_message = True def timer_callback(): + if msg_reader: + # Try to read out the contents for each message + try: + one_msg = next(msg_reader) + if not isinstance(one_msg, dict): + print('The contents in yaml file need to be a YAML format') + except StopIteration: + nonlocal more_message + more_message = False + return + # Set the message with contents + try: + nonlocal timestamp_fields + timestamp_fields = set_message_fields( + msg, one_msg, expand_header_auto=True, expand_time_now=True) + except Exception as e: + return 'Failed to populate field: {0}'.format(e) stamp_now = node.get_clock().now().to_msg() for field_setter in timestamp_fields: field_setter(stamp_now) @@ -205,7 +241,7 @@ def timer_callback(): timer_callback() if times != 1: timer = node.create_timer(period, timer_callback) - while times == 0 or count < times: + while (times == 0 or count < times) and more_message: rclpy.spin_once(node) # give some time for the messages to reach the wire before exiting time.sleep(keep_alive) @@ -213,3 +249,12 @@ def timer_callback(): else: # give some time for the messages to reach the wire before exiting time.sleep(keep_alive) + + +def read_msg_from_yaml(yaml_file): + with open(yaml_file, 'r') as f: + for document in yaml.load_all(f, Loader=yaml.FullLoader): + if document is None: + continue # Skip if there's no more document + + yield document From 4adef791b0de36ca0078cb552109dab769925227 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Thu, 29 Aug 2024 22:36:27 -0700 Subject: [PATCH 2/4] add test_pub_yaml for ros2topic. Signed-off-by: Tomoya Fujita --- ros2topic/test/resources/chatter.yaml | 10 +++++++ ros2topic/test/test_echo_pub.py | 39 +++++++++++++++++++++++++++ 2 files changed, 49 insertions(+) create mode 100644 ros2topic/test/resources/chatter.yaml diff --git a/ros2topic/test/resources/chatter.yaml b/ros2topic/test/resources/chatter.yaml new file mode 100644 index 000000000..4ac2880b8 --- /dev/null +++ b/ros2topic/test/resources/chatter.yaml @@ -0,0 +1,10 @@ +--- +data: 'Hello ROS Users' +--- +--- +data: Hello ROS Developers +--- +data: Hello ROS Developers +--- +--- +data: 'Hello ROS Users' diff --git a/ros2topic/test/test_echo_pub.py b/ros2topic/test/test_echo_pub.py index 6182073e5..19a3ba229 100644 --- a/ros2topic/test/test_echo_pub.py +++ b/ros2topic/test/test_echo_pub.py @@ -13,6 +13,7 @@ # limitations under the License. import functools +import pathlib import sys import unittest @@ -49,6 +50,8 @@ TEST_NODE = 'cli_echo_pub_test_node' TEST_NAMESPACE = 'cli_echo_pub' +TEST_RESOURCES_DIR = pathlib.Path(__file__).resolve().parent / 'resources' + @pytest.mark.rostest @launch_testing.markers.keep_alive @@ -294,6 +297,42 @@ def test_pub_times(self, launch_service, proc_info, proc_output): strict=True ) + @launch_testing.markers.retry_on_failure(times=5) + def test_pub_yaml(self, launch_service, proc_info, proc_output): + command_action = ExecuteProcess( + # yaml file prevails to the values 'data: hello' + cmd=(['ros2', 'topic', 'pub', '/clitest/topic/chatter', + 'std_msgs/String', 'data: hello', '--yaml-file', + str(TEST_RESOURCES_DIR / 'chatter.yaml')]), + additional_env={ + 'PYTHONUNBUFFERED': '1' + }, + output='screen' + ) + with launch_testing.tools.launch_process( + launch_service, command_action, proc_info, proc_output, + output_filter=launch_testing_ros.tools.basic_output_filter( + filtered_rmw_implementation=get_rmw_implementation_identifier() + ) + ) as command: + assert command.wait_for_shutdown(timeout=10) + assert command.exit_code == launch_testing.asserts.EXIT_OK + assert launch_testing.tools.expect_output( + expected_lines=[ + 'publisher: beginning loop', + "publishing #1: std_msgs.msg.String(data='Hello ROS Users')", + '', + "publishing #2: std_msgs.msg.String(data='Hello ROS Developers')", + '', + "publishing #3: std_msgs.msg.String(data='Hello ROS Developers')", + '', + "publishing #4: std_msgs.msg.String(data='Hello ROS Users')", + '', + ], + text=command.output, + strict=True + ) + @launch_testing.markers.retry_on_failure(times=5) def test_echo_basic(self, launch_service, proc_info, proc_output): params = [ From 07fb41b4dc6c817b46caae9828288cb86ef93ba3 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Thu, 5 Sep 2024 11:09:17 -0700 Subject: [PATCH 3/4] fix typos, should be YAML. Signed-off-by: Tomoya Fujita --- ros2topic/ros2topic/verb/pub.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ros2topic/ros2topic/verb/pub.py b/ros2topic/ros2topic/verb/pub.py index 3b78ce9d7..a93f909b8 100644 --- a/ros2topic/ros2topic/verb/pub.py +++ b/ros2topic/ros2topic/verb/pub.py @@ -100,7 +100,7 @@ def add_arguments(self, parser, cli_name): '(default: 0.1)') parser.add_argument( '--yaml-file', type=str, default=None, - help='Yaml file that has message contents, prevails . ' + help='YAML file that has message contents, prevails . ' 'e.g STDOUT from ros2 topic echo ') parser.add_argument( '-n', '--node-name', @@ -217,7 +217,7 @@ def timer_callback(): try: one_msg = next(msg_reader) if not isinstance(one_msg, dict): - print('The contents in yaml file need to be a YAML format') + print('The contents in YAML file need to be a YAML format') except StopIteration: nonlocal more_message more_message = False From b03cf8aaf0a97204acfd8dbd0290c3cb901ed3bc Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Mon, 9 Dec 2024 12:52:44 -0800 Subject: [PATCH 4/4] have `--yaml-file` option in mutual exclusive group with values and `--stdin`. Signed-off-by: Tomoya Fujita --- ros2topic/ros2topic/verb/pub.py | 18 ++++++++---------- ros2topic/test/test_echo_pub.py | 2 +- 2 files changed, 9 insertions(+), 11 deletions(-) diff --git a/ros2topic/ros2topic/verb/pub.py b/ros2topic/ros2topic/verb/pub.py index a93f909b8..f280f1bc7 100644 --- a/ros2topic/ros2topic/verb/pub.py +++ b/ros2topic/ros2topic/verb/pub.py @@ -70,6 +70,10 @@ def add_arguments(self, parser, cli_name): group.add_argument( '--stdin', action='store_true', help='Read values from standard input') + group.add_argument( + '--yaml-file', type=str, default=None, + help='YAML file that has message contents, ' + 'e.g STDOUT from ros2 topic echo ') parser.add_argument( '-r', '--rate', metavar='N', type=positive_float, default=1.0, help='Publishing rate in Hz (default: 1)') @@ -98,10 +102,6 @@ def add_arguments(self, parser, cli_name): '--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( - '--yaml-file', type=str, default=None, - help='YAML file that has message contents, prevails . ' - 'e.g STDOUT from ros2 topic echo ') parser.add_argument( '-n', '--node-name', help='Name of the created publishing node') @@ -124,12 +124,10 @@ def main(args): if args.once: times = 1 - values = None - if not args.yaml_file: - if args.stdin: - values = collect_stdin() - else: - values = args.values + if args.stdin: + values = collect_stdin() + else: + values = args.values with DirectNode(args, node_name=args.node_name) as node: return publisher( diff --git a/ros2topic/test/test_echo_pub.py b/ros2topic/test/test_echo_pub.py index 19a3ba229..14b278a11 100644 --- a/ros2topic/test/test_echo_pub.py +++ b/ros2topic/test/test_echo_pub.py @@ -302,7 +302,7 @@ def test_pub_yaml(self, launch_service, proc_info, proc_output): command_action = ExecuteProcess( # yaml file prevails to the values 'data: hello' cmd=(['ros2', 'topic', 'pub', '/clitest/topic/chatter', - 'std_msgs/String', 'data: hello', '--yaml-file', + 'std_msgs/String', '--yaml-file', str(TEST_RESOURCES_DIR / 'chatter.yaml')]), additional_env={ 'PYTHONUNBUFFERED': '1'