fix(rosbag): widen Reader.messages start by 1ns to dodge rosbags MCAP chunk-filter off-by-one (#279)

* fix(rosbag): widen Reader.messages start by 1ns to dodge rosbags MCAP chunk-filter off-by-one

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>

* style(pre-commit): autofix

---------

Co-authored-by: Claude Opus 4.6 <noreply@anthropic.com>
Co-authored-by: hakuturu583 <10348912+hakuturu583@users.noreply.github.com>

Author: hakuturu583

t4_devkit/rosbag/pandar_decoder.py

@@ -82,7 +82,8 @@ def __post_init__(self) -> None:
         object.__setattr__(self, "cos_el", np.cos(el_rad))
         object.__setattr__(self, "sin_el", np.sin(el_rad))
         object.__setattr__(
-            self, "azimuth_offset_rad",
+            self,
+            "azimuth_offset_rad",
             np.radians(np.array(self.azimuth_offset_deg, dtype=np.float32)),
         )
 
@@ -137,11 +138,13 @@ def __post_init__(self) -> None:
 # Model lookup by sensor type name.
 HESAI_MODELS: dict[str, _HesaiModelConfig] = {
     "XT32": _HesaiModelConfig(
-        name="XT32", elevation_deg=_XT32_ELEVATION_DEG,
+        name="XT32",
+        elevation_deg=_XT32_ELEVATION_DEG,
         azimuth_offset_deg=_XT32_AZIMUTH_OFFSET_DEG,
     ),
     "OT128": _HesaiModelConfig(
-        name="OT128", elevation_deg=_OT128_ELEVATION_DEG,
+        name="OT128",
+        elevation_deg=_OT128_ELEVATION_DEG,
         azimuth_offset_deg=_OT128_AZIMUTH_OFFSET_DEG,
     ),
 }

t4_devkit/rosbag/reader.py

@@ -37,11 +37,13 @@
 
 def _quat_to_matrix(x: float, y: float, z: float, w: float) -> np.ndarray:
     """Convert quaternion (x, y, z, w) to a 3x3 rotation matrix."""
-    return np.array([
-        [1 - 2 * (y * y + z * z), 2 * (x * y - w * z), 2 * (x * z + w * y)],
-        [2 * (x * y + w * z), 1 - 2 * (x * x + z * z), 2 * (y * z - w * x)],
-        [2 * (x * z - w * y), 2 * (y * z + w * x), 1 - 2 * (x * x + y * y)],
-    ])
+    return np.array(
+        [
+            [1 - 2 * (y * y + z * z), 2 * (x * y - w * z), 2 * (x * z + w * y)],
+            [2 * (x * y + w * z), 1 - 2 * (x * x + z * z), 2 * (y * z - w * x)],
+            [2 * (x * z - w * y), 2 * (y * z + w * x), 1 - 2 * (x * x + y * y)],
+        ]
+    )
 
 
 def _make_transform(R: np.ndarray, t: np.ndarray) -> np.ndarray:
@@ -223,7 +225,8 @@ def __init__(
                     continue
                 try:
                     self._channel_sensor2ego[m.channel] = _resolve_chain(
-                        self._tf_tree, m.frame_id,
+                        self._tf_tree,
+                        m.frame_id,
                     )
                 except ValueError:
                     logger.warning(
@@ -366,9 +369,15 @@ def get_pointcloud(
             raise ValueError(f"No connections found for topic '{topic}' (channel '{channel}')")
 
         with self._lock:
+            # Workaround for a rosbags MCAP off-by-one (storage_mcap.py:591):
+            # `start < x.message_end_time` excludes the chunk when start equals
+            # the chunk's last-message ts (which is inclusive), causing 1ns-window
+            # queries at chunk boundaries to return empty. Widening start by 1ns
+            # selects the correct chunk; the [start, stop) interval still uniquely
+            # identifies the target packet.
             for conn, ts_ns, rawdata in self._reader.messages(
                 connections=conns_for_topic,
-                start=target_ns,
+                start=target_ns - 1,
                 stop=target_ns + 1,
             ):
                 msg = self._typestore.deserialize_cdr(rawdata, conn.msgtype)

tests/rosbag/test_reader.py

@@ -136,6 +136,32 @@ def test_context_manager(self, bag_with_pointclouds: Path) -> None:
         with Rosbag2Reader(str(bag_with_pointclouds)) as reader:
             assert len(reader.channels) > 0
 
+    def test_get_pointcloud_start_widened(
+        self,
+        bag_with_pointclouds: Path,
+        monkeypatch: pytest.MonkeyPatch,
+    ) -> None:
+        """Regression: start is widened by 1ns to dodge rosbags MCAP chunk-filter off-by-one."""
+        mapping = [TopicMapping(channel="LIDAR_TOP", topic="/sensing/lidar/top/pointcloud")]
+        with Rosbag2Reader(str(bag_with_pointclouds), topic_mapping=mapping) as reader:
+            captured_starts: list[int] = []
+            original_messages = reader._reader.messages
+
+            def patched_messages(*args, **kwargs):
+                if "start" in kwargs:
+                    captured_starts.append(kwargs["start"])
+                return original_messages(*args, **kwargs)
+
+            monkeypatch.setattr(reader._reader, "messages", patched_messages)
+
+            target_ts_us = 1_704_067_200_000_000
+            target_ts_ns = target_ts_us * 1_000
+            pc = reader.get_pointcloud("LIDAR_TOP", target_ts_us)
+
+            assert pc.points.shape == (4, 3)
+            assert len(captured_starts) == 1
+            assert captured_starts[0] == target_ts_ns - 1
+
     def test_multiple_timestamps(self, bag_with_pointclouds: Path) -> None:
         mapping = [TopicMapping(channel="LIDAR_TOP", topic="/sensing/lidar/top/pointcloud")]
         with Rosbag2Reader(str(bag_with_pointclouds), topic_mapping=mapping) as reader: