From e8138a12787a829915e9aa553eaba0b1031c245d Mon Sep 17 00:00:00 2001 From: SangtaekLee <82325867+sangteak601@users.noreply.github.com> Date: Fri, 24 Jan 2025 21:51:11 +0000 Subject: [PATCH] support multiple fields in ros2topic echo (#964) * support multiple fields in ros2topic echo Signed-off-by: Sangtaek Lee Signed-off-by: Sangtaek Lee * add tests for echo multi fields Signed-off-by: Sangtaek Lee * fix line too long Signed-off-by: Sangtaek Lee * update line end Signed-off-by: Sangtaek Lee --------- Signed-off-by: Sangtaek Lee Signed-off-by: Sangtaek Lee --- ros2topic/ros2topic/verb/echo.py | 100 ++++++++++++++++++------------- ros2topic/test/test_cli.py | 72 ++++++++++++++++++++++ 2 files changed, 131 insertions(+), 41 deletions(-) diff --git a/ros2topic/ros2topic/verb/echo.py b/ros2topic/ros2topic/verb/echo.py index edd8b8917..db72b51ea 100644 --- a/ros2topic/ros2topic/verb/echo.py +++ b/ros2topic/ros2topic/verb/echo.py @@ -69,7 +69,7 @@ def add_arguments(self, parser, cli_name): ) ) parser.add_argument( - '--field', type=str, default=None, + '--field', action='append', type=str, default=None, help='Echo a selected field of a message. ' "Use '.' to select sub-fields. " 'For example, to echo the position field of a nav_msgs/msg/Odometry message: ' @@ -180,11 +180,14 @@ def main(self, *, args): self.csv = args.csv # Validate field selection - self.field = args.field - if self.field is not None: - self.field = list(filter(None, self.field.split('.'))) - if not self.field: - raise RuntimeError(f"Invalid field value '{args.field}'") + self.fields_list = [] + if args.field: + for field in args.field: + if field is not None: + field_filtered = list(filter(None, field.split('.'))) + self.fields_list.append(field_filtered) + if not field_filtered: + raise RuntimeError(f"Invalid field value '{field}'") self.truncate_length = args.truncate_length if not args.full_length else None self.no_arr = args.no_arr @@ -272,17 +275,26 @@ def _timed_out(self): self.future.set_result(True) def _subscriber_callback(self, msg, info): - submsg = msg - if self.field is not None: - for field in self.field: - try: - submsg = getattr(submsg, field) - except AttributeError as ex: - raise RuntimeError(f"Invalid field '{'.'.join(self.field)}': {ex}") + submsgs = [] + if self.fields_list: + for fields in self.fields_list: + submsg = msg + for field in fields: + try: + submsg = getattr(submsg, field) + except AttributeError as ex: + raise RuntimeError(f"Invalid field '{'.'.join(fields)}': {ex}") + submsgs.append(submsg) + else: + submsgs.append(msg) # Evaluate the current msg against the supplied expression - if self.filter_fn is not None and not self.filter_fn(submsg): - return + if self.filter_fn is not None: + for submsg in submsgs: + if not self.filter_fn(submsg): + submsgs.remove(submsg) + if not submsgs: + return if self.future is not None and self.once: self.future.set_result(True) @@ -291,33 +303,39 @@ def _subscriber_callback(self, msg, info): if self.clear_screen: clear_terminal() - if not hasattr(submsg, '__slots__'): - # raw - if self.include_message_info: - print('---Got new message, message info:---') - print(info) - print('---Message data:---') - print(submsg, end='\n---\n') - return - - if self.csv: - to_print = message_to_csv( - submsg, - truncate_length=self.truncate_length, - no_arr=self.no_arr, - no_str=self.no_str) + for i, submsg in enumerate(submsgs): + if i == len(submsgs)-1: + line_end = '---\n' + else: + line_end = '' + if not hasattr(submsg, '__slots__'): + # raw + if self.include_message_info: + print('---Got new message, message info:---') + print(info) + print('---Message data:---') + line_end = '\n' + line_end + print(submsg, end=line_end) + continue + + if self.csv: + to_print = message_to_csv( + submsg, + truncate_length=self.truncate_length, + no_arr=self.no_arr, + no_str=self.no_str) + if self.include_message_info: + to_print = f'{",".join(str(x) for x in info.values())},{to_print}' + print(to_print) + continue + # yaml if self.include_message_info: - to_print = f'{",".join(str(x) for x in info.values())},{to_print}' - print(to_print) - return - # yaml - if self.include_message_info: - print(yaml.dump(info), end='---\n') - print( - message_to_yaml( - submsg, truncate_length=self.truncate_length, - no_arr=self.no_arr, no_str=self.no_str, flow_style=self.flow_style), - end='---\n') + print(yaml.dump(info), end=line_end) + print( + message_to_yaml( + submsg, truncate_length=self.truncate_length, + no_arr=self.no_arr, no_str=self.no_str, flow_style=self.flow_style), + end=line_end) def _expr_eval(expr): diff --git a/ros2topic/test/test_cli.py b/ros2topic/test/test_cli.py index 97ffe6cbb..84640d094 100644 --- a/ros2topic/test/test_cli.py +++ b/ros2topic/test/test_cli.py @@ -589,6 +589,20 @@ def test_topic_echo_field(self): ), timeout=10) assert topic_command.wait_for_shutdown(timeout=10) + @launch_testing.markers.retry_on_failure(times=5, delay=1) + def test_topic_echo_multi_fields(self): + with self.launch_topic_command( + arguments=['echo', '/defaults', '--field', 'int8_value', '--field', 'uint8_value'], + ) as topic_command: + assert topic_command.wait_for_output(functools.partial( + launch_testing.tools.expect_output, expected_lines=[ + '-50', + '200', + '---', + ], strict=True + ), timeout=10) + assert topic_command.wait_for_shutdown(timeout=10) + @launch_testing.markers.retry_on_failure(times=5, delay=1) def test_topic_echo_field_nested(self): with self.launch_topic_command( @@ -604,6 +618,21 @@ def test_topic_echo_field_nested(self): ), timeout=10) assert topic_command.wait_for_shutdown(timeout=10) + @launch_testing.markers.retry_on_failure(times=5, delay=1) + def test_topic_echo_multi_fields_nested(self): + with self.launch_topic_command( + arguments=['echo', '/cmd_vel', '--field', 'twist.linear.x', + '--field', 'twist.linear.y'], + ) as topic_command: + assert topic_command.wait_for_output(functools.partial( + launch_testing.tools.expect_output, expected_lines=[ + '1.0', + '0.0', + '---', + ], strict=True + ), timeout=10) + assert topic_command.wait_for_shutdown(timeout=10) + @launch_testing.markers.retry_on_failure(times=5, delay=1) def test_topic_echo_field_not_a_member(self): with self.launch_topic_command( @@ -616,6 +645,28 @@ def test_topic_echo_field_not_a_member(self): ), timeout=10) assert topic_command.wait_for_shutdown(timeout=10) + @launch_testing.markers.retry_on_failure(times=5, delay=1) + def test_topic_echo_multi_fields_not_a_member(self): + with self.launch_topic_command( + arguments=['echo', '/arrays', '--field', 'not_member', '--field', 'alignment_check'], + ) as topic_command: + assert topic_command.wait_for_output(functools.partial( + launch_testing.tools.expect_output, expected_lines=[ + "Invalid field 'not_member': 'Arrays' object has no attribute 'not_member'", + ], strict=True + ), timeout=10) + assert topic_command.wait_for_shutdown(timeout=10) + + with self.launch_topic_command( + arguments=['echo', '/arrays', '--field', 'alignment_check', '--field', 'not_member'], + ) as topic_command: + assert topic_command.wait_for_output(functools.partial( + launch_testing.tools.expect_output, expected_lines=[ + "Invalid field 'not_member': 'Arrays' object has no attribute 'not_member'", + ], strict=True + ), timeout=10) + assert topic_command.wait_for_shutdown(timeout=10) + def test_topic_echo_field_invalid(self): with self.launch_topic_command( arguments=['echo', '/arrays', '--field', '/'], @@ -637,6 +688,27 @@ def test_topic_echo_field_invalid(self): ), timeout=10) assert topic_command.wait_for_shutdown(timeout=10) + def test_topic_echo_multi_fields_invalid(self): + with self.launch_topic_command( + arguments=['echo', '/arrays', '--field', '/', '--field', 'alignment_check'], + ) as topic_command: + assert topic_command.wait_for_output(functools.partial( + launch_testing.tools.expect_output, expected_lines=[ + "Invalid field '/': 'Arrays' object has no attribute '/'", + ], strict=True + ), timeout=10) + assert topic_command.wait_for_shutdown(timeout=10) + + with self.launch_topic_command( + arguments=['echo', '/arrays', '--field', 'alignment_check', '--field', '.'], + ) as topic_command: + assert topic_command.wait_for_output(functools.partial( + launch_testing.tools.expect_output, expected_lines=[ + "Invalid field value '.'", + ], strict=True + ), timeout=10) + assert topic_command.wait_for_shutdown(timeout=10) + def test_topic_echo_no_publisher(self): with self.launch_topic_command( arguments=['echo', '/this_topic_has_no_pub'],