fix: use TF static transform for PandarScan sensor-to-base_link conversion

The Hesai sensor frame uses native coordinates and the /tf_static tree
includes the full rotation from sensor frame to base_link. Revert the
incorrect Hesai-to-ROS axis swap in the decoder and instead read
/tf_static from the rosbag to compute the composed transform. Add
frame_id parameter to TopicMapping so users can specify the sensor's
TF frame (e.g. "hesai_top") for automatic coordinate transformation.

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

Author: hakuturu583

docs/tutorials/render.md

@@ -60,7 +60,7 @@ For `PointCloud2` topics, a simple dict mapping is sufficient:
 ... )
 ```
 
-For `PandarScan` topics, use `TopicMapping` with `sensor_type` to specify the Hesai sensor model (`"XT32"` or `"OT128"`):
+For `PandarScan` topics, use `TopicMapping` with `sensor_type` to specify the Hesai sensor model (`"XT32"` or `"OT128"`). Use `frame_id` to specify the TF frame so that the decoded point cloud is automatically transformed to `base_link`:
 
 ```python
 >>> from t4_devkit import T4Devkit
@@ -74,6 +74,7 @@ For `PandarScan` topics, use `TopicMapping` with `sensor_type` to specify the He
 ...             channel="LIDAR_CONCAT",
 ...             topic="/sensing/lidar/top/pandar_packets",
 ...             sensor_type="OT128",
+...             frame_id="hesai_top",
 ...         ),
 ...     ],
 ... )
@@ -82,7 +83,7 @@ For `PandarScan` topics, use `TopicMapping` with `sensor_type` to specify the He
 >>> t4.render_pointcloud()
 ```
 
-If `topic_mapping` is omitted, `PointCloud2` topics are auto-detected from the rosbag. `PandarScan` topics always require explicit `topic_mapping` with `sensor_type`.
+If `topic_mapping` is omitted, `PointCloud2` topics are auto-detected from the rosbag. `PandarScan` topics always require explicit `topic_mapping` with `sensor_type`. The `frame_id` parameter is optional but recommended — without it, points remain in the sensor's native coordinate frame.
 
 ### Save Recording
 

t4_devkit/rosbag/pandar_decoder.py

@@ -181,8 +181,8 @@ def _decode_packet(
         config: Model config for the sensor type.
 
     Returns:
-        Points array with shape ``(4, N)`` in ROS sensor frame
-        (x=forward, y=left, z=up).
+        Points array with shape ``(4, N)`` in Hesai native frame
+        (x=d*cos*sin, y=d*cos*cos, z=d*sin).
 
     Raises:
         ValueError: If the packet channel count doesn't match the config.
@@ -264,11 +264,11 @@ def _decode_packet(
 
     azimuths_rad = np.radians(azimuths_raw.astype(np.float32) / 100.0)
 
-    # Convert from Hesai native (x=right, y=forward, z=up) to
-    # ROS sensor frame (x=forward, y=left, z=up).
+    # Hesai native frame: x = d*cos(el)*sin(az), y = d*cos(el)*cos(az), z = d*sin(el)
+    # The TF tree (hesai_top -> base_link) handles the frame conversion.
     xy_dist = distances * cos_el
-    x = xy_dist * np.cos(azimuths_rad[:, np.newaxis])
-    y = -xy_dist * np.sin(azimuths_rad[:, np.newaxis])
+    x = xy_dist * np.sin(azimuths_rad[:, np.newaxis])
+    y = xy_dist * np.cos(azimuths_rad[:, np.newaxis])
     z = distances * sin_el
 
     return np.stack([x[valid], y[valid], z[valid], reflectivities[valid]], axis=0)

t4_devkit/rosbag/reader.py

@@ -6,6 +6,7 @@
 from pathlib import Path
 from typing import TYPE_CHECKING
 
+import numpy as np
 from rosbags.rosbag2 import Reader
 from rosbags.typesys import Stores, get_typestore
 
@@ -26,12 +27,73 @@
 
 _POINTCLOUD2_MSGTYPE = "sensor_msgs/msg/PointCloud2"
 _PANDARSCAN_MSGTYPE = "pandar_msgs/msg/PandarScan"
+_TF_STATIC_TOPIC = "/tf_static"
 
 _SUPPORTED_MSGTYPES = {_POINTCLOUD2_MSGTYPE, _PANDARSCAN_MSGTYPE}
 
 logger = logging.getLogger(__name__)
 
 
+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)],
+    ])
+
+
+def _build_tf_to_base(typestore: object, reader: Reader) -> dict[str, tuple[np.ndarray, np.ndarray]]:
+    """Read /tf_static and compute transforms from each frame to base_link.
+
+    Returns:
+        Dict mapping ``frame_id`` to ``(R, t)`` where ``R`` is a 3x3 rotation
+        matrix and ``t`` is a 3-element translation vector, representing the
+        transform from that frame to ``base_link``.
+    """
+    tf_conns = [c for c in reader.connections if c.topic == _TF_STATIC_TOPIC]
+    if not tf_conns:
+        return {}
+
+    # Parse the TF tree: child_frame -> (parent_frame, R, t)
+    tree: dict[str, tuple[str, np.ndarray, np.ndarray]] = {}
+    for conn, _ts, rawdata in reader.messages(connections=tf_conns):
+        msg = typestore.deserialize_cdr(rawdata, conn.msgtype)
+        for tf in msg.transforms:
+            parent = tf.header.frame_id
+            child = tf.child_frame_id
+            tr = tf.transform.translation
+            rot = tf.transform.rotation
+            R = _quat_to_matrix(rot.x, rot.y, rot.z, rot.w)
+            t = np.array([tr.x, tr.y, tr.z])
+            tree[child] = (parent, R, t)
+
+    # Compose transforms from each frame to base_link
+    result: dict[str, tuple[np.ndarray, np.ndarray]] = {}
+    result["base_link"] = (np.eye(3), np.zeros(3))
+
+    def _resolve(frame: str) -> tuple[np.ndarray, np.ndarray] | None:
+        if frame in result:
+            return result[frame]
+        if frame not in tree:
+            return None
+        parent, R_child, t_child = tree[frame]
+        parent_tf = _resolve(parent)
+        if parent_tf is None:
+            return None
+        R_parent, t_parent = parent_tf
+        # T_base = T_parent * T_child: p_base = R_parent*(R_child*p + t_child) + t_parent
+        R = R_parent @ R_child
+        t = R_parent @ t_child + t_parent
+        result[frame] = (R, t)
+        return result[frame]
+
+    for frame in tree:
+        _resolve(frame)
+
+    return result
+
+
 class Rosbag2Reader:
     """Reader for rosbag2 files that provides LiDAR point cloud data.
 
@@ -122,6 +184,25 @@ def __init__(
         for conn in self._connections:
             self._topic_connections.setdefault(conn.topic, []).append(conn)
 
+        # Read /tf_static and build transforms to base_link
+        self._tf_to_base = _build_tf_to_base(self._typestore, self._reader)
+
+        # For PandarScan topics with frame_id specified, look up TF transform
+        self._channel_tf: dict[str, tuple[np.ndarray, np.ndarray]] = {}
+        if topic_mapping is not None:
+            for m in topic_mapping:
+                if m.frame_id is None:
+                    continue
+                if m.frame_id in self._tf_to_base:
+                    self._channel_tf[m.channel] = self._tf_to_base[m.frame_id]
+                else:
+                    logger.warning(
+                        "Channel '%s': frame_id='%s' not found in /tf_static. "
+                        "Points will be in sensor frame.",
+                        m.channel,
+                        m.frame_id,
+                    )
+
         # Build timestamp index: channel -> sorted list of timestamp_ns
         # Also build a cached list of timestamp_us per channel for bisect lookups
         self._timestamp_ns: dict[str, list[int]] = {}
@@ -230,7 +311,13 @@ def get_pointcloud(
             ):
                 msg = self._typestore.deserialize_cdr(rawdata, conn.msgtype)
                 if conn.msgtype == _PANDARSCAN_MSGTYPE:
-                    return pandarscan_to_lidar(msg, self._channel_to_sensor_type[channel])
+                    pc = pandarscan_to_lidar(msg, self._channel_to_sensor_type[channel])
+                    tf = self._channel_tf.get(channel)
+                    if tf is not None:
+                        R, t = tf
+                        xyz = pc.points[:3, :]
+                        pc.points[:3, :] = R @ xyz + t[:, np.newaxis]
+                    return pc
                 return pointcloud2_to_lidar(msg)
 
         raise ValueError(

t4_devkit/rosbag/topic_mapping.py

@@ -21,11 +21,15 @@ class TopicMapping:
         sensor_type (str | None): Hesai sensor model name for PandarScan topics
             (e.g. ``"OT128"``, ``"XT32"``).
             Required when the topic type is ``pandar_msgs/msg/PandarScan``.
+        frame_id (str | None): TF frame ID of the sensor (e.g. ``"hesai_top"``).
+            When specified and ``/tf_static`` is available in the rosbag,
+            the decoded point cloud is transformed from this frame to ``base_link``.
     """
 
     channel: str = field(validator=[validators.instance_of(str), validators.min_len(1)])
     topic: str = field(validator=[validators.instance_of(str), _validate_topic_name])
     sensor_type: str | None = field(default=None)
+    frame_id: str | None = field(default=None)
 
     @staticmethod
     def from_dict(mapping: dict[str, str]) -> list[TopicMapping]:

tests/rosbag/test_pandar_decoder.py

@@ -88,9 +88,9 @@ def test_decode_single_block(self) -> None:
         assert points.shape[0] == 4
         assert points.shape[1] == laser_num
 
-        # At azimuth=0° in ROS frame: x = d*cos(el)*cos(0) = d*cos(el), y = -d*cos(el)*sin(0) = 0
-        # All y values should be ~0
-        np.testing.assert_array_almost_equal(points[1, :], 0.0, decimal=5)
+        # At azimuth=0°: x = d*cos(el)*sin(0) = 0, y = d*cos(el)*cos(0) = d*cos(el)
+        # All x values should be ~0
+        np.testing.assert_array_almost_equal(points[0, :], 0.0, decimal=5)
         # All intensities should be 128
         np.testing.assert_array_almost_equal(points[3, :], 128.0)
 
@@ -113,7 +113,7 @@ def test_decode_zero_distance_filtered(self) -> None:
         assert points.shape[1] == 0
 
     def test_decode_azimuth_90(self) -> None:
-        """Test decoding at azimuth=90° - y should be negative (left-hand convention)."""
+        """Test decoding at azimuth=90° - x should be positive for 0° elevation."""
         laser_num = 32
         distances = [[2500] * laser_num]  # 10m
         reflectivities = [[100] * laser_num]
@@ -129,16 +129,16 @@ def test_decode_azimuth_90(self) -> None:
         config = HESAI_MODELS["XT32"]
         points = _decode_packet(packet, config)
 
-        # At azimuth=90° in ROS frame: x = d*cos(el)*cos(90°) ≈ 0, y = -d*cos(el)*sin(90°) = -d*cos(el)
-        # Channel 15 (elevation=0°): x ≈ 0, y = -10.0
+        # At azimuth=90°: x = d*cos(el)*sin(90°) = d*cos(el), y = d*cos(el)*cos(90°) ≈ 0
+        # Channel 15 (elevation=0°): x = 10.0, y ≈ 0
         ch15_idx = None
         for i in range(points.shape[1]):
             if abs(points[2, i]) < 0.01:  # z ≈ 0 means elevation ≈ 0
                 ch15_idx = i
                 break
         assert ch15_idx is not None
-        assert points[0, ch15_idx] == pytest.approx(0.0, abs=0.1)
-        assert points[1, ch15_idx] == pytest.approx(-10.0, abs=0.1)
+        assert points[0, ch15_idx] == pytest.approx(10.0, abs=0.1)
+        assert points[1, ch15_idx] == pytest.approx(0.0, abs=0.1)
 
     def test_multiple_blocks(self) -> None:
         """Test decoding a packet with multiple blocks."""