Add ros1 benchmark coverage for rosbags and mcap

benchmarks/benchmark_utils.py

@@ -2,7 +2,10 @@
 from pathlib import Path
 
 import numpy as np
-from rosbags.rosbag2 import StoragePlugin, Writer
+from mcap.writer import Writer as McapWriter
+from rosbags.rosbag1 import Writer as Rosbag1Writer
+from rosbags.rosbag2 import StoragePlugin
+from rosbags.rosbag2 import Writer as Rosbag2Writer
 from rosbags.typesys import Stores, get_typestore
 
 
@@ -22,7 +25,7 @@ def create_test_mcap(path: Path, message_count: int = 1000, seed: int = 0) -> Pa
     Twist = typestore.types["geometry_msgs/msg/Twist"]
     Vector3 = typestore.types["geometry_msgs/msg/Vector3"]
 
-    with Writer(path, version=9, storage_plugin=StoragePlugin.MCAP) as writer:
+    with Rosbag2Writer(path, version=9, storage_plugin=StoragePlugin.MCAP) as writer:
         odom_conn = writer.add_connection("/odom", Odometry.__msgtype__, typestore=typestore)
         for i in range(message_count):
             timestamp = int(i * 1_500_000_000)
@@ -70,3 +73,123 @@ def create_test_mcap(path: Path, message_count: int = 1000, seed: int = 0) -> Pa
             writer.write(odom_conn, timestamp, typestore.serialize_cdr(odom, Odometry.__msgtype__))
 
     return next(Path(path).rglob("*.mcap"))
+
+
+def generate_ros1_odometries(message_count: int = 1000, seed: int = 0) -> list:
+    """Generate ROS 1 Odometry messages."""
+
+    rng = random.Random(seed)
+
+    typestore = get_typestore(Stores.ROS1_NOETIC)
+    Odometry = typestore.types["nav_msgs/msg/Odometry"]
+    Header = typestore.types["std_msgs/msg/Header"]
+    Time = typestore.types["builtin_interfaces/msg/Time"]
+    PoseWithCovariance = typestore.types["geometry_msgs/msg/PoseWithCovariance"]
+    Pose = typestore.types["geometry_msgs/msg/Pose"]
+    Point = typestore.types["geometry_msgs/msg/Point"]
+    Quaternion = typestore.types["geometry_msgs/msg/Quaternion"]
+    TwistWithCovariance = typestore.types["geometry_msgs/msg/TwistWithCovariance"]
+    Twist = typestore.types["geometry_msgs/msg/Twist"]
+    Vector3 = typestore.types["geometry_msgs/msg/Vector3"]
+
+    messages: list = []
+    for i in range(message_count):
+        timestamp = int(i * 1_500_000_000)
+        odom = Odometry(
+            header=Header(
+                seq=i,
+                stamp=Time(
+                    sec=timestamp // 1_000_000_000,
+                    nanosec=timestamp % 1_000_000_000,
+                ),
+                frame_id="map",
+            ),
+            child_frame_id="base_link",
+            pose=PoseWithCovariance(
+                pose=Pose(
+                    position=Point(
+                        x=rng.random(),
+                        y=rng.random(),
+                        z=rng.random(),
+                    ),
+                    orientation=Quaternion(
+                        x=rng.random(),
+                        y=rng.random(),
+                        z=rng.random(),
+                        w=rng.random(),
+                    ),
+                ),
+                covariance=np.array([rng.random() for _ in range(36)], dtype=np.float64),
+            ),
+            twist=TwistWithCovariance(
+                twist=Twist(
+                    linear=Vector3(
+                        x=rng.random(),
+                        y=rng.random(),
+                        z=rng.random(),
+                    ),
+                    angular=Vector3(
+                        x=rng.random(),
+                        y=rng.random(),
+                        z=rng.random(),
+                    ),
+                ),
+                covariance=np.array([rng.random() for _ in range(36)], dtype=np.float64),
+            ),
+        )
+        messages.append(odom)
+
+    return messages
+
+
+def create_test_ros1_bag(path: Path, message_count: int = 1000, seed: int = 0) -> Path:
+    """Create a ROS 1 rosbag with Odometry messages."""
+
+    typestore = get_typestore(Stores.ROS1_NOETIC)
+    msgtype = "nav_msgs/msg/Odometry"
+    bag_path = path.with_suffix(".bag")
+
+    with Rosbag1Writer(bag_path) as writer:
+        connection = writer.add_connection("/odom", msgtype, typestore=typestore)
+        for i, ros_msg in enumerate(generate_ros1_odometries(message_count, seed)):
+            timestamp = int(i * 1_500_000_000)
+            serialized_msg = typestore.serialize_ros1(ros_msg, msgtype)
+            writer.write(connection, timestamp, serialized_msg)
+
+    return bag_path
+
+
+def create_test_ros1_mcap(path: Path, message_count: int = 1000, seed: int = 0) -> Path:
+    """Create an MCAP file with ROS 1 Odometry messages."""
+
+    typestore = get_typestore(Stores.ROS1_NOETIC)
+    msgtype = "nav_msgs/msg/Odometry"
+    ros1_msgtype = msgtype.replace("/msg/", "/")
+    msgdef, _ = typestore.generate_msgdef(msgtype)
+    mcap_path = path.with_suffix(".mcap")
+
+    with open(mcap_path, "wb") as stream:
+        writer = McapWriter(stream)
+        writer.start(profile="ros1", library="pybag-benchmarks")
+        schema_id = writer.register_schema(
+            name=ros1_msgtype,
+            data=msgdef.encode(),
+            encoding="ros1msg",
+        )
+        channel_id = writer.register_channel(
+            topic="/odom",
+            schema_id=schema_id,
+            message_encoding="ros1",
+        )
+        for i, ros_msg in enumerate(generate_ros1_odometries(message_count, seed)):
+            timestamp = int(i * 1_500_000_000)
+            serialized_msg = typestore.serialize_ros1(ros_msg, msgtype)
+            writer.add_message(
+                channel_id=channel_id,
+                log_time=timestamp,
+                publish_time=timestamp,
+                data=serialized_msg,
+            )
+        writer.finish()
+
+    return mcap_path

benchmarks/test_ros1_reader_decoded.py

@@ -0,0 +1,40 @@
+from collections import deque
+from pathlib import Path
+from tempfile import TemporaryDirectory
+from typing import Any, Iterator
+
+from mcap.reader import make_reader
+from mcap_ros1.decoder import DecoderFactory
+from pytest_benchmark.fixture import BenchmarkFixture
+from rosbags.highlevel import AnyReader
+from rosbags.typesys import Stores, get_typestore
+
+from .benchmark_utils import create_test_ros1_bag, create_test_ros1_mcap
+
+
+def read_decoded_with_rosbags(bag: Path) -> Iterator[Any]:
+    typestore = get_typestore(Stores.ROS1_NOETIC)
+    with AnyReader([bag], default_typestore=typestore) as reader:
+        for connection, _timestamp, data in reader.messages():
+            yield reader.deserialize(data, connection.msgtype)
+
+
+def read_decoded_with_official(mcap: Path) -> Iterator[Any]:
+    with open(mcap, "rb") as stream:
+        reader = make_reader(stream, decoder_factories=[DecoderFactory()])
+        for _schema, _channel, _message, ros_msg in reader.iter_decoded_messages(
+            log_time_order=True,
+        ):
+            yield ros_msg
+
+
+def test_rosbags_read_decoded(benchmark: BenchmarkFixture) -> None:
+    with TemporaryDirectory() as tmpdir:
+        bag = create_test_ros1_bag(Path(tmpdir) / "ros1")
+        benchmark(lambda: deque(read_decoded_with_rosbags(bag), maxlen=0))
+
+
+def test_official_read_decoded(benchmark: BenchmarkFixture) -> None:
+    with TemporaryDirectory() as tmpdir:
+        mcap = create_test_ros1_mcap(Path(tmpdir) / "ros1")
+        benchmark(lambda: deque(read_decoded_with_official(mcap), maxlen=0))

benchmarks/test_ros1_reader_raw.py

@@ -0,0 +1,35 @@
+from collections import deque
+from pathlib import Path
+from tempfile import TemporaryDirectory
+from typing import Iterator
+
+from mcap.reader import make_reader
+from pytest_benchmark.fixture import BenchmarkFixture
+from rosbags.highlevel import AnyReader
+
+from .benchmark_utils import create_test_ros1_bag, create_test_ros1_mcap
+
+
+def read_raw_with_rosbags(bag: Path) -> Iterator[bytes]:
+    with AnyReader([bag]) as reader:
+        for _connection, _timestamp, data in reader.messages():
+            yield data
+
+
+def read_raw_with_official(mcap: Path) -> Iterator[bytes]:
+    with open(mcap, "rb") as stream:
+        reader = make_reader(stream)
+        for _schema, _channel, message in reader.iter_messages(log_time_order=True):
+            yield message.data
+
+
+def test_rosbags_read_raw(benchmark: BenchmarkFixture) -> None:
+    with TemporaryDirectory() as tmpdir:
+        bag = create_test_ros1_bag(Path(tmpdir) / "ros1")
+        benchmark(lambda: deque(read_raw_with_rosbags(bag), maxlen=0))
+
+
+def test_official_read_raw(benchmark: BenchmarkFixture) -> None:
+    with TemporaryDirectory() as tmpdir:
+        mcap = create_test_ros1_mcap(Path(tmpdir) / "ros1")
+        benchmark(lambda: deque(read_raw_with_official(mcap), maxlen=0))

benchmarks/test_ros1_writer.py

@@ -0,0 +1,76 @@
+from itertools import count
+from pathlib import Path
+from tempfile import TemporaryDirectory
+from typing import Iterable
+
+from mcap.writer import Writer as McapWriter
+from pytest_benchmark.fixture import BenchmarkFixture
+from rosbags.rosbag1 import Writer as Rosbag1Writer
+from rosbags.typesys import Stores, get_typestore
+
+from .benchmark_utils import generate_ros1_odometries
+
+
+def test_rosbags_write(benchmark: BenchmarkFixture) -> None:
+    typestore = get_typestore(Stores.ROS1_NOETIC)
+    msgtype = "nav_msgs/msg/Odometry"
+
+    def _write_with_rosbags(path: Path, messages: Iterable) -> None:
+        with Rosbag1Writer(path) as writer:
+            connection = writer.add_connection("/odom", msgtype, typestore=typestore)
+            for i, ros_msg in enumerate(messages):
+                timestamp = int(i * 1_500_000_000)
+                serialized = typestore.serialize_ros1(ros_msg, msgtype)
+                writer.write(connection, timestamp, serialized)
+
+    messages = generate_ros1_odometries()
+    with TemporaryDirectory() as tmpdir:
+        counter = count()
+        benchmark(
+            lambda: _write_with_rosbags(
+                Path(tmpdir) / f"rosbags_{next(counter)}.bag",
+                messages,
+            )
+        )
+
+
+def test_official_write(benchmark: BenchmarkFixture) -> None:
+    typestore = get_typestore(Stores.ROS1_NOETIC)
+    msgtype = "nav_msgs/msg/Odometry"
+    ros1_msgtype = msgtype.replace("/msg/", "/")
+    msgdef, _ = typestore.generate_msgdef(msgtype)
+
+    def _write_with_official(path: Path, messages: Iterable) -> None:
+        with open(path, "wb") as stream:
+            writer = McapWriter(stream)
+            writer.start(profile="ros1", library="pybag-benchmarks")
+            schema_id = writer.register_schema(
+                name=ros1_msgtype,
+                data=msgdef.encode(),
+                encoding="ros1msg",
+            )
+            channel_id = writer.register_channel(
+                topic="/odom",
+                schema_id=schema_id,
+                message_encoding="ros1",
+            )
+            for i, ros_msg in enumerate(messages):
+                timestamp = int(i * 1_500_000_000)
+                serialized = typestore.serialize_ros1(ros_msg, msgtype)
+                writer.add_message(
+                    channel_id=channel_id,
+                    log_time=timestamp,
+                    publish_time=timestamp,
+                    data=serialized,
+                )
+            writer.finish()
+
+    messages = generate_ros1_odometries()
+    with TemporaryDirectory() as tmpdir:
+        counter = count()
+        benchmark(
+            lambda: _write_with_official(
+                Path(tmpdir) / f"official_{next(counter)}.mcap",
+                messages,
+            )
+        )

pyproject.toml

@@ -24,6 +24,7 @@ test = [
     "pytest-benchmark>=4.0.0",
     "mcap>=1.2.2",
     "mcap-ros2-support>=0.5.5",
+    "mcap-ros1-support>=0.5.5",
     "rosbags>=0.10.10",
     "numpy>=2.2.3",
 ]

tests/test_mcap_reader.py

@@ -1,6 +1,6 @@
 """Tests for the MCAP reader."""
-import random
 import logging
+import random
 from pathlib import Path
 from tempfile import TemporaryDirectory