|
| 1 | +#!/usr/bin/env python3 |
| 2 | +"""cloud_shim.py — make the Go2's deskewed cloud digestible by Point-LIO. |
| 3 | +
|
| 4 | +The Go2 publishes /utlidar/cloud_deskewed as a plain PointCloud2 with only |
| 5 | +x,y,z,intensity (FLOAT32) — verified on hardware: no `ring`, no per-point |
| 6 | +`time`. Point-LIO's stock PointCloud2 handlers (Ouster/Velodyne/Hesai) all |
| 7 | +require ring+time and there is no generic-xyz path, so we repack each scan into |
| 8 | +the exact layout Point-LIO's VELO16 handler consumes — velodyne_ros::Point: |
| 9 | +x,y,z,intensity,time,ring — with time=0 and ring=0. |
| 10 | +
|
| 11 | +Why that's correct here: Unitree already motion-corrects (deskews) the cloud, |
| 12 | +so there is no intra-scan timing to preserve; with time==0 the VELO16 handler |
| 13 | +synthesizes a harmless yaw-based offset, and ring 0 (< scan_line) passes its |
| 14 | +filter. This keeps the upstream Point-LIO fork completely unmodified — all the |
| 15 | +Go2-specific adaptation lives here, in fast-to-iterate Python. |
| 16 | +""" |
| 17 | +import os |
| 18 | + |
| 19 | +import numpy as np |
| 20 | +import rclpy |
| 21 | +from rclpy.node import Node |
| 22 | +from rclpy.qos import QoSPresetProfiles |
| 23 | +from sensor_msgs.msg import PointCloud2, PointField |
| 24 | + |
| 25 | +IN_TOPIC = os.environ.get("LIDAR_TOPIC", "/utlidar/cloud_deskewed") |
| 26 | +OUT_TOPIC = os.environ.get("SHIM_OUT_TOPIC", "/point_lio/cloud") |
| 27 | + |
| 28 | +# velodyne_ros::Point wire layout Point-LIO expects (matched by field NAME by |
| 29 | +# pcl::fromROSMsg, so a compact self-consistent packing is fine). |
| 30 | +_OUT_STEP = 24 |
| 31 | +_OUT_FIELDS = [ |
| 32 | + PointField(name="x", offset=0, datatype=PointField.FLOAT32, count=1), |
| 33 | + PointField(name="y", offset=4, datatype=PointField.FLOAT32, count=1), |
| 34 | + PointField(name="z", offset=8, datatype=PointField.FLOAT32, count=1), |
| 35 | + PointField(name="intensity", offset=12, datatype=PointField.FLOAT32, count=1), |
| 36 | + PointField(name="time", offset=16, datatype=PointField.FLOAT32, count=1), |
| 37 | + PointField(name="ring", offset=20, datatype=PointField.UINT16, count=1), |
| 38 | +] |
| 39 | +_OUT_DTYPE = np.dtype({ |
| 40 | + "names": ["x", "y", "z", "intensity", "time", "ring"], |
| 41 | + "formats": ["<f4", "<f4", "<f4", "<f4", "<f4", "<u2"], |
| 42 | + "offsets": [0, 4, 8, 12, 16, 20], |
| 43 | + "itemsize": _OUT_STEP, |
| 44 | +}) |
| 45 | + |
| 46 | +_NP = { # PointField datatype -> numpy scalar |
| 47 | + PointField.INT8: "<i1", PointField.UINT8: "<u1", |
| 48 | + PointField.INT16: "<i2", PointField.UINT16: "<u2", |
| 49 | + PointField.INT32: "<i4", PointField.UINT32: "<u4", |
| 50 | + PointField.FLOAT32: "<f4", PointField.FLOAT64: "<f8", |
| 51 | +} |
| 52 | + |
| 53 | + |
| 54 | +def _read_field(raw, step, off, dt, n): |
| 55 | + """Read one named field out of a packed PointCloud2 buffer as float64.""" |
| 56 | + view = np.frombuffer(raw, dtype=np.uint8).reshape(n, step)[:, off:off + np.dtype(dt).itemsize] |
| 57 | + return view.copy().view(dt).reshape(n).astype(np.float64) |
| 58 | + |
| 59 | + |
| 60 | +class CloudShim(Node): |
| 61 | + def __init__(self): |
| 62 | + super().__init__("go2_cloud_shim") |
| 63 | + qos = QoSPresetProfiles.SENSOR_DATA.value |
| 64 | + self.pub = self.create_publisher(PointCloud2, OUT_TOPIC, qos) |
| 65 | + self.create_subscription(PointCloud2, IN_TOPIC, self.on_cloud, qos) |
| 66 | + self._warned = False |
| 67 | + self.get_logger().info(f"cloud_shim: {IN_TOPIC} -> {OUT_TOPIC} (velodyne layout)") |
| 68 | + |
| 69 | + def on_cloud(self, msg: PointCloud2): |
| 70 | + n = (len(msg.data) // msg.point_step) if msg.point_step else 0 |
| 71 | + if n == 0: |
| 72 | + return |
| 73 | + off = {f.name: (f.offset, _NP.get(f.datatype)) for f in msg.fields} |
| 74 | + if "x" not in off or "y" not in off or "z" not in off: |
| 75 | + if not self._warned: |
| 76 | + self.get_logger().error(f"input cloud lacks x/y/z fields: {list(off)}") |
| 77 | + self._warned = True |
| 78 | + return |
| 79 | + raw = bytes(msg.data) |
| 80 | + out = np.zeros(n, dtype=_OUT_DTYPE) |
| 81 | + out["x"] = _read_field(raw, msg.point_step, *off["x"], n) |
| 82 | + out["y"] = _read_field(raw, msg.point_step, *off["y"], n) |
| 83 | + out["z"] = _read_field(raw, msg.point_step, *off["z"], n) |
| 84 | + if "intensity" in off and off["intensity"][1]: |
| 85 | + out["intensity"] = _read_field(raw, msg.point_step, *off["intensity"], n) |
| 86 | + # Per-point time: a tiny monotonic ramp (0 -> 1 ms). The Go2 cloud is |
| 87 | + # already deskewed, so the scan is effectively instantaneous — but with |
| 88 | + # time==0 Point-LIO's VELO16 handler *synthesizes* a spinning-Velodyne |
| 89 | + # timing (omega=3.61 deg/ms), which is wrong for the non-repetitive |
| 90 | + # Mid-360 and can diverge the filter. A small >0 ramp makes |
| 91 | + # given_offset_time=true so it uses these ~0 values as-is instead. |
| 92 | + out["time"] = np.linspace(0.0, 1e-3, n, dtype=np.float32) |
| 93 | + # ring stays 0 (single logical ring; < scan_line so it passes the filter) |
| 94 | + |
| 95 | + m = PointCloud2() |
| 96 | + m.header = msg.header |
| 97 | + m.height = 1 |
| 98 | + m.width = n |
| 99 | + m.fields = _OUT_FIELDS |
| 100 | + m.is_bigendian = False |
| 101 | + m.point_step = _OUT_STEP |
| 102 | + m.row_step = _OUT_STEP * n |
| 103 | + m.is_dense = True |
| 104 | + m.data = out.tobytes() |
| 105 | + self.pub.publish(m) |
| 106 | + |
| 107 | + |
| 108 | +def main(): |
| 109 | + rclpy.init() |
| 110 | + rclpy.spin(CloudShim()) |
| 111 | + |
| 112 | + |
| 113 | +if __name__ == "__main__": |
| 114 | + main() |
0 commit comments