Enable ros2 topic echo with entries of array fields (#996)

Signed-off-by: Fabian Thomsen <[email protected]>

Author: FabianThomsen

ros2topic/ros2topic/verb/echo.py

@@ -12,6 +12,7 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+import re
 from typing import Optional
 from typing import TypeVar
 
@@ -72,7 +73,9 @@ def add_arguments(self, parser, cli_name):
             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: '
-                 "'ros2 topic echo /odom --field pose.pose.position'",
+                 "'ros2 topic echo /odom --field pose.pose.position'. "
+                 'Use the --field option multiple times to echo multiple fields. '
+                 'If the field is an array, use the syntax .[index] to select a single element.'
         )
         parser.add_argument(
             '--full-length', '-f', action='store_true',
@@ -215,12 +218,19 @@ def _timed_out(self):
     def _subscriber_callback(self, msg, info):
         submsgs = []
         if self.fields_list:
+            # Matches strings exactly in the format "[digits]" (e.g., "[123]")
+            # and captures the digits as a group
+            is_indexing = re.compile(r'^\[(\d+)\]$')
             for fields in self.fields_list:
                 submsg = msg
                 for field in fields:
+                    match = is_indexing.match(field)
                     try:
-                        submsg = getattr(submsg, field)
-                    except AttributeError as ex:
+                        if match is None:
+                            submsg = getattr(submsg, field)
+                        else:
+                            submsg = submsg[int(match.group(1))]
+                    except (AttributeError, IndexError, TypeError, ValueError) as ex:
                         raise RuntimeError(f"Invalid field '{'.'.join(fields)}': {ex}")
                 submsgs.append(submsg)
         else:

ros2topic/test/test_cli.py

@@ -616,6 +616,19 @@ 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_field_array(self):
+        with self.launch_topic_command(
+            arguments=['echo', '/arrays', '--field', 'float32_values_default.[2]'],
+        ) as topic_command:
+            assert topic_command.wait_for_output(functools.partial(
+                launch_testing.tools.expect_output, expected_lines=[
+                    '-1.125',
+                    '---',
+                ], 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_multi_fields_nested(self):
         with self.launch_topic_command(
@@ -631,6 +644,21 @@ def test_topic_echo_multi_fields_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_array(self):
+        with self.launch_topic_command(
+            arguments=['echo', '/arrays', '--field', 'float32_values_default.[2]', '--field',
+                       'string_values_default.[1]'],
+        ) as topic_command:
+            assert topic_command.wait_for_output(functools.partial(
+                launch_testing.tools.expect_output, expected_lines=[
+                    '-1.125',
+                    'max value',
+                    '---',
+                ], 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(
@@ -643,6 +671,45 @@ 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_field_array_not_an_array(self):
+        with self.launch_topic_command(
+            arguments=['echo', '/arrays', '--field', 'float32_values_default.[0].[0]'],
+        ) as topic_command:
+            assert topic_command.wait_for_output(functools.partial(
+                launch_testing.tools.expect_output, expected_lines=[
+                    "Invalid field 'float32_values_default.[0].[0]': invalid index to "
+                    'scalar variable.',
+                ], 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_array_index_out_of_bounds(self):
+        with self.launch_topic_command(
+            arguments=['echo', '/arrays', '--field', 'float32_values_default.[3]'],
+        ) as topic_command:
+            assert topic_command.wait_for_output(functools.partial(
+                launch_testing.tools.expect_output, expected_lines=[
+                    "Invalid field 'float32_values_default.[3]': index 3 is out of bounds "
+                    'for axis 0 with size 3',
+                ], 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_array_no_index(self):
+        with self.launch_topic_command(
+            arguments=['echo', '/arrays', '--field', 'float32_values_default.[abc]'],
+        ) as topic_command:
+            assert topic_command.wait_for_output(functools.partial(
+                launch_testing.tools.expect_output, expected_lines=[
+                    "Invalid field 'float32_values_default.[abc]': 'numpy.ndarray' object "
+                    "has no attribute '[abc]'",
+                ], 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_multi_fields_not_a_member(self):
         with self.launch_topic_command(