Add filter option to ros2topic (#575)

* Refactor ros2topic echo verb implementation

* Move subscription functions into class for easier reuse arguments.
* Prefix functions with an underscore to indicate they are private.

Signed-off-by: Jacob Perron <[email protected]>

* Add filter option to ros2topic

A new '--filter' option makes it easy to echo only part of a message.
For example, to only echo the position part of an odometry message:

    ros2 topic echo /odom --filter pose/pose/position

The filter option takes one value that must not be empty (or only forward slashes '/').

Signed-off-by: Jacob Perron <[email protected]>

* Change option name and syntax

Option is now named '--field' and the '.' character is used as the delimiter.

Also updated the help message with an example and added a test for accessing a nested field.

Signed-off-by: Jacob Perron <[email protected]>

Author: jacobperron

ros2topic/ros2topic/verb/echo.py

@@ -12,8 +12,6 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-from typing import Any
-from typing import Callable
 from typing import Optional
 from typing import TypeVar
 
@@ -59,6 +57,13 @@ def add_arguments(self, parser, cli_name):
             '--csv', action='store_true',
             help='Output all recursive fields separated by commas (e.g. for '
                  'plotting)')
+        parser.add_argument(
+            '--field', 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: '
+                 "'ros2 topic echo /odom --field pose.pose.position'",
+        )
         parser.add_argument(
             '--full-length', '-f', action='store_true',
             help='Output all elements for arrays, bytes, and string with a '
@@ -78,98 +83,111 @@ def add_arguments(self, parser, cli_name):
             '--raw', action='store_true', help='Echo the raw binary representation')
 
     def main(self, *, args):
-        return main(args)
+        # Select print function
+        self.print_func = _print_yaml
+        if args.csv:
+            self.print_func = _print_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.truncate_length = args.truncate_length if not args.full_length else None
+        self.no_arr = args.no_arr
+        self.no_str = args.no_str
+
+        qos_profile = qos_profile_from_short_keys(
+            args.qos_profile,
+            reliability=args.qos_reliability,
+            durability=args.qos_durability,
+            depth=args.qos_depth,
+            history=args.qos_history)
+
+        with NodeStrategy(args) as node:
+            if args.message_type is None:
+                message_type = get_msg_class(
+                    node, args.topic_name, include_hidden_topics=True)
+            else:
+                try:
+                    message_type = get_message(args.message_type)
+                except (AttributeError, ModuleNotFoundError, ValueError):
+                    raise RuntimeError('The passed message type is invalid')
+
+            if message_type is None:
+                raise RuntimeError(
+                    'Could not determine the type for the passed topic')
+
+            self.subscribe_and_spin(
+                node,
+                args.topic_name,
+                message_type,
+                qos_profile,
+                args.lost_messages,
+                args.raw)
+
+    def subscribe_and_spin(
+        self,
+        node: Node,
+        topic_name: str,
+        message_type: MsgType,
+        qos_profile: QoSProfile,
+        report_lost_messages: bool,
+        raw: bool
+    ) -> Optional[str]:
+        """Initialize a node with a single subscription and spin."""
+        event_callbacks = None
+        if report_lost_messages:
+            event_callbacks = SubscriptionEventCallbacks(
+                message_lost=_message_lost_event_callback)
+        try:
+            node.create_subscription(
+                message_type,
+                topic_name,
+                self._subscriber_callback,
+                qos_profile,
+                event_callbacks=event_callbacks,
+                raw=raw)
+        except UnsupportedEventTypeError:
+            assert report_lost_messages
+            print(
+                f"The rmw implementation '{get_rmw_implementation_identifier()}'"
+                ' does not support reporting lost messages'
+            )
+        rclpy.spin(node)
 
+    def _subscriber_callback(self, msg):
+        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}")
 
-def main(args):
-    if not args.csv:
-        truncate_length = args.truncate_length if not args.full_length else None
-        callback = subscriber_cb(truncate_length, args.no_arr, args.no_str)
-    else:
-        truncate_length = args.truncate_length if not args.full_length else None
-        callback = subscriber_cb_csv(truncate_length, args.no_arr, args.no_str)
-    qos_profile = qos_profile_from_short_keys(
-        args.qos_profile, reliability=args.qos_reliability, durability=args.qos_durability,
-        depth=args.qos_depth, history=args.qos_history)
-    with NodeStrategy(args) as node:
-        if args.message_type is None:
-            message_type = get_msg_class(
-                node, args.topic_name, include_hidden_topics=True)
-        else:
-            try:
-                message_type = get_message(args.message_type)
-            except (AttributeError, ModuleNotFoundError, ValueError):
-                raise RuntimeError('The passed message type is invalid')
-
-        if message_type is None:
-            raise RuntimeError(
-                'Could not determine the type for the passed topic')
-
-        subscriber(
-            node,
-            args.topic_name,
-            message_type,
-            callback,
-            qos_profile,
-            args.lost_messages,
-            args.raw)
-
-
-def subscriber(
-    node: Node,
-    topic_name: str,
-    message_type: MsgType,
-    callback: Callable[[MsgType], Any],
-    qos_profile: QoSProfile,
-    report_lost_messages: bool,
-    raw: bool
-) -> Optional[str]:
-    """Initialize a node with a single subscription and spin."""
-    event_callbacks = None
-    if report_lost_messages:
-        event_callbacks = SubscriptionEventCallbacks(
-            message_lost=message_lost_event_callback)
-    try:
-        node.create_subscription(
-            message_type,
-            topic_name,
-            callback,
-            qos_profile,
-            event_callbacks=event_callbacks,
-            raw=raw)
-    except UnsupportedEventTypeError:
-        assert report_lost_messages
-        print(
-            f"The rmw implementation '{get_rmw_implementation_identifier()}'"
-            ' does not support reporting lost messages'
-        )
-    rclpy.spin(node)
+        self.print_func(submsg, self.truncate_length, self.no_arr, self.no_str)
 
 
-def subscriber_cb(truncate_length, noarr, nostr):
-    def cb(msg):
-        nonlocal truncate_length, noarr, nostr
-        if isinstance(msg, bytes):
-            print(msg, end='\n---\n')
-        else:
-            print(
-                message_to_yaml(
-                    msg, truncate_length=truncate_length, no_arr=noarr, no_str=nostr),
-                end='---\n')
-    return cb
+def _print_yaml(msg, truncate_length, noarr, nostr):
+    if hasattr(msg, '__slots__'):
+        print(
+            message_to_yaml(
+                msg, truncate_length=truncate_length, no_arr=noarr, no_str=nostr),
+            end='---\n')
+    else:
+        print(msg, end='\n---\n')
 
 
-def subscriber_cb_csv(truncate_length, noarr, nostr):
-    def cb(msg):
-        nonlocal truncate_length, noarr, nostr
-        if isinstance(msg, bytes):
-            print(msg)
-        else:
-            print(message_to_csv(msg, truncate_length=truncate_length, no_arr=noarr, no_str=nostr))
-    return cb
+def _print_csv(msg, truncate_length, noarr, nostr):
+    if hasattr(msg, '__slots__'):
+        print(message_to_csv(msg, truncate_length=truncate_length, no_arr=noarr, no_str=nostr))
+    else:
+        print(msg)
 
 
-def message_lost_event_callback(message_lost_status):
+def _message_lost_event_callback(message_lost_status):
     print(
         'A message was lost!!!\n\ttotal count change:'
         f'{message_lost_status.total_count_change}'

ros2topic/test/test_cli.py

@@ -543,6 +543,67 @@ def test_truncate_length_topic_echo(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(self):
+        with self.launch_topic_command(
+            arguments=['echo', '/arrays', '--field', 'alignment_check'],
+        ) as topic_command:
+            assert topic_command.wait_for_output(functools.partial(
+                launch_testing.tools.expect_output, expected_lines=[
+                    '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_nested(self):
+        with self.launch_topic_command(
+            arguments=['echo', '/cmd_vel', '--field', 'twist.angular'],
+        ) as topic_command:
+            assert topic_command.wait_for_output(functools.partial(
+                launch_testing.tools.expect_output, expected_lines=[
+                    'x: 0.0',
+                    'y: 0.0',
+                    'z: 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(
+            arguments=['echo', '/arrays', '--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', '/'],
+        ) 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', '.'],
+        ) 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'],