[ros2topic] Add option to echo serialized messages (#470)

I found this feature useful when doing some debugging.

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

Author: jacobperron

ros2topic/ros2topic/verb/echo.py

@@ -74,6 +74,8 @@ def add_arguments(self, parser, cli_name):
             '--no-str', action='store_true', help="Don't print string fields of messages")
         parser.add_argument(
             '--lost-messages', action='store_true', help='Report when a message is lost')
+        parser.add_argument(
+            '--raw', action='store_true', help='Echo the raw binary representation')
 
     def main(self, *, args):
         return main(args)
@@ -101,7 +103,13 @@ def main(args):
                 'Could not determine the type for the passed topic')
 
         subscriber(
-            node, args.topic_name, message_type, callback, qos_profile, args.lost_messages)
+            node,
+            args.topic_name,
+            message_type,
+            callback,
+            qos_profile,
+            args.lost_messages,
+            args.raw)
 
 
 def subscriber(
@@ -110,7 +118,8 @@ def subscriber(
     message_type: MsgType,
     callback: Callable[[MsgType], Any],
     qos_profile: QoSProfile,
-    report_lost_messages: bool
+    report_lost_messages: bool,
+    raw: bool
 ) -> Optional[str]:
     """Initialize a node with a single subscription and spin."""
     event_callbacks = None
@@ -119,7 +128,12 @@ def subscriber(
             message_lost=message_lost_event_callback)
     try:
         node.create_subscription(
-            message_type, topic_name, callback, qos_profile, event_callbacks=event_callbacks)
+            message_type,
+            topic_name,
+            callback,
+            qos_profile,
+            event_callbacks=event_callbacks,
+            raw=raw)
     except UnsupportedEventTypeError:
         assert report_lost_messages
         print(
@@ -132,18 +146,23 @@ def subscriber(
 def subscriber_cb(truncate_length, noarr, nostr):
     def cb(msg):
         nonlocal truncate_length, noarr, nostr
-        print(
-            message_to_yaml(
-                msg, truncate_length=truncate_length, no_arr=noarr, no_str=nostr),
-            end='---\n')
+        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 subscriber_cb_csv(truncate_length, noarr, nostr):
     def cb(msg):
         nonlocal truncate_length, noarr, nostr
-        print(message_to_csv(msg, truncate_length=truncate_length,
-                             no_arr=noarr, no_str=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
 
 

ros2topic/test/test_echo_pub.py

@@ -12,6 +12,7 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+import functools
 import sys
 import unittest
 
@@ -309,3 +310,45 @@ def publish_message():
                     # Cleanup
                     self.node.destroy_timer(publish_timer)
                     self.node.destroy_publisher(publisher)
+
+    @launch_testing.markers.retry_on_failure(times=5)
+    def test_echo_raw(self, launch_service, proc_info, proc_output):
+        topic = '/clitest/topic/echo_raw'
+        publisher = self.node.create_publisher(String, topic, 10)
+        assert publisher
+
+        def publish_message():
+            publisher.publish(String(data='hello'))
+
+        publish_timer = self.node.create_timer(0.5, publish_message)
+
+        try:
+            command_action = ExecuteProcess(
+                cmd=['ros2', 'topic', 'echo', '--raw', topic, 'std_msgs/msg/String'],
+                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:
+                # The future won't complete - we will hit the timeout
+                self.executor.spin_until_future_complete(
+                    rclpy.task.Future(), timeout_sec=5
+                )
+                assert command.wait_for_output(functools.partial(
+                    launch_testing.tools.expect_output, expected_lines=[
+                        "b'\\x00\\x01\\x00\\x00\\x06\\x00\\x00\\x00hello\\x00\\x00\\x00'",
+                        '---',
+                    ], strict=True
+                ), timeout=10), 'Echo CLI did not print expected message'
+            assert command.wait_for_shutdown(timeout=10)
+
+        finally:
+            # Cleanup
+            self.node.destroy_timer(publish_timer)
+            self.node.destroy_publisher(publisher)