mcap_ros2: Fix serde of empty messages (#949)

### Public-Facing Changes

mcap_ros2: Fix serde of empty messages

### Description
In case a message definition definition is empty, ROS 2 adds a `uint8
structure_needs_at_least_one_member` field when converting to IDL, to
satisfy the requirement from IDL of not being empty. See also
https://design.ros2.org/articles/legacy_interface_definition.html

This patch inserts or skips that extra member when serializing or
deserializing respectively.

This PR is pretty much equivalent to
foxglove/rosmsg2-serialization#16

Fixes #943

python/mcap-ros2-support/mcap_ros2/__init__.py

@@ -1 +1 @@
-__version__ = "0.5.0"
+__version__ = "0.5.1"

python/mcap-ros2-support/mcap_ros2/_dynamic.py

@@ -1,5 +1,6 @@
 """ROS2 message definition parsing and message deserialization."""
 
+import os
 import re
 from io import BytesIO
 from types import SimpleNamespace
@@ -264,6 +265,13 @@ def _read_complex_type(
     )
     msg = Msg()
 
+    if len(msgdef.fields) == 0:
+        # In case a message definition definition is empty, ROS 2 adds a
+        # `uint8 structure_needs_at_least_one_member` field when converting to IDL,
+        # to satisfy the requirement from IDL of not being empty.
+        # See also https://design.ros2.org/articles/legacy_interface_definition.html
+        reader.uint8()
+
     for field in msgdef.fields:
         ftype = field.type
         if not ftype.is_primitive_type():
@@ -333,6 +341,13 @@ def _write_complex_type(
     ros2_msg: Any,
     writer: CdrWriter,
 ) -> None:
+    if len(fields) == 0:
+        # In case a message definition definition is empty, ROS 2 adds a
+        # `uint8 structure_needs_at_least_one_member` field when converting to IDL,
+        # to satisfy the requirement from IDL of not being empty.
+        # See also https://design.ros2.org/articles/legacy_interface_definition.html
+        writer.write_uint8(0x00)
+
     for field in fields:
         ftype = field.type
         if not ftype.is_primitive_type():
@@ -501,12 +516,14 @@ def _for_each_msgdef(
 ) -> None:
     cur_schema_name = schema_name
 
+    # Remove empty lines
+    schema_text = os.linesep.join([s for s in schema_text.splitlines() if s.strip()])
+
     # Split schema_text by separator lines containing at least 3 = characters
     # (e.g. "===") using a regular expression
     for cur_schema_text in re.split(r"^={3,}$", schema_text, flags=re.MULTILINE):
         cur_schema_text = cur_schema_text.strip()
-        if not cur_schema_text:
-            continue
+
         # Check for a "MSG: pkg_name/msg_name" line
         match = re.match(r"^MSG:\s+(\S+)$", cur_schema_text, flags=re.MULTILINE)
         if match:

python/mcap-ros2-support/tests/generate.py

@@ -20,6 +20,19 @@ def serialize(self, buff: BytesIO):
         buff.write(b"\x00")  # Null terminator
 
 
+class Empty:
+    _type = "std_msgs/msg/Empty"
+    _full_text = ""
+
+    def __init__(self):
+        pass
+
+    def serialize(self, buff: BytesIO):
+        buff.write(b"\x00\x03")  # CDR header (little-endian, 3)
+        buff.write(b"\x00\x00")  # Alignment padding
+        buff.write(b"\x00")  # uint8 structure_needs_at_least_one_member
+
+
 @contextlib.contextmanager
 def generate_sample_data():
     file = TemporaryFile("w+b")
@@ -33,15 +46,34 @@ def generate_sample_data():
     )
 
     for i in range(10):
-        s = String(data=f"string message {i}")
+        msg = String(data=f"string message {i}")
         buff = BytesIO()
-        s.serialize(buff)  # type: ignore
+        msg.serialize(buff)  # type: ignore
         writer.add_message(
             channel_id=string_channel_id,
             log_time=i * 1000,
             data=buff.getvalue(),
             publish_time=i * 1000,
         )
+
+    empty_schema_id = writer.register_schema(
+        name=Empty._type, encoding="ros2msg", data=Empty._full_text.encode()  # type: ignore
+    )
+    empty_channel_id = writer.register_channel(
+        topic="/empty", message_encoding="cdr", schema_id=empty_schema_id
+    )
+
+    for i in range(10):
+        msg = Empty()
+        buff = BytesIO()
+        msg.serialize(buff)  # type: ignore
+        writer.add_message(
+            channel_id=empty_channel_id,
+            log_time=i * 1000,
+            data=buff.getvalue(),
+            publish_time=i * 1000,
+        )
+
     writer.finish()
     file.seek(0)
 

python/mcap-ros2-support/tests/test_cdr_reader.py

@@ -14,6 +14,7 @@
     "000000000100000000000000000000000000000000000000000000000000000000000000"
     "00000000"
 )
+std_msgs__Empty = "0001000000"
 
 
 def test_parse_tfmessage():
@@ -81,6 +82,13 @@ def test_parse_parameter_event():
     assert reader.decoded_bytes() == len(data)
 
 
+def test_parse_empty_msg():
+    data = bytes.fromhex(std_msgs__Empty)
+    reader = CdrReader(data)
+    assert reader.uint8() == 0  # uint8 structure_needs_at_least_one_member
+    assert reader.decoded_bytes() == len(data)
+
+
 def test_read_big_endian():
     data = bytes.fromhex("000100001234000056789abcdef0000000000000")
     reader = CdrReader(data)

python/mcap-ros2-support/tests/test_ros2_decoder.py

@@ -9,9 +9,18 @@ def test_ros2_decoder():
     with generate_sample_data() as m:
         reader = make_reader(m, decoder_factories=[DecoderFactory()])
         count = 0
-        for index, (_, _, _, ros_msg) in enumerate(reader.iter_decoded_messages()):
+        for index, (_, _, _, ros_msg) in enumerate(
+            reader.iter_decoded_messages("/chatter")
+        ):
             assert ros_msg.data == f"string message {index}"
             assert ros_msg._type == "std_msgs/String"
             assert ros_msg._full_text == "# std_msgs/String\nstring data"
             count += 1
         assert count == 10
+
+        count = 0
+        for _, _, _, ros_msg in reader.iter_decoded_messages("/empty"):
+            assert ros_msg._type == "std_msgs/Empty"
+            assert ros_msg._full_text == "# std_msgs/Empty"
+            count += 1
+        assert count == 10

python/mcap-ros2-support/tests/test_ros2_writer.py

@@ -35,3 +35,27 @@ def test_write_messages():
         assert msg.message.log_time == index
         assert msg.message.publish_time == index
         assert msg.message.sequence == index
+
+
+def test_write_std_msgs_empty_messages():
+    output = BytesIO()
+    ros_writer = Ros2Writer(output=output)
+    schema = ros_writer.register_msgdef("std_msgs/msg/Empty", "")
+    for i in range(0, 10):
+        ros_writer.write_message(
+            topic="/test",
+            schema=schema,
+            message={},
+            log_time=i,
+            publish_time=i,
+            sequence=i,
+        )
+    ros_writer.finish()
+
+    output.seek(0)
+    for index, msg in enumerate(read_ros2_messages(output)):
+        assert msg.channel.topic == "/test"
+        assert msg.schema.name == "std_msgs/msg/Empty"
+        assert msg.message.log_time == index
+        assert msg.message.publish_time == index
+        assert msg.message.sequence == index