From f954e86a3768629ae20aa75f12790e797aabfc83 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Thu, 12 Feb 2026 03:25:39 +0800 Subject: [PATCH 01/15] mid360 livox integration --- dimos/hardware/sensors/lidar/__init__.py | 0 .../hardware/sensors/lidar/livox/__init__.py | 0 dimos/hardware/sensors/lidar/livox/mid360.py | 493 ++++++++++++++++ dimos/hardware/sensors/lidar/livox/sdk.py | 534 ++++++++++++++++++ dimos/hardware/sensors/lidar/module.py | 147 +++++ dimos/hardware/sensors/lidar/spec.py | 43 ++ dimos/msgs/sensor_msgs/Imu.py | 123 ++++ dimos/msgs/sensor_msgs/PointCloud2.py | 21 +- dimos/msgs/sensor_msgs/__init__.py | 2 + dimos/spec/perception.py | 12 +- dimos/spec/utils.py | 26 + dimos/visualization/rerun/bridge.py | 34 +- 12 files changed, 1394 insertions(+), 41 deletions(-) create mode 100644 dimos/hardware/sensors/lidar/__init__.py create mode 100644 dimos/hardware/sensors/lidar/livox/__init__.py create mode 100644 dimos/hardware/sensors/lidar/livox/mid360.py create mode 100644 dimos/hardware/sensors/lidar/livox/sdk.py create mode 100644 dimos/hardware/sensors/lidar/module.py create mode 100644 dimos/hardware/sensors/lidar/spec.py create mode 100644 dimos/msgs/sensor_msgs/Imu.py diff --git a/dimos/hardware/sensors/lidar/__init__.py b/dimos/hardware/sensors/lidar/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/dimos/hardware/sensors/lidar/livox/__init__.py b/dimos/hardware/sensors/lidar/livox/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/dimos/hardware/sensors/lidar/livox/mid360.py b/dimos/hardware/sensors/lidar/livox/mid360.py new file mode 100644 index 0000000000..9c06c4af20 --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/mid360.py @@ -0,0 +1,493 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Livox Mid-360 LiDAR hardware driver using Livox SDK2 ctypes bindings.""" + +from __future__ import annotations + +from dataclasses import dataclass, field +from functools import cache +import json +import logging +from queue import Empty, Queue +import tempfile +import threading +import time +from typing import TYPE_CHECKING, Any + +import numpy as np +import open3d as o3d # type: ignore[import-untyped] +import open3d.core as o3c # type: ignore[import-untyped] +from reactivex import create + +from dimos.hardware.sensors.lidar.livox.sdk import ( + AsyncControlCallbackType, + ImuDataCallbackType, + InfoChangeCallbackType, + LivoxLidarDeviceType, + LivoxLidarEthernetPacket, + LivoxLidarInfo, + LivoxLidarPointDataType, + LivoxLidarWorkMode, + LivoxSDK, + PointCloudCallbackType, + get_packet_timestamp_ns, + parse_cartesian_high_points, + parse_cartesian_low_points, + parse_imu_data, +) +from dimos.hardware.sensors.lidar.spec import LidarConfig, LidarHardware +from dimos.msgs.geometry_msgs import Vector3 +from dimos.msgs.sensor_msgs.Imu import Imu +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.utils.reactive import backpressure + +if TYPE_CHECKING: + from reactivex.observable import Observable + +logger = logging.getLogger(__name__) + +# Gravity constant for converting accelerometer data from g to m/s^2 +GRAVITY_MS2 = 9.80665 + + +@dataclass +class LivoxMid360Config(LidarConfig): + """Configuration for the Livox Mid-360 LiDAR.""" + + host_ip: str = "192.168.1.5" + lidar_ips: list[str] = field(default_factory=lambda: ["192.168.1.155"]) + frequency: float = 10.0 # Hz, point cloud output rate + frame_id: str = "lidar_link" + frame_id_prefix: str | None = None + enable_imu: bool = True + sdk_lib_path: str | None = None + + # SDK port configuration + cmd_data_port: int = 56100 + push_msg_port: int = 56200 + point_data_port: int = 56300 + imu_data_port: int = 56400 + log_data_port: int = 56500 + host_cmd_data_port: int = 56101 + host_push_msg_port: int = 56201 + host_point_data_port: int = 56301 + host_imu_data_port: int = 56401 + host_log_data_port: int = 56501 + + # Socket buffer size (bytes) to avoid packet loss at high data rates + recv_buf_size: int = 4 * 1024 * 1024 # 4 MB + + +class LivoxMid360(LidarHardware["LivoxMid360Config"]): + """Livox Mid-360 LiDAR driver using SDK2 ctypes bindings. + + Produces Observable[PointCloud2] at the configured frame rate (~10 Hz), + and optionally Observable[Imu] at ~200 Hz. + + Uses a thread-safe queue to pass data from SDK C-thread callbacks + to Python consumer threads that build messages. + """ + + default_config = LivoxMid360Config + + def __init__(self, *args: object, **kwargs: object) -> None: + super().__init__(*args, **kwargs) + self._sdk: LivoxSDK | None = None + self._config_path: str | None = None + self._stop_event = threading.Event() + + # Point cloud frame accumulator state + self._pc_queue: Queue[tuple[int, int, np.ndarray, np.ndarray]] = Queue(maxsize=4096) + self._pc_consumer_thread: threading.Thread | None = None + self._pc_observer: Any[PointCloud2] | None = None + + # IMU state + self._imu_queue: Queue[tuple[int, np.ndarray]] = Queue(maxsize=4096) + self._imu_consumer_thread: threading.Thread | None = None + self._imu_observer: Any[Imu] | None = None + + # Device tracking + self._connected_handles: dict[int, LivoxLidarDeviceType] = {} + + def _frame_id(self, suffix: str) -> str: + prefix = self.config.frame_id_prefix + if prefix: + return f"{prefix}/{suffix}" + return suffix + + # ------------------------------------------------------------------ + # SDK config generation + # ------------------------------------------------------------------ + + def _write_sdk_config(self) -> str: + """Generate a temporary JSON config file for the SDK.""" + config = { + "MID360": { + "lidar_net_info": { + "cmd_data_port": self.config.cmd_data_port, + "push_msg_port": self.config.push_msg_port, + "point_data_port": self.config.point_data_port, + "imu_data_port": self.config.imu_data_port, + "log_data_port": self.config.log_data_port, + }, + "host_net_info": [ + { + "host_ip": self.config.host_ip, + "multicast_ip": "224.1.1.5", + "cmd_data_port": self.config.host_cmd_data_port, + "push_msg_port": self.config.host_push_msg_port, + "point_data_port": self.config.host_point_data_port, + "imu_data_port": self.config.host_imu_data_port, + "log_data_port": self.config.host_log_data_port, + } + ], + } + } + fd, path = tempfile.mkstemp(suffix=".json", prefix="livox_mid360_") + with open(fd, "w") as f: + json.dump(config, f) + return path + + # ------------------------------------------------------------------ + # SDK callbacks (called from C threads - keep minimal, just enqueue) + # ------------------------------------------------------------------ + + def _on_point_cloud( + self, + handle: int, + dev_type: int, + packet_ptr: LivoxLidarEthernetPacket, # type: ignore[override] + client_data: object, + ) -> None: + """SDK point cloud callback. Copies data and enqueues for processing.""" + if self._stop_event.is_set(): + return + try: + packet = packet_ptr.contents if hasattr(packet_ptr, "contents") else packet_ptr + data_type = packet.data_type + + if data_type == LivoxLidarPointDataType.CARTESIAN_HIGH: + xyz, reflectivities, _tags = parse_cartesian_high_points(packet) + elif data_type == LivoxLidarPointDataType.CARTESIAN_LOW: + xyz, reflectivities, _tags = parse_cartesian_low_points(packet) + else: + return # skip spherical for now + + ts_ns = get_packet_timestamp_ns(packet) + frame_cnt = packet.frame_cnt + + self._pc_queue.put_nowait((frame_cnt, ts_ns, xyz, reflectivities)) + except Exception: + logger.debug("Error in point cloud callback", exc_info=True) + + def _on_imu_data( + self, + handle: int, + dev_type: int, + packet_ptr: LivoxLidarEthernetPacket, # type: ignore[override] + client_data: object, + ) -> None: + """SDK IMU callback. Copies data and enqueues.""" + if self._stop_event.is_set(): + return + try: + packet = packet_ptr.contents if hasattr(packet_ptr, "contents") else packet_ptr + ts_ns = get_packet_timestamp_ns(packet) + imu_points = parse_imu_data(packet) + self._imu_queue.put_nowait((ts_ns, imu_points)) + except Exception: + logger.debug("Error in IMU callback", exc_info=True) + + def _on_info_change( + self, + handle: int, + info_ptr: LivoxLidarInfo, # type: ignore[override] + client_data: object, + ) -> None: + """SDK device info change callback. Tracks connected devices.""" + try: + info = info_ptr.contents if hasattr(info_ptr, "contents") else info_ptr + dev_type = LivoxLidarDeviceType(info.dev_type) + sn = info.sn.decode("utf-8", errors="replace").rstrip("\x00") + ip = info.lidar_ip.decode("utf-8", errors="replace").rstrip("\x00") + self._connected_handles[handle] = dev_type + logger.info( + "Livox device connected: handle=%d type=%s sn=%s ip=%s", + handle, + dev_type.name, + sn, + ip, + ) + + # Set to normal work mode + self._sdk_set_work_mode(handle) + + # Enable/disable IMU based on config + if self.config.enable_imu: + self._sdk_enable_imu(handle) + except Exception: + logger.debug("Error in info change callback", exc_info=True) + + def _sdk_set_work_mode(self, handle: int) -> None: + if self._sdk is None: + return + + def _on_work_mode(status: int, handle: int, response: object, client_data: object) -> None: + if status == 0: + logger.info("Work mode set to NORMAL for handle %d", handle) + else: + logger.warning("Failed to set work mode for handle %d: status=%d", handle, status) + + _cb = AsyncControlCallbackType(_on_work_mode) + self._sdk._callbacks[f"work_mode_cb_{handle}"] = _cb + self._sdk.lib.SetLivoxLidarWorkMode(handle, int(LivoxLidarWorkMode.NORMAL), _cb, None) + + def _sdk_enable_imu(self, handle: int) -> None: + if self._sdk is None: + return + + def _on_imu_enable(status: int, handle: int, response: object, client_data: object) -> None: + if status == 0: + logger.info("IMU enabled for handle %d", handle) + else: + logger.warning("Failed to enable IMU for handle %d: status=%d", handle, status) + + _cb = AsyncControlCallbackType(_on_imu_enable) + self._sdk._callbacks[f"imu_enable_cb_{handle}"] = _cb + self._sdk.lib.EnableLivoxLidarImuData(handle, _cb, None) + + # ------------------------------------------------------------------ + # Consumer threads (Python-side, build messages from queued data) + # ------------------------------------------------------------------ + + def _pointcloud_consumer_loop(self) -> None: + """Drain the point cloud queue, accumulate by time window, emit PointCloud2.""" + frame_points: list[np.ndarray] = [] + frame_reflectivities: list[np.ndarray] = [] + frame_timestamp: float | None = None + frame_interval = 1.0 / self.config.frequency if self.config.frequency > 0 else 0.1 + last_emit = time.monotonic() + + while not self._stop_event.is_set(): + try: + _frame_cnt, ts_ns, xyz, reflectivities = self._pc_queue.get(timeout=0.05) + except Empty: + # Check if we should emit on timeout + if frame_points and (time.monotonic() - last_emit) >= frame_interval: + self._emit_pointcloud(frame_points, frame_reflectivities, frame_timestamp) + frame_points = [] + frame_reflectivities = [] + frame_timestamp = None + last_emit = time.monotonic() + continue + + frame_points.append(xyz) + frame_reflectivities.append(reflectivities) + if frame_timestamp is None: + frame_timestamp = ts_ns / 1e9 + + # Emit when time window is full + if (time.monotonic() - last_emit) >= frame_interval: + self._emit_pointcloud(frame_points, frame_reflectivities, frame_timestamp) + frame_points = [] + frame_reflectivities = [] + frame_timestamp = None + last_emit = time.monotonic() + + # Emit any remaining data + if frame_points: + self._emit_pointcloud(frame_points, frame_reflectivities, frame_timestamp) + + def _emit_pointcloud( + self, + frame_points: list[np.ndarray], + frame_reflectivities: list[np.ndarray], + frame_timestamp: float | None, + ) -> None: + if not self._pc_observer or self._stop_event.is_set(): + return + + all_points = np.concatenate(frame_points, axis=0) + all_reflectivities = np.concatenate(frame_reflectivities, axis=0) + + if len(all_points) == 0: + return + + # Build Open3D tensor point cloud + pcd_t = o3d.t.geometry.PointCloud() + pcd_t.point["positions"] = o3c.Tensor(all_points, dtype=o3c.float32) + # Store reflectivity as intensity (normalized to 0-1 range) + pcd_t.point["intensities"] = o3c.Tensor( + all_reflectivities.astype(np.float32).reshape(-1, 1) / 255.0, + dtype=o3c.float32, + ) + + pc2 = PointCloud2( + pointcloud=pcd_t, + frame_id=self._frame_id(self.config.frame_id), + ts=frame_timestamp if frame_timestamp else time.time(), + ) + + try: + self._pc_observer.on_next(pc2) + except Exception: + logger.debug("Error emitting point cloud", exc_info=True) + + def _imu_consumer_loop(self) -> None: + """Drain the IMU queue and emit Imu messages.""" + while not self._stop_event.is_set(): + try: + ts_ns, imu_points = self._imu_queue.get(timeout=0.5) + except Empty: + continue + + if not self._imu_observer or self._stop_event.is_set(): + continue + + ts = ts_ns / 1e9 + for i in range(len(imu_points)): + pt = imu_points[i] + imu_msg = Imu( + angular_velocity=Vector3( + float(pt["gyro_x"]), + float(pt["gyro_y"]), + float(pt["gyro_z"]), + ), + linear_acceleration=Vector3( + float(pt["acc_x"]) * GRAVITY_MS2, + float(pt["acc_y"]) * GRAVITY_MS2, + float(pt["acc_z"]) * GRAVITY_MS2, + ), + frame_id=self._frame_id("imu_link"), + ts=ts, + ) + try: + self._imu_observer.on_next(imu_msg) + except Exception: + logger.debug("Error emitting IMU data", exc_info=True) + + # ------------------------------------------------------------------ + # Public API: Observable streams + # ------------------------------------------------------------------ + + @cache + def pointcloud_stream(self) -> Observable[PointCloud2]: + """Observable stream of PointCloud2 messages (~10 Hz full-frame scans).""" + + def subscribe(observer, scheduler=None): # type: ignore[no-untyped-def] + self._pc_observer = observer + try: + self._start_sdk() + except Exception as e: + observer.on_error(e) + return + + def dispose() -> None: + self._pc_observer = None + self.stop() + + return dispose + + return backpressure(create(subscribe)) + + @cache + def imu_stream(self) -> Observable[Imu]: + """Observable stream of Imu messages (~200 Hz).""" + + def subscribe(observer, scheduler=None): # type: ignore[no-untyped-def] + self._imu_observer = observer + + def dispose() -> None: + self._imu_observer = None + + return dispose + + return backpressure(create(subscribe)) + + # ------------------------------------------------------------------ + # Lifecycle + # ------------------------------------------------------------------ + + def _start_sdk(self) -> None: + """Initialize and start the Livox SDK.""" + if self._sdk is not None: + return # already running + + self._stop_event.clear() + + # Write temp config + self._config_path = self._write_sdk_config() + + # Load and init SDK + self._sdk = LivoxSDK(self.config.sdk_lib_path) + self._sdk.init(self._config_path, self.config.host_ip) + + # Register callbacks (must keep references to prevent GC) + pc_cb = PointCloudCallbackType(self._on_point_cloud) + imu_cb = ImuDataCallbackType(self._on_imu_data) + info_cb = InfoChangeCallbackType(self._on_info_change) + + self._sdk.set_point_cloud_callback(pc_cb) + if self.config.enable_imu: + self._sdk.set_imu_callback(imu_cb) + self._sdk.set_info_change_callback(info_cb) + + # Start SDK background threads + self._sdk.start() + logger.info( + "Livox SDK started (host_ip=%s, lidar_ips=%s)", + self.config.host_ip, + self.config.lidar_ips, + ) + + # Start consumer threads + self._pc_consumer_thread = threading.Thread( + target=self._pointcloud_consumer_loop, daemon=True, name="livox-pc-consumer" + ) + self._pc_consumer_thread.start() + + if self.config.enable_imu: + self._imu_consumer_thread = threading.Thread( + target=self._imu_consumer_loop, daemon=True, name="livox-imu-consumer" + ) + self._imu_consumer_thread.start() + + def stop(self) -> None: + """Stop the SDK and all consumer threads.""" + self._stop_event.set() + + if self._sdk is not None: + self._sdk.uninit() + self._sdk = None + + for thread in [self._pc_consumer_thread, self._imu_consumer_thread]: + if thread is not None and thread.is_alive(): + thread.join(timeout=3.0) + + self._pc_consumer_thread = None + self._imu_consumer_thread = None + self._connected_handles.clear() + + # Cleanup temp config + if self._config_path: + import os + + try: + os.unlink(self._config_path) + except OSError: + pass + self._config_path = None + + logger.info("Livox Mid-360 stopped") diff --git a/dimos/hardware/sensors/lidar/livox/sdk.py b/dimos/hardware/sensors/lidar/livox/sdk.py new file mode 100644 index 0000000000..eb5fbcbccb --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/sdk.py @@ -0,0 +1,534 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""ctypes bindings for Livox SDK2 (liblivox_lidar_sdk_shared.so). + +Provides Python access to the Livox LiDAR SDK2 C API for device +communication, point cloud streaming, and IMU data. + +The SDK .so must be pre-built from https://github.com/Livox-SDK/Livox-SDK2. +Set LIVOX_SDK2_LIB_PATH env var or install to /usr/local/lib. +""" + +from __future__ import annotations + +import ctypes +import ctypes.util +from enum import IntEnum +import logging +import os +from pathlib import Path +from typing import Any + +import numpy as np + +logger = logging.getLogger(__name__) + + +# --------------------------------------------------------------------------- +# Enums +# --------------------------------------------------------------------------- + + +class LivoxLidarDeviceType(IntEnum): + HUB = 0 + MID40 = 1 + TELE = 2 + HORIZON = 3 + MID70 = 6 + AVIA = 7 + MID360 = 9 + INDUSTRIAL_HAP = 10 + HAP = 15 + PA = 16 + + +class LivoxLidarPointDataType(IntEnum): + IMU = 0x00 + CARTESIAN_HIGH = 0x01 + CARTESIAN_LOW = 0x02 + SPHERICAL = 0x03 + + +class LivoxLidarWorkMode(IntEnum): + NORMAL = 0x01 + WAKE_UP = 0x02 + SLEEP = 0x03 + ERROR = 0x04 + POWER_ON_SELF_TEST = 0x05 + MOTOR_STARTING = 0x06 + MOTOR_STOPPING = 0x07 + UPGRADE = 0x08 + + +class LivoxLidarScanPattern(IntEnum): + NON_REPETITIVE = 0x00 + REPETITIVE = 0x01 + REPETITIVE_LOW_FRAME_RATE = 0x02 + + +class LivoxLidarStatus(IntEnum): + SEND_FAILED = -9 + HANDLER_IMPL_NOT_EXIST = -8 + INVALID_HANDLE = -7 + CHANNEL_NOT_EXIST = -6 + NOT_ENOUGH_MEMORY = -5 + TIMEOUT = -4 + NOT_SUPPORTED = -3 + NOT_CONNECTED = -2 + FAILURE = -1 + SUCCESS = 0 + + +# --------------------------------------------------------------------------- +# Packed C structures (all #pragma pack(1) in the SDK header) +# --------------------------------------------------------------------------- + + +class LivoxLidarCartesianHighRawPoint(ctypes.LittleEndianStructure): + """High-resolution cartesian point (14 bytes). Coordinates in mm.""" + + _pack_ = 1 + _fields_ = [ + ("x", ctypes.c_int32), + ("y", ctypes.c_int32), + ("z", ctypes.c_int32), + ("reflectivity", ctypes.c_uint8), + ("tag", ctypes.c_uint8), + ] + + +class LivoxLidarCartesianLowRawPoint(ctypes.LittleEndianStructure): + """Low-resolution cartesian point (8 bytes). Coordinates in cm.""" + + _pack_ = 1 + _fields_ = [ + ("x", ctypes.c_int16), + ("y", ctypes.c_int16), + ("z", ctypes.c_int16), + ("reflectivity", ctypes.c_uint8), + ("tag", ctypes.c_uint8), + ] + + +class LivoxLidarSpherPoint(ctypes.LittleEndianStructure): + """Spherical coordinate point (10 bytes).""" + + _pack_ = 1 + _fields_ = [ + ("depth", ctypes.c_uint32), + ("theta", ctypes.c_uint16), + ("phi", ctypes.c_uint16), + ("reflectivity", ctypes.c_uint8), + ("tag", ctypes.c_uint8), + ] + + +class LivoxLidarImuRawPoint(ctypes.LittleEndianStructure): + """IMU data point (24 bytes). Gyro in rad/s, accel in g.""" + + _pack_ = 1 + _fields_ = [ + ("gyro_x", ctypes.c_float), + ("gyro_y", ctypes.c_float), + ("gyro_z", ctypes.c_float), + ("acc_x", ctypes.c_float), + ("acc_y", ctypes.c_float), + ("acc_z", ctypes.c_float), + ] + + +class LivoxLidarEthernetPacket(ctypes.LittleEndianStructure): + """Point cloud / IMU ethernet packet header (32 bytes + variable data).""" + + _pack_ = 1 + _fields_ = [ + ("version", ctypes.c_uint8), + ("length", ctypes.c_uint16), + ("time_interval", ctypes.c_uint16), # unit: 0.1 us + ("dot_num", ctypes.c_uint16), + ("udp_cnt", ctypes.c_uint16), + ("frame_cnt", ctypes.c_uint8), + ("data_type", ctypes.c_uint8), + ("time_type", ctypes.c_uint8), + ("rsvd", ctypes.c_uint8 * 12), + ("crc32", ctypes.c_uint32), + ("timestamp", ctypes.c_uint8 * 8), + ("data", ctypes.c_uint8 * 1), # variable-length payload + ] + + +class LivoxLidarInfo(ctypes.LittleEndianStructure): + """Device info received on connection.""" + + _pack_ = 1 + _fields_ = [ + ("dev_type", ctypes.c_uint8), + ("sn", ctypes.c_char * 16), + ("lidar_ip", ctypes.c_char * 16), + ] + + +class LivoxLidarAsyncControlResponse(ctypes.LittleEndianStructure): + _pack_ = 1 + _fields_ = [ + ("ret_code", ctypes.c_uint8), + ("error_key", ctypes.c_uint16), + ] + + +# --------------------------------------------------------------------------- +# numpy dtypes for efficient batch parsing of point data +# --------------------------------------------------------------------------- + +CART_HIGH_DTYPE = np.dtype( + [("x", " tuple[np.ndarray, np.ndarray, np.ndarray]: + """Parse high-res cartesian points from a packet. + + Returns: + (xyz_meters, reflectivities, tags) where xyz is (N,3) float32 in meters. + """ + dot_num = packet.dot_num + if dot_num == 0: + empty = np.empty((0, 3), dtype=np.float32) + return empty, np.empty(0, dtype=np.uint8), np.empty(0, dtype=np.uint8) + + data_ptr = ctypes.addressof(packet) + LivoxLidarEthernetPacket.data.offset + buf = (ctypes.c_uint8 * (dot_num * CART_HIGH_DTYPE.itemsize)).from_address(data_ptr) + points = np.frombuffer(buf, dtype=CART_HIGH_DTYPE, count=dot_num) + + xyz = np.column_stack([points["x"], points["y"], points["z"]]).astype(np.float32) / 1000.0 + return xyz, points["reflectivity"].copy(), points["tag"].copy() + + +def parse_cartesian_low_points( + packet: LivoxLidarEthernetPacket, +) -> tuple[np.ndarray, np.ndarray, np.ndarray]: + """Parse low-res cartesian points. Returns xyz in meters.""" + dot_num = packet.dot_num + if dot_num == 0: + empty = np.empty((0, 3), dtype=np.float32) + return empty, np.empty(0, dtype=np.uint8), np.empty(0, dtype=np.uint8) + + data_ptr = ctypes.addressof(packet) + LivoxLidarEthernetPacket.data.offset + buf = (ctypes.c_uint8 * (dot_num * CART_LOW_DTYPE.itemsize)).from_address(data_ptr) + points = np.frombuffer(buf, dtype=CART_LOW_DTYPE, count=dot_num) + + xyz = np.column_stack([points["x"], points["y"], points["z"]]).astype(np.float32) / 100.0 + return xyz, points["reflectivity"].copy(), points["tag"].copy() + + +def parse_imu_data(packet: LivoxLidarEthernetPacket) -> np.ndarray: + """Parse IMU data from a packet. Returns structured array with gyro/accel fields.""" + dot_num = packet.dot_num + if dot_num == 0: + return np.empty(0, dtype=IMU_DTYPE) + + data_ptr = ctypes.addressof(packet) + LivoxLidarEthernetPacket.data.offset + buf = (ctypes.c_uint8 * (dot_num * IMU_DTYPE.itemsize)).from_address(data_ptr) + return np.frombuffer(buf, dtype=IMU_DTYPE, count=dot_num).copy() + + +def get_packet_timestamp_ns(packet: LivoxLidarEthernetPacket) -> int: + """Extract the 64-bit nanosecond timestamp from a packet.""" + return int.from_bytes(bytes(packet.timestamp), byteorder="little") + + +# --------------------------------------------------------------------------- +# SDK library loader +# --------------------------------------------------------------------------- + +_SDK_LIB_NAMES = [ + "livox_lidar_sdk_shared", + "liblivox_lidar_sdk_shared.so", + "liblivox_lidar_sdk_shared.so.2", +] + +_SDK_SEARCH_PATHS = [ + "/usr/local/lib", + "/usr/lib", + "/usr/lib/x86_64-linux-gnu", + "/usr/lib/aarch64-linux-gnu", +] + + +def _find_sdk_library(lib_path: str | None = None) -> str: + """Find the Livox SDK2 shared library. + + Search order: + 1. Explicit lib_path argument + 2. LIVOX_SDK2_LIB_PATH environment variable + 3. ctypes.util.find_library + 4. Well-known system paths + """ + if lib_path and Path(lib_path).exists(): + return lib_path + + env_path = os.environ.get("LIVOX_SDK2_LIB_PATH") + if env_path and Path(env_path).exists(): + return env_path + + for name in _SDK_LIB_NAMES: + found = ctypes.util.find_library(name) + if found: + return found + + for search_dir in _SDK_SEARCH_PATHS: + for name in _SDK_LIB_NAMES: + candidate = Path(search_dir) / name + if candidate.exists(): + return str(candidate) + + raise RuntimeError( + "Livox SDK2 shared library not found. " + "Build from https://github.com/Livox-SDK/Livox-SDK2 and install, " + "or set LIVOX_SDK2_LIB_PATH to the .so file path." + ) + + +def load_sdk(lib_path: str | None = None) -> ctypes.CDLL: + """Load the Livox SDK2 shared library and set up function signatures.""" + path = _find_sdk_library(lib_path) + lib = ctypes.CDLL(path) + logger.info("Loaded Livox SDK2 from %s", path) + + # --- SDK lifecycle --- + lib.LivoxLidarSdkInit.argtypes = [ctypes.c_char_p, ctypes.c_char_p, ctypes.c_void_p] + lib.LivoxLidarSdkInit.restype = ctypes.c_bool + + lib.LivoxLidarSdkStart.argtypes = [] + lib.LivoxLidarSdkStart.restype = ctypes.c_bool + + lib.LivoxLidarSdkUninit.argtypes = [] + lib.LivoxLidarSdkUninit.restype = None + + # --- Callbacks --- + lib.SetLivoxLidarPointCloudCallBack.argtypes = [PointCloudCallbackType, ctypes.c_void_p] + lib.SetLivoxLidarPointCloudCallBack.restype = None + + lib.SetLivoxLidarImuDataCallback.argtypes = [ImuDataCallbackType, ctypes.c_void_p] + lib.SetLivoxLidarImuDataCallback.restype = None + + lib.SetLivoxLidarInfoChangeCallback.argtypes = [InfoChangeCallbackType, ctypes.c_void_p] + lib.SetLivoxLidarInfoChangeCallback.restype = None + + lib.SetLivoxLidarInfoCallback.argtypes = [InfoCallbackType, ctypes.c_void_p] + lib.SetLivoxLidarInfoCallback.restype = None + + # --- Device configuration --- + lib.SetLivoxLidarWorkMode.argtypes = [ + ctypes.c_uint32, + ctypes.c_int32, + AsyncControlCallbackType, + ctypes.c_void_p, + ] + lib.SetLivoxLidarWorkMode.restype = ctypes.c_int32 + + lib.EnableLivoxLidarImuData.argtypes = [ + ctypes.c_uint32, + AsyncControlCallbackType, + ctypes.c_void_p, + ] + lib.EnableLivoxLidarImuData.restype = ctypes.c_int32 + + lib.DisableLivoxLidarImuData.argtypes = [ + ctypes.c_uint32, + AsyncControlCallbackType, + ctypes.c_void_p, + ] + lib.DisableLivoxLidarImuData.restype = ctypes.c_int32 + + lib.SetLivoxLidarPclDataType.argtypes = [ + ctypes.c_uint32, + ctypes.c_int32, + AsyncControlCallbackType, + ctypes.c_void_p, + ] + lib.SetLivoxLidarPclDataType.restype = ctypes.c_int32 + + lib.SetLivoxLidarScanPattern.argtypes = [ + ctypes.c_uint32, + ctypes.c_int32, + AsyncControlCallbackType, + ctypes.c_void_p, + ] + lib.SetLivoxLidarScanPattern.restype = ctypes.c_int32 + + # --- Console logging --- + lib.DisableLivoxSdkConsoleLogger.argtypes = [] + lib.DisableLivoxSdkConsoleLogger.restype = None + + return lib + + +class LivoxSDK: + """Convenience wrapper managing SDK lifecycle and callback registration. + + Prevents garbage collection of ctypes callback references (which would + cause segfaults when the SDK tries to call them from C threads). + """ + + def __init__(self, lib_path: str | None = None) -> None: + self._lib = load_sdk(lib_path) + self._initialized = False + # prevent GC of callback C function pointers + self._callbacks: dict[str, Any] = {} + + @property + def lib(self) -> ctypes.CDLL: + return self._lib + + def init(self, config_path: str, host_ip: str, quiet: bool = True) -> None: + """Initialize the SDK with a JSON config file and host IP.""" + if self._initialized: + raise RuntimeError("SDK already initialized. Call uninit() first.") + if quiet: + self._lib.DisableLivoxSdkConsoleLogger() + ok = self._lib.LivoxLidarSdkInit( + config_path.encode(), + host_ip.encode(), + None, + ) + if not ok: + raise RuntimeError( + f"LivoxLidarSdkInit failed (config={config_path}, host_ip={host_ip})" + ) + self._initialized = True + + def start(self) -> None: + """Start the SDK (begins device communication on background threads).""" + if not self._initialized: + raise RuntimeError("SDK not initialized. Call init() first.") + ok = self._lib.LivoxLidarSdkStart() + if not ok: + raise RuntimeError("LivoxLidarSdkStart failed") + + def uninit(self) -> None: + """Shut down the SDK and release resources.""" + if self._initialized: + self._lib.LivoxLidarSdkUninit() + self._initialized = False + self._callbacks.clear() + + def set_point_cloud_callback(self, callback: Any) -> None: + self._callbacks["pointcloud"] = callback # prevent GC + self._lib.SetLivoxLidarPointCloudCallBack(callback, None) + + def set_imu_callback(self, callback: Any) -> None: + self._callbacks["imu"] = callback + self._lib.SetLivoxLidarImuDataCallback(callback, None) + + def set_info_change_callback(self, callback: Any) -> None: + self._callbacks["info_change"] = callback + self._lib.SetLivoxLidarInfoChangeCallback(callback, None) + + def set_info_callback(self, callback: Any) -> None: + self._callbacks["info"] = callback + self._lib.SetLivoxLidarInfoCallback(callback, None) + + def set_work_mode(self, handle: int, mode: LivoxLidarWorkMode, callback: Any = None) -> int: + cb = callback or AsyncControlCallbackType(0) # type: ignore[arg-type] + if callback: + self._callbacks[f"work_mode_{handle}"] = callback + return self._lib.SetLivoxLidarWorkMode(handle, int(mode), cb, None) # type: ignore[no-any-return] + + def enable_imu_data(self, handle: int, callback: Any = None) -> int: + cb = callback or AsyncControlCallbackType(0) # type: ignore[arg-type] + if callback: + self._callbacks[f"imu_enable_{handle}"] = callback + return self._lib.EnableLivoxLidarImuData(handle, cb, None) # type: ignore[no-any-return] + + def disable_imu_data(self, handle: int, callback: Any = None) -> int: + cb = callback or AsyncControlCallbackType(0) # type: ignore[arg-type] + if callback: + self._callbacks[f"imu_disable_{handle}"] = callback + return self._lib.DisableLivoxLidarImuData(handle, cb, None) # type: ignore[no-any-return] diff --git a/dimos/hardware/sensors/lidar/module.py b/dimos/hardware/sensors/lidar/module.py new file mode 100644 index 0000000000..d637ea347a --- /dev/null +++ b/dimos/hardware/sensors/lidar/module.py @@ -0,0 +1,147 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""LiDAR module wrappers that convert LidarHardware observables into module streams.""" + +from __future__ import annotations + +from dataclasses import dataclass, field +import time +from typing import TYPE_CHECKING, Any + +import reactivex as rx +from reactivex import operators as ops + +from dimos.core import Module, ModuleConfig, Out, rpc +from dimos.msgs.geometry_msgs import Quaternion, Transform, Vector3 +from dimos.spec import perception + +if TYPE_CHECKING: + from collections.abc import Callable + + from dimos.hardware.sensors.lidar.spec import LidarHardware + from dimos.msgs.sensor_msgs.Imu import Imu + from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + + +def default_lidar_transform() -> Transform: + return Transform( + translation=Vector3(0.0, 0.0, 0.0), + rotation=Quaternion(0.0, 0.0, 0.0, 1.0), + frame_id="base_link", + child_frame_id="lidar_link", + ) + + +@dataclass +class LidarModuleConfig(ModuleConfig): + frame_id: str = "lidar_link" + transform: Transform | None = field(default_factory=default_lidar_transform) + hardware: Callable[[], LidarHardware[Any]] | LidarHardware[Any] | None = None + frequency: float = 0.0 # Hz, 0 means no limit + + +class LidarModule(Module[LidarModuleConfig], perception.Lidar): + """Generic LiDAR module — pointcloud only. + + Publishes PointCloud2 messages and TF transforms. + """ + + pointcloud: Out[PointCloud2] + + hardware: LidarHardware[Any] + + config: LidarModuleConfig + default_config = LidarModuleConfig + + @rpc + def start(self) -> None: + super().start() + self._init_hardware() + + stream = self.hardware.pointcloud_stream() + + if self.config.frequency > 0: + stream = stream.pipe(ops.sample(1.0 / self.config.frequency)) + + self._disposables.add( + stream.subscribe(lambda pc: self.pointcloud.publish(pc)), + ) + + self._disposables.add( + rx.interval(1.0).subscribe(lambda _: self._publish_tf()), + ) + + def _init_hardware(self) -> None: + if callable(self.config.hardware): + self.hardware = self.config.hardware() + else: + self.hardware = self.config.hardware # type: ignore[assignment] + + def _publish_tf(self) -> None: + if not self.config.transform: + return + ts = time.time() + lidar_link = self.config.transform + lidar_link.ts = ts + self.tf.publish(lidar_link) + + def stop(self) -> None: + if self.hardware and hasattr(self.hardware, "stop"): + self.hardware.stop() + super().stop() + + +# --------------------------------------------------------------------------- +# Livox-specific: LiDAR + IMU +# --------------------------------------------------------------------------- + + +@dataclass +class LivoxLidarModuleConfig(LidarModuleConfig): + enable_imu: bool = True + + +class LivoxLidarModule(LidarModule, perception.IMU): + """Livox LiDAR module — pointcloud + IMU. + + Extends LidarModule with IMU stream support for sensors like the Mid-360. + """ + + imu: Out[Imu] + + config: LivoxLidarModuleConfig # type: ignore[assignment] + default_config = LivoxLidarModuleConfig # type: ignore[assignment] + + @rpc + def start(self) -> None: + super().start() + + if self.config.enable_imu: + imu_stream = self.hardware.imu_stream() + if imu_stream is not None: + self._disposables.add( + imu_stream.subscribe(lambda imu_msg: self.imu.publish(imu_msg)), + ) + + +lidar_module = LidarModule.blueprint +livox_lidar_module = LivoxLidarModule.blueprint + +__all__ = [ + "LidarModule", + "LivoxLidarModule", + "lidar_module", + "livox_lidar_module", +] diff --git a/dimos/hardware/sensors/lidar/spec.py b/dimos/hardware/sensors/lidar/spec.py new file mode 100644 index 0000000000..d01b451479 --- /dev/null +++ b/dimos/hardware/sensors/lidar/spec.py @@ -0,0 +1,43 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from abc import ABC, abstractmethod +from typing import Generic, Protocol, TypeVar + +from reactivex.observable import Observable + +from dimos.msgs.sensor_msgs.Imu import Imu +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.protocol.service import Configurable # type: ignore[attr-defined] + + +class LidarConfig(Protocol): + frame_id_prefix: str | None + frequency: float # Hz, point cloud output rate + + +LidarConfigT = TypeVar("LidarConfigT", bound=LidarConfig) + + +class LidarHardware(ABC, Configurable[LidarConfigT], Generic[LidarConfigT]): + """Abstract base class for LiDAR hardware drivers.""" + + @abstractmethod + def pointcloud_stream(self) -> Observable[PointCloud2]: + """Observable stream of point clouds from the LiDAR.""" + pass + + def imu_stream(self) -> Observable[Imu] | None: + """Optional observable stream of IMU data. Returns None if unsupported.""" + return None diff --git a/dimos/msgs/sensor_msgs/Imu.py b/dimos/msgs/sensor_msgs/Imu.py new file mode 100644 index 0000000000..0fadc3f3f7 --- /dev/null +++ b/dimos/msgs/sensor_msgs/Imu.py @@ -0,0 +1,123 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from __future__ import annotations + +import time + +from dimos_lcm.sensor_msgs.Imu import Imu as LCMImu + +from dimos.msgs.geometry_msgs import Quaternion, Vector3 +from dimos.types.timestamped import Timestamped + + +def _sec_nsec(ts: float) -> tuple[int, int]: + s = int(ts) + return s, int((ts - s) * 1_000_000_000) + + +class Imu(Timestamped): + """IMU sensor message mirroring ROS sensor_msgs/Imu. + + Contains orientation, angular velocity, and linear acceleration + with optional covariance matrices (3x3 row-major as flat 9-element lists). + """ + + msg_name = "sensor_msgs.Imu" + + def __init__( + self, + angular_velocity: Vector3 | None = None, + linear_acceleration: Vector3 | None = None, + orientation: Quaternion | None = None, + orientation_covariance: list[float] | None = None, + angular_velocity_covariance: list[float] | None = None, + linear_acceleration_covariance: list[float] | None = None, + frame_id: str = "imu_link", + ts: float | None = None, + ) -> None: + self.ts = ts if ts is not None else time.time() # type: ignore[assignment] + self.frame_id = frame_id + self.angular_velocity = angular_velocity or Vector3(0.0, 0.0, 0.0) + self.linear_acceleration = linear_acceleration or Vector3(0.0, 0.0, 0.0) + self.orientation = orientation or Quaternion(0.0, 0.0, 0.0, 1.0) + self.orientation_covariance = orientation_covariance or [0.0] * 9 + self.angular_velocity_covariance = angular_velocity_covariance or [0.0] * 9 + self.linear_acceleration_covariance = linear_acceleration_covariance or [0.0] * 9 + + def lcm_encode(self) -> bytes: + msg = LCMImu() + msg.header.stamp.sec, msg.header.stamp.nsec = _sec_nsec(self.ts) + msg.header.frame_id = self.frame_id + + msg.orientation.x = self.orientation.x + msg.orientation.y = self.orientation.y + msg.orientation.z = self.orientation.z + msg.orientation.w = self.orientation.w + msg.orientation_covariance = self.orientation_covariance + + msg.angular_velocity.x = self.angular_velocity.x + msg.angular_velocity.y = self.angular_velocity.y + msg.angular_velocity.z = self.angular_velocity.z + msg.angular_velocity_covariance = self.angular_velocity_covariance + + msg.linear_acceleration.x = self.linear_acceleration.x + msg.linear_acceleration.y = self.linear_acceleration.y + msg.linear_acceleration.z = self.linear_acceleration.z + msg.linear_acceleration_covariance = self.linear_acceleration_covariance + + return msg.lcm_encode() # type: ignore[no-any-return] + + @classmethod + def lcm_decode(cls, data: bytes) -> Imu: + msg = LCMImu.lcm_decode(data) + ts = msg.header.stamp.sec + (msg.header.stamp.nsec / 1_000_000_000) + return cls( + angular_velocity=Vector3( + msg.angular_velocity.x, + msg.angular_velocity.y, + msg.angular_velocity.z, + ), + linear_acceleration=Vector3( + msg.linear_acceleration.x, + msg.linear_acceleration.y, + msg.linear_acceleration.z, + ), + orientation=Quaternion( + msg.orientation.x, + msg.orientation.y, + msg.orientation.z, + msg.orientation.w, + ), + orientation_covariance=list(msg.orientation_covariance), + angular_velocity_covariance=list(msg.angular_velocity_covariance), + linear_acceleration_covariance=list(msg.linear_acceleration_covariance), + frame_id=msg.header.frame_id, + ts=ts, + ) + + def __str__(self) -> str: + return ( + f"Imu(frame_id='{self.frame_id}', " + f"gyro=({self.angular_velocity.x:.3f}, {self.angular_velocity.y:.3f}, {self.angular_velocity.z:.3f}), " + f"accel=({self.linear_acceleration.x:.3f}, {self.linear_acceleration.y:.3f}, {self.linear_acceleration.z:.3f}))" + ) + + def __repr__(self) -> str: + return ( + f"Imu(ts={self.ts}, frame_id='{self.frame_id}', " + f"angular_velocity={self.angular_velocity}, " + f"linear_acceleration={self.linear_acceleration}, " + f"orientation={self.orientation})" + ) diff --git a/dimos/msgs/sensor_msgs/PointCloud2.py b/dimos/msgs/sensor_msgs/PointCloud2.py index 39a0146e4c..911c66997e 100644 --- a/dimos/msgs/sensor_msgs/PointCloud2.py +++ b/dimos/msgs/sensor_msgs/PointCloud2.py @@ -611,20 +611,18 @@ def to_rerun( voxel_size: float = 0.05, colormap: str | None = "turbo", colors: list[int] | None = None, - mode: str = "boxes", + mode: str = "points", size: float | None = None, fill_mode: str = "solid", **kwargs: object, ) -> Archetype: - import rerun as rr - - """Convert to Rerun Points3D or Boxes3D archetype. + """Convert to Rerun archetype for visualization. Args: voxel_size: size for visualization colormap: Optional colormap name (e.g., "turbo", "viridis") to color by height colors: Optional RGB color [r, g, b] for all points (0-255) - mode: Visualization mode - "points" for spheres, "boxes" for cubes (default) + mode: "points" for raw points, "boxes" for cubes (default), or "spheres" for sized spheres size: Box size for mode="boxes" (e.g., voxel_size). Defaults to radii*2. fill_mode: Fill mode for boxes - "solid", "majorwireframe", or "densewireframe" **kwargs: Additional args (ignored for compatibility) @@ -632,14 +630,15 @@ def to_rerun( Returns: rr.Points3D or rr.Boxes3D archetype for logging to Rerun """ + import rerun as rr + points, _ = self.as_numpy() if len(points) == 0: - return rr.Points3D([]) if mode == "points" else rr.Boxes3D(centers=[]) + return rr.Points3D([]) if mode != "boxes" else rr.Boxes3D(centers=[]) # Determine colors point_colors = None if colormap is not None: - # Color by height (z-coordinate) z = points[:, 2] z_norm = (z - z.min()) / (z.max() - z.min() + 1e-8) cmap = _get_matplotlib_cmap(colormap) @@ -647,8 +646,12 @@ def to_rerun( elif colors is not None: point_colors = colors - if mode == "boxes": - # Use boxes for voxel visualization + if mode == "points": + return rr.Points3D( + positions=points, + colors=point_colors, + ) + elif mode == "boxes": box_size = size if size is not None else voxel_size half = box_size / 2 return rr.Boxes3D( diff --git a/dimos/msgs/sensor_msgs/__init__.py b/dimos/msgs/sensor_msgs/__init__.py index b58dda8db5..7fec2d2793 100644 --- a/dimos/msgs/sensor_msgs/__init__.py +++ b/dimos/msgs/sensor_msgs/__init__.py @@ -1,5 +1,6 @@ from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos.msgs.sensor_msgs.Imu import Imu from dimos.msgs.sensor_msgs.JointCommand import JointCommand from dimos.msgs.sensor_msgs.JointState import JointState from dimos.msgs.sensor_msgs.Joy import Joy @@ -10,6 +11,7 @@ "CameraInfo", "Image", "ImageFormat", + "Imu", "JointCommand", "JointState", "Joy", diff --git a/dimos/spec/perception.py b/dimos/spec/perception.py index 4dc682523f..33e3d1dc97 100644 --- a/dimos/spec/perception.py +++ b/dimos/spec/perception.py @@ -15,7 +15,7 @@ from typing import Protocol from dimos.core import Out -from dimos.msgs.sensor_msgs import CameraInfo, Image as ImageMsg, PointCloud2 +from dimos.msgs.sensor_msgs import CameraInfo, Image as ImageMsg, Imu, PointCloud2 class Image(Protocol): @@ -34,3 +34,13 @@ class DepthCamera(Camera): class Pointcloud(Protocol): pointcloud: Out[PointCloud2] + + +class IMU(Protocol): + imu: Out[Imu] + + +class Lidar(Pointcloud, Protocol): + """LiDAR sensor providing point clouds.""" + + pass diff --git a/dimos/spec/utils.py b/dimos/spec/utils.py index b9786b91b5..31b6d3e5b5 100644 --- a/dimos/spec/utils.py +++ b/dimos/spec/utils.py @@ -13,6 +13,7 @@ # limitations under the License. import inspect +import typing from typing import Any, Protocol, runtime_checkable from annotation_protocol import AnnotationProtocol # type: ignore[import-not-found,import-untyped] @@ -99,6 +100,31 @@ def foo(self) -> int: ... return isinstance(obj, strict_proto) +def assert_implements_protocol(cls: type, protocol: type) -> None: + """Assert that cls has all annotations required by a Protocol. + + Works with any Protocol (not just Spec subclasses). Checks that every + annotation defined by the protocol is present on cls with a matching type. + + Example: + class MyProto(Protocol): + x: Out[int] + + class Good: + x: Out[int] + + assert_implements_protocol(Good, MyProto) # passes + """ + proto_hints = typing.get_type_hints(protocol, include_extras=True) + cls_hints = typing.get_type_hints(cls, include_extras=True) + + for name, expected_type in proto_hints.items(): + assert name in cls_hints, f"{cls.__name__} missing '{name}' required by {protocol.__name__}" + assert cls_hints[name] == expected_type, ( + f"{cls.__name__}.{name}: expected {expected_type}, got {cls_hints[name]}" + ) + + def get_protocol_method_signatures(proto: type[object]) -> dict[str, inspect.Signature]: """ Return a mapping of method_name -> inspect.Signature diff --git a/dimos/visualization/rerun/bridge.py b/dimos/visualization/rerun/bridge.py index 5263e59a44..af8c4de4cf 100644 --- a/dimos/visualization/rerun/bridge.py +++ b/dimos/visualization/rerun/bridge.py @@ -255,6 +255,7 @@ def start(self) -> None: # Initialize and spawn Rerun viewer rr.init("dimos") + if self.config.viewer_mode == "native": rr.spawn(connect=True, memory_limit=self.config.memory_limit) elif self.config.viewer_mode == "web": @@ -309,35 +310,6 @@ def run_bridge( # any pubsub that supports subscribe_all and topic that supports str(topic) # is acceptable here pubsubs=[LCM(autoconf=True)], - # Custom converters for specific rerun entity paths - # Normally all these would be specified in their respectative modules - # Until this is implemented we have central overrides here - # - # This is unsustainable once we move to multi robot etc - visual_override={ - "world/camera_info": lambda camera_info: camera_info.to_rerun( - image_topic="/world/color_image", - optical_frame="camera_optical", - ), - "world/global_map": lambda grid: grid.to_rerun(voxel_size=0.1), - "world/navigation_costmap": lambda grid: grid.to_rerun( - colormap="Accent", - z_offset=0.015, - opacity=0.2, - background="#484981", - ), - }, - # slapping a go2 shaped box on top of tf/base_link - static={ - "world/tf/base_link": lambda rr: [ - rr.Boxes3D( - half_sizes=[0.35, 0.155, 0.2], - colors=[(0, 255, 127)], - fill_mode="wireframe", - ), - rr.Transform3D(parent_frame="tf#/base_link"), - ] - }, ) bridge.start() @@ -346,7 +318,7 @@ def run_bridge( signal.pause() -def _cli( +def main( viewer_mode: str = typer.Option( "native", help="Viewer mode: native (desktop), web (browser), none (headless)" ), @@ -359,7 +331,7 @@ def _cli( if __name__ == "__main__": - typer.run(_cli) + typer.run(main) # you don't need to include this in your blueprint if you are not creating a # custom rerun configuration for your deployment, you can also run rerun-bridge standalone From 6618eede4cf7f29925fc14d937a62ff77c3eb695 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Thu, 12 Feb 2026 03:31:43 +0800 Subject: [PATCH 02/15] Fix LiDAR module port resolution broken by `from __future__ import annotations` The future annotations import made Out[PointCloud2] and Out[Imu] lazy strings, preventing the Module metaclass from creating real port descriptors. Move type imports out of TYPE_CHECKING block so ports are properly resolved at class definition time. --- dimos/hardware/sensors/lidar/module.py | 15 +++++---------- 1 file changed, 5 insertions(+), 10 deletions(-) diff --git a/dimos/hardware/sensors/lidar/module.py b/dimos/hardware/sensors/lidar/module.py index d637ea347a..9a277f42ae 100644 --- a/dimos/hardware/sensors/lidar/module.py +++ b/dimos/hardware/sensors/lidar/module.py @@ -14,26 +14,21 @@ """LiDAR module wrappers that convert LidarHardware observables into module streams.""" -from __future__ import annotations - +from collections.abc import Callable from dataclasses import dataclass, field import time -from typing import TYPE_CHECKING, Any +from typing import Any import reactivex as rx from reactivex import operators as ops from dimos.core import Module, ModuleConfig, Out, rpc +from dimos.hardware.sensors.lidar.spec import LidarHardware from dimos.msgs.geometry_msgs import Quaternion, Transform, Vector3 +from dimos.msgs.sensor_msgs.Imu import Imu +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.spec import perception -if TYPE_CHECKING: - from collections.abc import Callable - - from dimos.hardware.sensors.lidar.spec import LidarHardware - from dimos.msgs.sensor_msgs.Imu import Imu - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 - def default_lidar_transform() -> Transform: return Transform( From 2746433a3592c82b4679c923bf6a711f7aaab1d2 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Thu, 12 Feb 2026 03:58:31 +0800 Subject: [PATCH 03/15] Move LivoxLidarModule into livox/ directory Livox-specific module (LivoxLidarModule, LivoxLidarModuleConfig) now lives in dimos/hardware/sensors/lidar/livox/module.py alongside the driver and SDK code. Generic LidarModule stays in the parent. --- dimos/hardware/sensors/lidar/livox/module.py | 59 ++++++++++++++ .../lidar/livox/test_spec_compliance.py | 29 +++++++ dimos/hardware/sensors/lidar/module.py | 38 +-------- test_livox_hw.py | 80 +++++++++++++++++++ 4 files changed, 169 insertions(+), 37 deletions(-) create mode 100644 dimos/hardware/sensors/lidar/livox/module.py create mode 100644 dimos/hardware/sensors/lidar/livox/test_spec_compliance.py create mode 100644 test_livox_hw.py diff --git a/dimos/hardware/sensors/lidar/livox/module.py b/dimos/hardware/sensors/lidar/livox/module.py new file mode 100644 index 0000000000..bdde07225f --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/module.py @@ -0,0 +1,59 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Livox-specific LiDAR module with IMU support.""" + +from dataclasses import dataclass + +from dimos.core import Out, rpc +from dimos.hardware.sensors.lidar.module import LidarModule, LidarModuleConfig +from dimos.msgs.sensor_msgs.Imu import Imu +from dimos.spec import perception + + +@dataclass +class LivoxLidarModuleConfig(LidarModuleConfig): + enable_imu: bool = True + + +class LivoxLidarModule(LidarModule, perception.IMU): + """Livox LiDAR module — pointcloud + IMU. + + Extends LidarModule with IMU stream support for sensors like the Mid-360. + """ + + imu: Out[Imu] + + config: LivoxLidarModuleConfig # type: ignore[assignment] + default_config = LivoxLidarModuleConfig # type: ignore[assignment] + + @rpc + def start(self) -> None: + super().start() + + if self.config.enable_imu: + imu_stream = self.hardware.imu_stream() + if imu_stream is not None: + self._disposables.add( + imu_stream.subscribe(lambda imu_msg: self.imu.publish(imu_msg)), + ) + + +livox_lidar_module = LivoxLidarModule.blueprint + +__all__ = [ + "LivoxLidarModule", + "LivoxLidarModuleConfig", + "livox_lidar_module", +] diff --git a/dimos/hardware/sensors/lidar/livox/test_spec_compliance.py b/dimos/hardware/sensors/lidar/livox/test_spec_compliance.py new file mode 100644 index 0000000000..79abb58dbd --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/test_spec_compliance.py @@ -0,0 +1,29 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Spec compliance tests for the Livox LiDAR module.""" + +from __future__ import annotations + +from dimos.hardware.sensors.lidar.livox.module import LivoxLidarModule +from dimos.spec import perception +from dimos.spec.utils import assert_implements_protocol + + +def test_livox_module_implements_lidar_spec() -> None: + assert_implements_protocol(LivoxLidarModule, perception.Lidar) + + +def test_livox_module_implements_imu_spec() -> None: + assert_implements_protocol(LivoxLidarModule, perception.IMU) diff --git a/dimos/hardware/sensors/lidar/module.py b/dimos/hardware/sensors/lidar/module.py index 9a277f42ae..660044174b 100644 --- a/dimos/hardware/sensors/lidar/module.py +++ b/dimos/hardware/sensors/lidar/module.py @@ -25,7 +25,6 @@ from dimos.core import Module, ModuleConfig, Out, rpc from dimos.hardware.sensors.lidar.spec import LidarHardware from dimos.msgs.geometry_msgs import Quaternion, Transform, Vector3 -from dimos.msgs.sensor_msgs.Imu import Imu from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.spec import perception @@ -98,45 +97,10 @@ def stop(self) -> None: super().stop() -# --------------------------------------------------------------------------- -# Livox-specific: LiDAR + IMU -# --------------------------------------------------------------------------- - - -@dataclass -class LivoxLidarModuleConfig(LidarModuleConfig): - enable_imu: bool = True - - -class LivoxLidarModule(LidarModule, perception.IMU): - """Livox LiDAR module — pointcloud + IMU. - - Extends LidarModule with IMU stream support for sensors like the Mid-360. - """ - - imu: Out[Imu] - - config: LivoxLidarModuleConfig # type: ignore[assignment] - default_config = LivoxLidarModuleConfig # type: ignore[assignment] - - @rpc - def start(self) -> None: - super().start() - - if self.config.enable_imu: - imu_stream = self.hardware.imu_stream() - if imu_stream is not None: - self._disposables.add( - imu_stream.subscribe(lambda imu_msg: self.imu.publish(imu_msg)), - ) - - lidar_module = LidarModule.blueprint -livox_lidar_module = LivoxLidarModule.blueprint __all__ = [ "LidarModule", - "LivoxLidarModule", + "LidarModuleConfig", "lidar_module", - "livox_lidar_module", ] diff --git a/test_livox_hw.py b/test_livox_hw.py new file mode 100644 index 0000000000..4384454302 --- /dev/null +++ b/test_livox_hw.py @@ -0,0 +1,80 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Deploy LivoxLidarModule with LCM transport for pointcloud and IMU.""" + +import time + +from dimos.core import In, Module, start +from dimos.core.transport import LCMTransport +from dimos.hardware.sensors.lidar.livox.mid360 import LivoxMid360 +from dimos.hardware.sensors.lidar.livox.module import LivoxLidarModule +from dimos.msgs.sensor_msgs.Imu import Imu +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + +pc_count = 0 +imu_count = 0 + + +class LidarListener(Module): + pointcloud: In[PointCloud2] + imu: In[Imu] + + def start(self): + super().start() + self.pointcloud.subscribe(self._on_pc) + self.imu.subscribe(self._on_imu) + + def _on_pc(self, pc): + global pc_count + pc_count += 1 + n = len(pc.pointcloud.points) if hasattr(pc.pointcloud, "points") else "?" + print(f" [PC #{pc_count}] {n} points, ts={pc.ts:.3f}", flush=True) + + def _on_imu(self, imu_msg): + global imu_count + imu_count += 1 + if imu_count % 200 == 1: + print( + f" [IMU #{imu_count}] acc=({imu_msg.linear_acceleration.x:.2f}, " + f"{imu_msg.linear_acceleration.y:.2f}, {imu_msg.linear_acceleration.z:.2f})", + flush=True, + ) + + +if __name__ == "__main__": + dimos = start(2) + + lidar = dimos.deploy( + LivoxLidarModule, + hardware=lambda: LivoxMid360(host_ip="192.168.1.5", lidar_ips=["192.168.1.155"]), + ) + listener = dimos.deploy(LidarListener) + + lidar.pointcloud.transport = LCMTransport("/lidar/pointcloud", PointCloud2) + lidar.imu.transport = LCMTransport("/lidar/imu", Imu) + listener.pointcloud.transport = LCMTransport("/lidar/pointcloud", PointCloud2) + listener.imu.transport = LCMTransport("/lidar/imu", Imu) + + lidar.start() + listener.start() + + print("LivoxLidarModule + listener running. Ctrl+C to stop.", flush=True) + try: + while True: + time.sleep(1) + except KeyboardInterrupt: + pass + + dimos.stop() From 1bbb3fb1c5273abfca089a08ff35d92371f341b6 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Thu, 12 Feb 2026 15:35:19 +0800 Subject: [PATCH 04/15] livox voxelization --- dimos/hardware/sensors/lidar/livox/mid360.py | 9 ++++++++- dimos/msgs/sensor_msgs/PointCloud2.py | 5 +++++ .../unitree_webrtc/unitree_go2_blueprints.py | 2 +- dimos/visualization/rerun/bridge.py | 5 +++++ test_livox_hw.py | 16 ++++++++++++++-- 5 files changed, 33 insertions(+), 4 deletions(-) diff --git a/dimos/hardware/sensors/lidar/livox/mid360.py b/dimos/hardware/sensors/lidar/livox/mid360.py index 9c06c4af20..dbf8a5d81b 100644 --- a/dimos/hardware/sensors/lidar/livox/mid360.py +++ b/dimos/hardware/sensors/lidar/livox/mid360.py @@ -68,7 +68,7 @@ class LivoxMid360Config(LidarConfig): host_ip: str = "192.168.1.5" lidar_ips: list[str] = field(default_factory=lambda: ["192.168.1.155"]) - frequency: float = 10.0 # Hz, point cloud output rate + frequency: float = 5.0 # Hz, point cloud output rate frame_id: str = "lidar_link" frame_id_prefix: str | None = None enable_imu: bool = True @@ -86,6 +86,9 @@ class LivoxMid360Config(LidarConfig): host_imu_data_port: int = 56401 host_log_data_port: int = 56501 + # Voxel downsampling size in meters (None = no downsampling) + voxel_size: float | None = None + # Socket buffer size (bytes) to avoid packet loss at high data rates recv_buf_size: int = 4 * 1024 * 1024 # 4 MB @@ -334,6 +337,10 @@ def _emit_pointcloud( dtype=o3c.float32, ) + # Optional voxel downsampling + if self.config.voxel_size is not None: + pcd_t = pcd_t.voxel_down_sample(self.config.voxel_size) + pc2 = PointCloud2( pointcloud=pcd_t, frame_id=self._frame_id(self.config.frame_id), diff --git a/dimos/msgs/sensor_msgs/PointCloud2.py b/dimos/msgs/sensor_msgs/PointCloud2.py index 911c66997e..7b087356ec 100644 --- a/dimos/msgs/sensor_msgs/PointCloud2.py +++ b/dimos/msgs/sensor_msgs/PointCloud2.py @@ -654,6 +654,11 @@ def to_rerun( elif mode == "boxes": box_size = size if size is not None else voxel_size half = box_size / 2 + # Snap points to voxel grid centers so boxes tile properly + points = np.floor(points / box_size) * box_size + half + points, unique_idx = np.unique(points, axis=0, return_index=True) + if point_colors is not None and isinstance(point_colors, np.ndarray): + point_colors = point_colors[unique_idx] return rr.Boxes3D( centers=points, half_sizes=[half, half, half], diff --git a/dimos/robot/unitree_webrtc/unitree_go2_blueprints.py b/dimos/robot/unitree_webrtc/unitree_go2_blueprints.py index 56a3710dcf..96f722ad98 100644 --- a/dimos/robot/unitree_webrtc/unitree_go2_blueprints.py +++ b/dimos/robot/unitree_webrtc/unitree_go2_blueprints.py @@ -88,7 +88,7 @@ image_topic="/world/color_image", optical_frame="camera_optical", ), - "world/global_map": lambda grid: grid.to_rerun(voxel_size=0.1), + "world/global_map": lambda grid: grid.to_rerun(voxel_size=0.1, mode="boxes"), "world/navigation_costmap": lambda grid: grid.to_rerun( colormap="Accent", z_offset=0.015, diff --git a/dimos/visualization/rerun/bridge.py b/dimos/visualization/rerun/bridge.py index af8c4de4cf..4e39f0b64c 100644 --- a/dimos/visualization/rerun/bridge.py +++ b/dimos/visualization/rerun/bridge.py @@ -247,6 +247,11 @@ def _on_message(self, msg: Any, topic: Any) -> None: else: rr.log(entity_path, cast("Archetype", rerun_data)) + # # Connect entity to its TF frame so transforms apply correctly + # frame_id = getattr(msg, "frame_id", None) + # if frame_id and not is_rerun_multi(rerun_data): + # rr.log(entity_path, rr.Transform3D(parent_frame="tf#/" + frame_id)) + @rpc def start(self) -> None: import rerun as rr diff --git a/test_livox_hw.py b/test_livox_hw.py index 4384454302..75daacff38 100644 --- a/test_livox_hw.py +++ b/test_livox_hw.py @@ -22,6 +22,7 @@ from dimos.hardware.sensors.lidar.livox.module import LivoxLidarModule from dimos.msgs.sensor_msgs.Imu import Imu from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.visualization.rerun.bridge import RerunBridgeModule pc_count = 0 imu_count = 0 @@ -54,13 +55,23 @@ def _on_imu(self, imu_msg): if __name__ == "__main__": - dimos = start(2) + dimos = start(3) lidar = dimos.deploy( LivoxLidarModule, - hardware=lambda: LivoxMid360(host_ip="192.168.1.5", lidar_ips=["192.168.1.155"]), + hardware=lambda: LivoxMid360( + host_ip="192.168.1.5", + lidar_ips=["192.168.1.155"], + frequency=1.0, # , voxel_size=0.25 + ), ) listener = dimos.deploy(LidarListener) + bridge = dimos.deploy( + RerunBridgeModule, + visual_override={ + # "world/lidar/pointcloud": lambda pc: pc.to_rerun(mode="boxes", voxel_size=0.25), + }, + ) lidar.pointcloud.transport = LCMTransport("/lidar/pointcloud", PointCloud2) lidar.imu.transport = LCMTransport("/lidar/imu", Imu) @@ -69,6 +80,7 @@ def _on_imu(self, imu_msg): lidar.start() listener.start() + bridge.start() print("LivoxLidarModule + listener running. Ctrl+C to stop.", flush=True) try: From 0474e74a464c568c0bd9cbcf394b6bbf7f01b260 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Thu, 12 Feb 2026 19:02:05 +0800 Subject: [PATCH 05/15] native module implementation --- dimos/core/__init__.py | 4 + dimos/core/native_echo.py | 37 ++++++ dimos/core/native_module.py | 202 +++++++++++++++++++++++++++++++ dimos/core/test_native_module.py | 168 +++++++++++++++++++++++++ 4 files changed, 411 insertions(+) create mode 100755 dimos/core/native_echo.py create mode 100644 dimos/core/native_module.py create mode 100644 dimos/core/test_native_module.py diff --git a/dimos/core/__init__.py b/dimos/core/__init__.py index 5a4acc1762..28c63b4683 100644 --- a/dimos/core/__init__.py +++ b/dimos/core/__init__.py @@ -11,6 +11,7 @@ import dimos.core.colors as colors from dimos.core.core import rpc from dimos.core.module import Module, ModuleBase, ModuleConfig, ModuleConfigT +from dimos.core.native_module import LogFormat, NativeModule, NativeModuleConfig from dimos.core.rpc_client import RPCClient from dimos.core.stream import In, Out, RemoteIn, RemoteOut, Transport from dimos.core.transport import ( @@ -39,10 +40,13 @@ "DimosCluster", "In", "LCMTransport", + "LogFormat", "Module", "ModuleBase", "ModuleConfig", "ModuleConfigT", + "NativeModule", + "NativeModuleConfig", "Out", "PubSubTF", "RPCSpec", diff --git a/dimos/core/native_echo.py b/dimos/core/native_echo.py new file mode 100755 index 0000000000..0749ec4703 --- /dev/null +++ b/dimos/core/native_echo.py @@ -0,0 +1,37 @@ +#!/usr/bin/env python3 +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Echo binary for NativeModule tests. + +Dumps CLI args as a JSON log line to stdout, then waits for SIGTERM. +""" + +import json +import os +import signal +import sys + +signal.signal(signal.SIGTERM, lambda *_: sys.exit(0)) + +output_path = os.environ.get("NATIVE_ECHO_OUTPUT") +if output_path: + with open(output_path, "w") as f: + json.dump(sys.argv[1:], f) +else: + json.dump({"event": "echo_args", "args": sys.argv[1:]}, sys.stdout) + sys.stdout.write("\n") + sys.stdout.flush() + +signal.pause() diff --git a/dimos/core/native_module.py b/dimos/core/native_module.py new file mode 100644 index 0000000000..721bb5399e --- /dev/null +++ b/dimos/core/native_module.py @@ -0,0 +1,202 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""NativeModule: blueprint-integrated wrapper for native (C/C++) executables. + +A NativeModule is a thin Python Module subclass that declares In/Out ports +for blueprint wiring but delegates all real work to a managed subprocess. +The native process receives its LCM topic names via CLI args and does +pub/sub directly on the LCM multicast bus. + +Example usage:: + + @dataclass(kw_only=True) + class MyConfig(NativeModuleConfig): + executable: str = "./build/my_module" + some_param: float = 1.0 + + class MyCppModule(NativeModule): + default_config = MyConfig + pointcloud: Out[PointCloud2] + cmd_vel: In[Twist] + + # Works with autoconnect, remappings, etc. + autoconnect( + MyCppModule.blueprint(), + SomeConsumer.blueprint(), + ).build().loop() +""" + +from __future__ import annotations + +from dataclasses import dataclass, field +import enum +import json +import os +import signal +import subprocess +import threading +from typing import IO + +from dimos.core.core import rpc +from dimos.core.module import Module, ModuleConfig +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +class LogFormat(enum.Enum): + TEXT = "text" + JSON = "json" + + +@dataclass(kw_only=True) +class NativeModuleConfig(ModuleConfig): + """Configuration for a native (C/C++) subprocess module.""" + + executable: str + extra_args: list[str] = field(default_factory=list) + extra_env: dict[str, str] = field(default_factory=dict) + cwd: str | None = None + shutdown_timeout: float = 10.0 + log_format: LogFormat = LogFormat.TEXT + + +class NativeModule(Module): + """Module that wraps a native executable as a managed subprocess. + + Subclass this, declare In/Out ports, and set ``default_config`` to a + :class:`NativeModuleConfig` subclass pointing at the executable. + + On ``start()``, the binary is launched with CLI args:: + + -- ... + + The native process should parse these args and pub/sub on the given + LCM topics directly. On ``stop()``, the process receives SIGTERM. + """ + + default_config: type[NativeModuleConfig] = NativeModuleConfig # type: ignore[assignment] + _process: subprocess.Popen[bytes] | None = None + _io_threads: list[threading.Thread] + + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + self._io_threads = [] + + @rpc + def start(self) -> None: + if self._process is not None and self._process.poll() is None: + logger.warning("Native process already running", pid=self._process.pid) + return + + topics = self._collect_topics() + extra = self._build_extra_args() + + cmd = [self.config.executable] + for name, topic_str in topics.items(): + cmd.extend([f"--{name}", topic_str]) + cmd.extend(extra) + cmd.extend(self.config.extra_args) + + env = {**os.environ, **self.config.extra_env} + cwd = self.config.cwd + + logger.info("Starting native process", cmd=" ".join(cmd), cwd=cwd) + self._process = subprocess.Popen( + cmd, + env=env, + cwd=cwd, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + ) + logger.info("Native process started", pid=self._process.pid) + + self._io_threads = [ + self._start_reader(self._process.stdout, "info"), + self._start_reader(self._process.stderr, "warning"), + ] + + @rpc + def stop(self) -> None: + if self._process is not None and self._process.poll() is None: + logger.info("Stopping native process", pid=self._process.pid) + self._process.send_signal(signal.SIGTERM) + try: + self._process.wait(timeout=self.config.shutdown_timeout) + except subprocess.TimeoutExpired: + logger.warning( + "Native process did not exit, sending SIGKILL", pid=self._process.pid + ) + self._process.kill() + self._process.wait(timeout=5) + for t in self._io_threads: + t.join(timeout=2) + self._io_threads = [] + self._process = None + super().stop() + + def _start_reader(self, stream: IO[bytes] | None, level: str) -> threading.Thread: + """Spawn a daemon thread that pipes a subprocess stream through the logger.""" + t = threading.Thread(target=self._read_log_stream, args=(stream, level), daemon=True) + t.start() + return t + + def _read_log_stream(self, stream: IO[bytes] | None, level: str) -> None: + if stream is None: + return + log_fn = getattr(logger, level) + for raw in stream: + line = raw.decode("utf-8", errors="replace").rstrip() + if not line: + continue + if self.config.log_format == LogFormat.JSON: + try: + data = json.loads(line) + event = data.pop("event", line) + log_fn(event, **data) + continue + except (json.JSONDecodeError, TypeError): + # TODO: log a warning about malformed JSON and the line contents + pass + log_fn(line, pid=self._process.pid if self._process else None) + stream.close() + + def _collect_topics(self) -> dict[str, str]: + """Extract LCM topic strings from blueprint-assigned stream transports.""" + topics: dict[str, str] = {} + for name in list(self.inputs) + list(self.outputs): + stream = getattr(self, name, None) + if stream is None: + continue + transport = getattr(stream, "_transport", None) + if transport is None: + continue + topic = getattr(transport, "topic", None) + if topic is not None: + topics[name] = str(topic) + return topics + + def _build_extra_args(self) -> list[str]: + """Override in subclasses to pass additional config as CLI args. + + Called after topic args, before ``config.extra_args``. + """ + return [] + + +__all__ = [ + "NativeModule", + "NativeModuleConfig", +] diff --git a/dimos/core/test_native_module.py b/dimos/core/test_native_module.py new file mode 100644 index 0000000000..8403d0f745 --- /dev/null +++ b/dimos/core/test_native_module.py @@ -0,0 +1,168 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Tests for NativeModule: blueprint wiring, topic collection, CLI arg generation. + +Every test launches the real native_echo.py subprocess via blueprint.build(). +The echo script writes received CLI args to a temp file for assertions. +""" + +from dataclasses import dataclass +import json +import os +from pathlib import Path +import tempfile +import time + +import pytest + +from dimos.core.blueprints import autoconnect +from dimos.core.core import rpc +from dimos.core.module import Module +from dimos.core.native_module import LogFormat, NativeModule, NativeModuleConfig +from dimos.core.stream import In, Out +from dimos.core.transport import LCMTransport +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.sensor_msgs.Imu import Imu +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + +_ECHO = str(Path(__file__).parent / "native_echo.py") + + +@pytest.fixture +def args_file(): + """Temp file where native_echo.py writes the CLI args it received.""" + fd, path = tempfile.mkstemp(suffix=".json", prefix="native_echo_") + os.close(fd) + os.unlink(path) + try: + yield path + finally: + if os.path.exists(path): + os.unlink(path) + + +def read_json_file(path: str) -> dict[str, str]: + """Read and parse --key value pairs from the echo output file.""" + raw: list[str] = json.loads(Path(path).read_text()) + result = {} + i = 0 + while i < len(raw): + if raw[i].startswith("--") and i + 1 < len(raw): + result[raw[i][2:]] = raw[i + 1] + i += 2 + else: + i += 1 + return result + + +# -- Test modules -- + + +@dataclass(kw_only=True) +class StubNativeConfig(NativeModuleConfig): + executable: str = _ECHO + log_format: LogFormat = LogFormat.JSON + some_param: float = 1.5 + + +class StubNativeModule(NativeModule): + default_config = StubNativeConfig + pointcloud: Out[PointCloud2] + imu: Out[Imu] + cmd_vel: In[Twist] + + def _build_extra_args(self) -> list[str]: + cfg: StubNativeConfig = self.config # type: ignore[assignment] + return ["--some_param", str(cfg.some_param)] + + +class StubConsumer(Module): + pointcloud: In[PointCloud2] + imu: In[Imu] + + @rpc + def start(self) -> None: + pass + + +class StubProducer(Module): + cmd_vel: Out[Twist] + + @rpc + def start(self) -> None: + pass + + +def test_manual(dimos_cluster, args_file) -> None: + native_module = dimos_cluster.deploy( + StubNativeModule, + some_param=2.5, + extra_env={"NATIVE_ECHO_OUTPUT": args_file}, + ) + + native_module.pointcloud.transport = LCMTransport("/my/custom/lidar", PointCloud2) + native_module.cmd_vel.transport = LCMTransport("/cmd_vel", Twist) + native_module.start() + time.sleep(1) + native_module.stop() + + assert read_json_file(args_file) == { + "cmd_vel": "/cmd_vel#geometry_msgs.Twist", + "pointcloud": "/my/custom/lidar#sensor_msgs.PointCloud2", + "some_param": "2.5", + } + + +def test_autoconnect(args_file) -> None: + """autoconnect passes correct topic args to the native subprocess.""" + blueprint = autoconnect( + StubNativeModule.blueprint( + some_param=2.5, + extra_env={"NATIVE_ECHO_OUTPUT": args_file}, + ), + StubConsumer.blueprint(), + StubProducer.blueprint(), + ).transports( + { + ("pointcloud", PointCloud2): LCMTransport("/my/custom/lidar", PointCloud2), + }, + ) + + coordinator = blueprint.global_config(viewer_backend="none").build() + try: + # Validate blueprint wiring: all modules deployed + native = coordinator.get_instance(StubNativeModule) + consumer = coordinator.get_instance(StubConsumer) + producer = coordinator.get_instance(StubProducer) + assert native is not None + assert consumer is not None + assert producer is not None + + # Out→In topics match between connected modules + assert native.pointcloud.transport.topic == consumer.pointcloud.transport.topic + assert native.imu.transport.topic == consumer.imu.transport.topic + assert producer.cmd_vel.transport.topic == native.cmd_vel.transport.topic + + # Custom transport was applied + assert native.pointcloud.transport.topic == "/my/custom/lidar" + finally: + coordinator.stop() + + assert read_json_file(args_file) == { + "cmd_vel": "/cmd_vel#geometry_msgs.Twist", + "pointcloud": "/my/custom/lidar#sensor_msgs.PointCloud2", + "imu": "/imu#sensor_msgs.Imu", + "some_param": "2.5", + } From ba87583affa319c85c6cb30209d91991fed10027 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Thu, 12 Feb 2026 19:40:07 +0800 Subject: [PATCH 06/15] livox native module wrap --- .../sensors/lidar/livox/cpp/CMakeLists.txt | 42 ++ .../sensors/lidar/livox/cpp/README.md | 73 +++ .../sensors/lidar/livox/cpp/__init__.py | 0 .../lidar/livox/cpp/dimos_native_module.hpp | 64 +++ .../hardware/sensors/lidar/livox/cpp/main.cpp | 437 ++++++++++++++++++ .../sensors/lidar/livox/cpp/module.py | 136 ++++++ .../sensors/lidar/livox/livox_blueprints.py | 22 + dimos/robot/all_blueprints.py | 4 + 8 files changed, 778 insertions(+) create mode 100644 dimos/hardware/sensors/lidar/livox/cpp/CMakeLists.txt create mode 100644 dimos/hardware/sensors/lidar/livox/cpp/README.md create mode 100644 dimos/hardware/sensors/lidar/livox/cpp/__init__.py create mode 100644 dimos/hardware/sensors/lidar/livox/cpp/dimos_native_module.hpp create mode 100644 dimos/hardware/sensors/lidar/livox/cpp/main.cpp create mode 100644 dimos/hardware/sensors/lidar/livox/cpp/module.py create mode 100644 dimos/hardware/sensors/lidar/livox/livox_blueprints.py diff --git a/dimos/hardware/sensors/lidar/livox/cpp/CMakeLists.txt b/dimos/hardware/sensors/lidar/livox/cpp/CMakeLists.txt new file mode 100644 index 0000000000..b0c80c5834 --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/cpp/CMakeLists.txt @@ -0,0 +1,42 @@ +cmake_minimum_required(VERSION 3.14) +project(livox_mid360_native CXX) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +# Fetch dimos-lcm for C++ message headers +include(FetchContent) +FetchContent_Declare(dimos_lcm + GIT_REPOSITORY https://github.com/dimensionalOS/dimos-lcm.git + GIT_TAG main + GIT_SHALLOW TRUE +) +FetchContent_MakeAvailable(dimos_lcm) + +# Find LCM +find_package(PkgConfig REQUIRED) +pkg_check_modules(LCM REQUIRED lcm) + +# Livox SDK2 (installed to /usr/local) +find_library(LIVOX_SDK livox_lidar_sdk_shared HINTS /usr/local/lib) +if(NOT LIVOX_SDK) + message(FATAL_ERROR "Livox SDK2 not found. Install from https://github.com/Livox-SDK/Livox-SDK2") +endif() + +add_executable(mid360_native main.cpp) + +target_include_directories(mid360_native PRIVATE + ${dimos_lcm_SOURCE_DIR}/generated/cpp_lcm_msgs + ${LCM_INCLUDE_DIRS} + ${CMAKE_CURRENT_SOURCE_DIR} + /usr/local/include # Livox SDK2 headers +) + +target_link_libraries(mid360_native PRIVATE + ${LCM_LIBRARIES} + ${LIVOX_SDK} +) + +target_link_directories(mid360_native PRIVATE + ${LCM_LIBRARY_DIRS} +) diff --git a/dimos/hardware/sensors/lidar/livox/cpp/README.md b/dimos/hardware/sensors/lidar/livox/cpp/README.md new file mode 100644 index 0000000000..a66a10edd5 --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/cpp/README.md @@ -0,0 +1,73 @@ +# Livox Mid-360 Native Module (C++) + +Native C++ driver for the Livox Mid-360 LiDAR. Publishes PointCloud2 and IMU +data directly on LCM, bypassing Python for minimal latency. + +## Prerequisites + +- [LCM](https://lcm-proj.github.io/) (`pacman -S lcm` or build from source) +- [Livox SDK2](https://github.com/Livox-SDK/Livox-SDK2) — build and install to `/usr/local` +- CMake >= 3.14 + +### Installing Livox SDK2 + +```bash +cd ~/src +git clone https://github.com/Livox-SDK/Livox-SDK2.git +cd Livox-SDK2 && mkdir build && cd build +cmake .. && make -j$(nproc) +sudo make install +``` + +## Build + +```bash +cd dimos/hardware/sensors/lidar/livox/cpp +mkdir -p build && cd build +cmake .. +make -j$(nproc) +``` + +The binary lands at `build/mid360_native`. + +CMake automatically fetches [dimos-lcm](https://github.com/dimensionalOS/dimos-lcm) +for the C++ message headers on first configure. + +## Usage + +Normally launched by `Mid360CppModule` via the NativeModule framework — you +don't run it directly. The Python wrapper passes LCM topic strings and config +as CLI args: + +```python +from dimos.hardware.sensors.lidar.livox.cpp.module import Mid360CppModule +from dimos.core.blueprints import autoconnect + +autoconnect( + Mid360CppModule.blueprint(host_ip="192.168.1.5"), + SomeConsumer.blueprint(), +).build().loop() +``` + +### Manual invocation (for debugging) + +```bash +./build/mid360_native \ + --pointcloud '/pointcloud#sensor_msgs.PointCloud2' \ + --imu '/imu#sensor_msgs.Imu' \ + --host_ip 192.168.1.5 \ + --lidar_ip 192.168.1.155 \ + --frequency 10 +``` + +Topic strings must include the `#type` suffix — this is the actual LCM channel +name used by dimos subscribers. + +## File overview + +| File | Description | +|---|---| +| `main.cpp` | Livox SDK2 callbacks, frame accumulation, LCM publishing | +| `dimos_native_module.hpp` | Reusable header for parsing NativeModule CLI args | +| `module.py` | Python NativeModule wrapper (`Mid360CppModule`) | +| `CMakeLists.txt` | Build config, fetches dimos-lcm headers automatically | diff --git a/dimos/hardware/sensors/lidar/livox/cpp/__init__.py b/dimos/hardware/sensors/lidar/livox/cpp/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/dimos/hardware/sensors/lidar/livox/cpp/dimos_native_module.hpp b/dimos/hardware/sensors/lidar/livox/cpp/dimos_native_module.hpp new file mode 100644 index 0000000000..73ee618c34 --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/cpp/dimos_native_module.hpp @@ -0,0 +1,64 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 +// +// Lightweight header-only helper for dimos NativeModule C++ binaries. +// Parses -- CLI args passed by the Python NativeModule wrapper. + +#pragma once + +#include +#include +#include + +namespace dimos { + +class NativeModule { +public: + NativeModule(int argc, char** argv) { + for (int i = 1; i < argc; ++i) { + std::string arg(argv[i]); + if (arg.size() > 2 && arg[0] == '-' && arg[1] == '-' && i + 1 < argc) { + args_[arg.substr(2)] = argv[++i]; + } + } + } + + /// Get the full LCM channel string for a declared port. + /// Format is "#", e.g. "/pointcloud#sensor_msgs.PointCloud2". + /// This is the exact channel name used by Python LCMTransport subscribers. + const std::string& topic(const std::string& port) const { + auto it = args_.find(port); + if (it == args_.end()) { + throw std::runtime_error("NativeModule: no topic for port '" + port + "'"); + } + return it->second; + } + + /// Get a string arg value, or a default if not present. + std::string arg(const std::string& key, const std::string& default_val = "") const { + auto it = args_.find(key); + return it != args_.end() ? it->second : default_val; + } + + /// Get a float arg value, or a default if not present. + float arg_float(const std::string& key, float default_val = 0.0f) const { + auto it = args_.find(key); + return it != args_.end() ? std::stof(it->second) : default_val; + } + + /// Get an int arg value, or a default if not present. + int arg_int(const std::string& key, int default_val = 0) const { + auto it = args_.find(key); + return it != args_.end() ? std::stoi(it->second) : default_val; + } + + /// Check if a port/arg was provided. + bool has(const std::string& key) const { + return args_.count(key) > 0; + } + +private: + std::map args_; +}; + +} // namespace dimos diff --git a/dimos/hardware/sensors/lidar/livox/cpp/main.cpp b/dimos/hardware/sensors/lidar/livox/cpp/main.cpp new file mode 100644 index 0000000000..3c7cf5ce17 --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/cpp/main.cpp @@ -0,0 +1,437 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 +// +// Livox Mid-360 native module for dimos NativeModule framework. +// +// Publishes PointCloud2 and Imu messages on LCM topics received via CLI args. +// Usage: ./mid360_native --pointcloud --imu [--host_ip ] [--lidar_ip ] ... + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "dimos_native_module.hpp" + +#include "geometry_msgs/Quaternion.hpp" +#include "geometry_msgs/Vector3.hpp" +#include "sensor_msgs/Imu.hpp" +#include "sensor_msgs/PointCloud2.hpp" +#include "sensor_msgs/PointField.hpp" +#include "std_msgs/Header.hpp" +#include "std_msgs/Time.hpp" + +// Gravity constant for converting accelerometer data from g to m/s^2 +static constexpr double GRAVITY_MS2 = 9.80665; + +// Livox data_type values (not provided as named constants in SDK2 header) +static constexpr uint8_t DATA_TYPE_IMU = 0x00; +static constexpr uint8_t DATA_TYPE_CARTESIAN_HIGH = 0x01; +static constexpr uint8_t DATA_TYPE_CARTESIAN_LOW = 0x02; + +// --------------------------------------------------------------------------- +// Global state +// --------------------------------------------------------------------------- + +static std::atomic g_running{true}; +static lcm::LCM* g_lcm = nullptr; +static std::string g_pointcloud_topic; +static std::string g_imu_topic; +static std::string g_frame_id = "lidar_link"; +static std::string g_imu_frame_id = "imu_link"; +static float g_frequency = 10.0f; + +// Frame accumulator +static std::mutex g_pc_mutex; +static std::vector g_accumulated_xyz; // interleaved x,y,z +static std::vector g_accumulated_intensity; // per-point intensity +static double g_frame_timestamp = 0.0; +static bool g_frame_has_timestamp = false; + +// --------------------------------------------------------------------------- +// Helpers +// --------------------------------------------------------------------------- + +static std_msgs::Time time_from_seconds(double t) { + std_msgs::Time ts; + ts.sec = static_cast(t); + ts.nsec = static_cast((t - ts.sec) * 1e9); + return ts; +} + +static double get_timestamp_ns(const LivoxLidarEthernetPacket* pkt) { + uint64_t ns = 0; + std::memcpy(&ns, pkt->timestamp, sizeof(uint64_t)); + return static_cast(ns); +} + +static std_msgs::Header make_header(const std::string& frame_id, double ts) { + static std::atomic seq{0}; + std_msgs::Header h; + h.seq = seq.fetch_add(1, std::memory_order_relaxed); + h.stamp = time_from_seconds(ts); + h.frame_id = frame_id; + return h; +} + +// --------------------------------------------------------------------------- +// Build and publish PointCloud2 +// --------------------------------------------------------------------------- + +static void publish_pointcloud(const std::vector& xyz, + const std::vector& intensity, + double timestamp) { + if (!g_lcm || xyz.empty()) return; + + int num_points = static_cast(xyz.size()) / 3; + + sensor_msgs::PointCloud2 pc; + pc.header = make_header(g_frame_id, timestamp); + pc.height = 1; + pc.width = num_points; + pc.is_bigendian = 0; + pc.is_dense = 1; + + // Fields: x, y, z (float32), intensity (float32) + pc.fields_length = 4; + pc.fields.resize(4); + + auto make_field = [](const std::string& name, int32_t offset) { + sensor_msgs::PointField f; + f.name = name; + f.offset = offset; + f.datatype = sensor_msgs::PointField::FLOAT32; + f.count = 1; + return f; + }; + + pc.fields[0] = make_field("x", 0); + pc.fields[1] = make_field("y", 4); + pc.fields[2] = make_field("z", 8); + pc.fields[3] = make_field("intensity", 12); + + pc.point_step = 16; // 4 floats * 4 bytes + pc.row_step = pc.point_step * num_points; + + // Pack point data + pc.data_length = pc.row_step; + pc.data.resize(pc.data_length); + + for (int i = 0; i < num_points; ++i) { + float* dst = reinterpret_cast(pc.data.data() + i * 16); + dst[0] = xyz[i * 3 + 0]; + dst[1] = xyz[i * 3 + 1]; + dst[2] = xyz[i * 3 + 2]; + dst[3] = intensity[i]; + } + + g_lcm->publish(g_pointcloud_topic, &pc); +} + +// --------------------------------------------------------------------------- +// SDK callbacks +// --------------------------------------------------------------------------- + +static void on_point_cloud(const uint32_t /*handle*/, const uint8_t /*dev_type*/, + LivoxLidarEthernetPacket* data, void* /*client_data*/) { + if (!g_running.load() || data == nullptr) return; + + double ts_ns = get_timestamp_ns(data); + double ts = ts_ns / 1e9; + uint16_t dot_num = data->dot_num; + + std::lock_guard lock(g_pc_mutex); + + if (!g_frame_has_timestamp) { + g_frame_timestamp = ts; + g_frame_has_timestamp = true; + } + + if (data->data_type == DATA_TYPE_CARTESIAN_HIGH) { + auto* pts = reinterpret_cast(data->data); + for (uint16_t i = 0; i < dot_num; ++i) { + // Livox high-precision coordinates are in mm, convert to meters + g_accumulated_xyz.push_back(static_cast(pts[i].x) / 1000.0f); + g_accumulated_xyz.push_back(static_cast(pts[i].y) / 1000.0f); + g_accumulated_xyz.push_back(static_cast(pts[i].z) / 1000.0f); + g_accumulated_intensity.push_back(static_cast(pts[i].reflectivity) / 255.0f); + } + } else if (data->data_type == DATA_TYPE_CARTESIAN_LOW) { + auto* pts = reinterpret_cast(data->data); + for (uint16_t i = 0; i < dot_num; ++i) { + // Livox low-precision coordinates are in cm, convert to meters + g_accumulated_xyz.push_back(static_cast(pts[i].x) / 100.0f); + g_accumulated_xyz.push_back(static_cast(pts[i].y) / 100.0f); + g_accumulated_xyz.push_back(static_cast(pts[i].z) / 100.0f); + g_accumulated_intensity.push_back(static_cast(pts[i].reflectivity) / 255.0f); + } + } +} + +static void on_imu_data(const uint32_t /*handle*/, const uint8_t /*dev_type*/, + LivoxLidarEthernetPacket* data, void* /*client_data*/) { + if (!g_running.load() || data == nullptr || !g_lcm) return; + if (g_imu_topic.empty()) return; + + double ts = get_timestamp_ns(data) / 1e9; + auto* imu_pts = reinterpret_cast(data->data); + uint16_t dot_num = data->dot_num; + + for (uint16_t i = 0; i < dot_num; ++i) { + sensor_msgs::Imu msg; + msg.header = make_header(g_imu_frame_id, ts); + + // Orientation unknown — set to identity with high covariance + msg.orientation.x = 0.0; + msg.orientation.y = 0.0; + msg.orientation.z = 0.0; + msg.orientation.w = 1.0; + msg.orientation_covariance[0] = -1.0; // indicates unknown + + msg.angular_velocity.x = static_cast(imu_pts[i].gyro_x); + msg.angular_velocity.y = static_cast(imu_pts[i].gyro_y); + msg.angular_velocity.z = static_cast(imu_pts[i].gyro_z); + + msg.linear_acceleration.x = static_cast(imu_pts[i].acc_x) * GRAVITY_MS2; + msg.linear_acceleration.y = static_cast(imu_pts[i].acc_y) * GRAVITY_MS2; + msg.linear_acceleration.z = static_cast(imu_pts[i].acc_z) * GRAVITY_MS2; + + g_lcm->publish(g_imu_topic, &msg); + } +} + +static void on_info_change(const uint32_t handle, const LivoxLidarInfo* info, + void* /*client_data*/) { + if (info == nullptr) return; + + char sn[17] = {}; + std::memcpy(sn, info->sn, 16); + char ip[17] = {}; + std::memcpy(ip, info->lidar_ip, 16); + + printf("[mid360] Device connected: handle=%u type=%u sn=%s ip=%s\n", + handle, info->dev_type, sn, ip); + + // Set to normal work mode + SetLivoxLidarWorkMode(handle, kLivoxLidarNormal, nullptr, nullptr); + + // Enable IMU + if (!g_imu_topic.empty()) { + EnableLivoxLidarImuData(handle, nullptr, nullptr); + } +} + +// --------------------------------------------------------------------------- +// Signal handling +// --------------------------------------------------------------------------- + +static void signal_handler(int /*sig*/) { + g_running.store(false); +} + +// --------------------------------------------------------------------------- +// SDK config file generation +// --------------------------------------------------------------------------- + +struct SdkPorts { + int cmd_data = 56100; + int push_msg = 56200; + int point_data = 56300; + int imu_data = 56400; + int log_data = 56500; + int host_cmd_data = 56101; + int host_push_msg = 56201; + int host_point_data = 56301; + int host_imu_data = 56401; + int host_log_data = 56501; +}; + +static std::string write_sdk_config(const std::string& host_ip, + const std::string& lidar_ip, + const SdkPorts& ports) { + char tmpl[] = "/tmp/livox_mid360_XXXXXX"; + int fd = mkstemp(tmpl); + if (fd < 0) { + perror("mkstemp"); + return ""; + } + + // fdopen takes ownership of fd — fclose will close it + FILE* fp = fdopen(fd, "w"); + if (!fp) { + perror("fdopen"); + close(fd); + return ""; + } + + fprintf(fp, + "{\n" + " \"MID360\": {\n" + " \"lidar_net_info\": {\n" + " \"cmd_data_port\": %d,\n" + " \"push_msg_port\": %d,\n" + " \"point_data_port\": %d,\n" + " \"imu_data_port\": %d,\n" + " \"log_data_port\": %d\n" + " },\n" + " \"host_net_info\": [\n" + " {\n" + " \"host_ip\": \"%s\",\n" + " \"multicast_ip\": \"224.1.1.5\",\n" + " \"cmd_data_port\": %d,\n" + " \"push_msg_port\": %d,\n" + " \"point_data_port\": %d,\n" + " \"imu_data_port\": %d,\n" + " \"log_data_port\": %d\n" + " }\n" + " ]\n" + " }\n" + "}\n", + ports.cmd_data, ports.push_msg, ports.point_data, + ports.imu_data, ports.log_data, + host_ip.c_str(), + ports.host_cmd_data, ports.host_push_msg, ports.host_point_data, + ports.host_imu_data, ports.host_log_data); + fclose(fp); + + return tmpl; +} + +// --------------------------------------------------------------------------- +// Main +// --------------------------------------------------------------------------- + +int main(int argc, char** argv) { + dimos::NativeModule mod(argc, argv); + + // Required: LCM topics for ports + g_pointcloud_topic = mod.has("pointcloud") ? mod.topic("pointcloud") : ""; + g_imu_topic = mod.has("imu") ? mod.topic("imu") : ""; + + if (g_pointcloud_topic.empty()) { + fprintf(stderr, "Error: --pointcloud is required\n"); + return 1; + } + + // Optional config args + std::string host_ip = mod.arg("host_ip", "192.168.1.5"); + std::string lidar_ip = mod.arg("lidar_ip", "192.168.1.155"); + g_frequency = mod.arg_float("frequency", 10.0f); + g_frame_id = mod.arg("frame_id", "lidar_link"); + g_imu_frame_id = mod.arg("imu_frame_id", "imu_link"); + + // SDK network ports (configurable for multi-sensor setups) + SdkPorts ports; + ports.cmd_data = mod.arg_int("cmd_data_port", 56100); + ports.push_msg = mod.arg_int("push_msg_port", 56200); + ports.point_data = mod.arg_int("point_data_port", 56300); + ports.imu_data = mod.arg_int("imu_data_port", 56400); + ports.log_data = mod.arg_int("log_data_port", 56500); + ports.host_cmd_data = mod.arg_int("host_cmd_data_port", 56101); + ports.host_push_msg = mod.arg_int("host_push_msg_port", 56201); + ports.host_point_data = mod.arg_int("host_point_data_port", 56301); + ports.host_imu_data = mod.arg_int("host_imu_data_port", 56401); + ports.host_log_data = mod.arg_int("host_log_data_port", 56501); + + printf("[mid360] Starting native Livox Mid-360 module\n"); + printf("[mid360] pointcloud topic: %s\n", g_pointcloud_topic.c_str()); + printf("[mid360] imu topic: %s\n", g_imu_topic.empty() ? "(disabled)" : g_imu_topic.c_str()); + printf("[mid360] host_ip: %s lidar_ip: %s frequency: %.1f Hz\n", + host_ip.c_str(), lidar_ip.c_str(), g_frequency); + + // Signal handlers + signal(SIGTERM, signal_handler); + signal(SIGINT, signal_handler); + + // Init LCM + lcm::LCM lcm; + if (!lcm.good()) { + fprintf(stderr, "Error: LCM init failed\n"); + return 1; + } + g_lcm = &lcm; + + // Write SDK config + std::string config_path = write_sdk_config(host_ip, lidar_ip, ports); + if (config_path.empty()) { + fprintf(stderr, "Error: failed to write SDK config\n"); + return 1; + } + + // Init Livox SDK + if (!LivoxLidarSdkInit(config_path.c_str(), host_ip.c_str())) { + fprintf(stderr, "Error: LivoxLidarSdkInit failed\n"); + std::remove(config_path.c_str()); + return 1; + } + + // Register callbacks + SetLivoxLidarPointCloudCallBack(on_point_cloud, nullptr); + if (!g_imu_topic.empty()) { + SetLivoxLidarImuDataCallback(on_imu_data, nullptr); + } + SetLivoxLidarInfoChangeCallback(on_info_change, nullptr); + + // Start SDK + if (!LivoxLidarSdkStart()) { + fprintf(stderr, "Error: LivoxLidarSdkStart failed\n"); + LivoxLidarSdkUninit(); + std::remove(config_path.c_str()); + return 1; + } + + printf("[mid360] SDK started, waiting for device...\n"); + + // Main loop: periodically emit accumulated point clouds + auto frame_interval = std::chrono::microseconds( + static_cast(1e6 / g_frequency)); + auto last_emit = std::chrono::steady_clock::now(); + + while (g_running.load()) { + // Handle LCM (for any subscriptions, though we mostly publish) + lcm.handleTimeout(10); // 10ms timeout + + auto now = std::chrono::steady_clock::now(); + if (now - last_emit >= frame_interval) { + // Swap out the accumulated data + std::vector xyz; + std::vector intensity; + double ts = 0.0; + + { + std::lock_guard lock(g_pc_mutex); + if (!g_accumulated_xyz.empty()) { + xyz.swap(g_accumulated_xyz); + intensity.swap(g_accumulated_intensity); + ts = g_frame_timestamp; + g_frame_has_timestamp = false; + } + } + + if (!xyz.empty()) { + publish_pointcloud(xyz, intensity, ts); + } + + last_emit = now; + } + } + + // Cleanup + printf("[mid360] Shutting down...\n"); + LivoxLidarSdkUninit(); + std::remove(config_path.c_str()); + g_lcm = nullptr; + + printf("[mid360] Done.\n"); + return 0; +} diff --git a/dimos/hardware/sensors/lidar/livox/cpp/module.py b/dimos/hardware/sensors/lidar/livox/cpp/module.py new file mode 100644 index 0000000000..dc73ef6548 --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/cpp/module.py @@ -0,0 +1,136 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Python NativeModule wrapper for the C++ Livox Mid-360 driver. + +Declares the same ports as LivoxLidarModule (pointcloud, imu) but delegates +all real work to the ``mid360_native`` C++ binary, which talks directly to +the Livox SDK2 C API and publishes on LCM. + +Usage:: + + from dimos.hardware.sensors.lidar.livox.cpp.module import Mid360CppModule + from dimos.core.blueprints import autoconnect + + autoconnect( + Mid360CppModule.blueprint(host_ip="192.168.1.5"), + SomeConsumer.blueprint(), + ).build().loop() +""" + +from __future__ import annotations + +from dataclasses import dataclass +from pathlib import Path +from typing import TYPE_CHECKING + +from dimos.core.native_module import NativeModule, NativeModuleConfig +from dimos.spec import perception + +if TYPE_CHECKING: + from dimos.core import Out + from dimos.msgs.sensor_msgs.Imu import Imu + from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + +_DEFAULT_EXECUTABLE = str(Path(__file__).parent / "build" / "mid360_native") + + +@dataclass(kw_only=True) +class Mid360CppConfig(NativeModuleConfig): + """Config for the C++ Mid-360 native module.""" + + executable: str = _DEFAULT_EXECUTABLE + host_ip: str = "192.168.1.5" + lidar_ip: str = "192.168.1.155" + frequency: float = 10.0 + enable_imu: bool = True + frame_id: str = "lidar_link" + imu_frame_id: str = "imu_link" + + # SDK port configuration (match defaults in LivoxMid360Config) + cmd_data_port: int = 56100 + push_msg_port: int = 56200 + point_data_port: int = 56300 + imu_data_port: int = 56400 + log_data_port: int = 56500 + host_cmd_data_port: int = 56101 + host_push_msg_port: int = 56201 + host_point_data_port: int = 56301 + host_imu_data_port: int = 56401 + host_log_data_port: int = 56501 + + +class Mid360CppModule(NativeModule, perception.Lidar, perception.IMU): + """Livox Mid-360 LiDAR module backed by a native C++ binary. + + Ports: + pointcloud (Out[PointCloud2]): Point cloud frames at configured frequency. + imu (Out[Imu]): IMU data at ~200 Hz (if enabled). + """ + + default_config: type[Mid360CppConfig] = Mid360CppConfig # type: ignore[assignment] + pointcloud: Out[PointCloud2] + imu: Out[Imu] + + def _collect_topics(self) -> dict[str, str]: + cfg: Mid360CppConfig = self.config # type: ignore[assignment] + topics = super()._collect_topics() + if not cfg.enable_imu: + topics.pop("imu", None) + return topics + + def _build_extra_args(self) -> list[str]: + """Pass hardware config to the C++ binary as CLI args.""" + cfg: Mid360CppConfig = self.config # type: ignore[assignment] + return [ + "--host_ip", + cfg.host_ip, + "--lidar_ip", + cfg.lidar_ip, + "--frequency", + str(cfg.frequency), + "--frame_id", + cfg.frame_id, + "--imu_frame_id", + cfg.imu_frame_id, + "--cmd_data_port", + str(cfg.cmd_data_port), + "--push_msg_port", + str(cfg.push_msg_port), + "--point_data_port", + str(cfg.point_data_port), + "--imu_data_port", + str(cfg.imu_data_port), + "--log_data_port", + str(cfg.log_data_port), + "--host_cmd_data_port", + str(cfg.host_cmd_data_port), + "--host_push_msg_port", + str(cfg.host_push_msg_port), + "--host_point_data_port", + str(cfg.host_point_data_port), + "--host_imu_data_port", + str(cfg.host_imu_data_port), + "--host_log_data_port", + str(cfg.host_log_data_port), + ] + + +mid360_cpp_module = Mid360CppModule.blueprint + +__all__ = [ + "Mid360CppConfig", + "Mid360CppModule", + "mid360_cpp_module", +] diff --git a/dimos/hardware/sensors/lidar/livox/livox_blueprints.py b/dimos/hardware/sensors/lidar/livox/livox_blueprints.py new file mode 100644 index 0000000000..d93dc62a41 --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/livox_blueprints.py @@ -0,0 +1,22 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from dimos.core.blueprints import autoconnect +from dimos.hardware.sensors.lidar.livox.cpp.module import Mid360CppModule +from dimos.visualization.rerun.bridge import rerun_bridge + +mid360cpp = autoconnect( + Mid360CppModule.blueprint(), + rerun_bridge(), +).global_config(n_dask_workers=2, robot_model="mid360_livox_cpp") diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 70cc455d34..d3a6a58993 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -40,6 +40,7 @@ "demo-osm": "dimos.mapping.osm.demo_osm:demo_osm", "demo-skill": "dimos.agents.skills.demo_skill:demo_skill", "dual-xarm6-planner": "dimos.manipulation.manipulation_blueprints:dual_xarm6_planner", + "mid360cpp": "dimos.hardware.sensors.lidar.livox.livox_blueprints:mid360cpp", "unitree-g1": "dimos.robot.unitree_webrtc.unitree_g1_blueprints:unitree_g1", "unitree-g1-agentic": "dimos.robot.unitree_webrtc.unitree_g1_blueprints:unitree_g1_agentic", "unitree-g1-agentic-sim": "dimos.robot.unitree_webrtc.unitree_g1_blueprints:unitree_g1_agentic_sim", @@ -88,9 +89,12 @@ "human_input": "dimos.agents.cli.human", "joint_trajectory_controller": "dimos.manipulation.control.trajectory_controller.joint_trajectory_controller", "keyboard_teleop": "dimos.robot.unitree_webrtc.keyboard_teleop", + "lidar_module": "dimos.hardware.sensors.lidar.module", + "livox_lidar_module": "dimos.hardware.sensors.lidar.livox.module", "llm_agent": "dimos.agents.agent", "manipulation_module": "dimos.manipulation.manipulation_module", "mapper": "dimos.robot.unitree_webrtc.type.map", + "mid360_cpp_module": "dimos.hardware.sensors.lidar.livox.cpp.module", "navigation_skill": "dimos.agents.skills.navigation", "object_scene_registration_module": "dimos.perception.object_scene_registration", "object_tracking": "dimos.perception.object_tracker", From 8cd4ce98ad2c5f70db6fed1bc1146b467c3be9fa Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Thu, 12 Feb 2026 19:50:02 +0800 Subject: [PATCH 07/15] bugfix --- dimos/hardware/sensors/lidar/livox/cpp/module.py | 7 ------- 1 file changed, 7 deletions(-) diff --git a/dimos/hardware/sensors/lidar/livox/cpp/module.py b/dimos/hardware/sensors/lidar/livox/cpp/module.py index dc73ef6548..8ed346a672 100644 --- a/dimos/hardware/sensors/lidar/livox/cpp/module.py +++ b/dimos/hardware/sensors/lidar/livox/cpp/module.py @@ -83,13 +83,6 @@ class Mid360CppModule(NativeModule, perception.Lidar, perception.IMU): pointcloud: Out[PointCloud2] imu: Out[Imu] - def _collect_topics(self) -> dict[str, str]: - cfg: Mid360CppConfig = self.config # type: ignore[assignment] - topics = super()._collect_topics() - if not cfg.enable_imu: - topics.pop("imu", None) - return topics - def _build_extra_args(self) -> list[str]: """Pass hardware config to the C++ binary as CLI args.""" cfg: Mid360CppConfig = self.config # type: ignore[assignment] From 5c5f100a3f9f87888d4a5a8b30d13e85481498ce Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Thu, 12 Feb 2026 19:56:45 +0800 Subject: [PATCH 08/15] removed py livox implementation entirely --- .../sensors/lidar/livox/cpp/README.md | 4 +- .../sensors/lidar/livox/cpp/__init__.py | 0 .../sensors/lidar/livox/cpp/module.py | 129 ----- .../sensors/lidar/livox/livox_blueprints.py | 2 +- dimos/hardware/sensors/lidar/livox/mid360.py | 500 ---------------- dimos/hardware/sensors/lidar/livox/module.py | 124 +++- dimos/hardware/sensors/lidar/livox/sdk.py | 534 ------------------ .../lidar/livox/test_spec_compliance.py | 6 +- 8 files changed, 103 insertions(+), 1196 deletions(-) delete mode 100644 dimos/hardware/sensors/lidar/livox/cpp/__init__.py delete mode 100644 dimos/hardware/sensors/lidar/livox/cpp/module.py delete mode 100644 dimos/hardware/sensors/lidar/livox/mid360.py delete mode 100644 dimos/hardware/sensors/lidar/livox/sdk.py diff --git a/dimos/hardware/sensors/lidar/livox/cpp/README.md b/dimos/hardware/sensors/lidar/livox/cpp/README.md index a66a10edd5..5e78e48f86 100644 --- a/dimos/hardware/sensors/lidar/livox/cpp/README.md +++ b/dimos/hardware/sensors/lidar/livox/cpp/README.md @@ -40,7 +40,7 @@ don't run it directly. The Python wrapper passes LCM topic strings and config as CLI args: ```python -from dimos.hardware.sensors.lidar.livox.cpp.module import Mid360CppModule +from dimos.hardware.sensors.lidar.livox.module import Mid360CppModule from dimos.core.blueprints import autoconnect autoconnect( @@ -69,5 +69,5 @@ name used by dimos subscribers. |---|---| | `main.cpp` | Livox SDK2 callbacks, frame accumulation, LCM publishing | | `dimos_native_module.hpp` | Reusable header for parsing NativeModule CLI args | -| `module.py` | Python NativeModule wrapper (`Mid360CppModule`) | +| `../module.py` | Python NativeModule wrapper (`Mid360CppModule`) | | `CMakeLists.txt` | Build config, fetches dimos-lcm headers automatically | diff --git a/dimos/hardware/sensors/lidar/livox/cpp/__init__.py b/dimos/hardware/sensors/lidar/livox/cpp/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/dimos/hardware/sensors/lidar/livox/cpp/module.py b/dimos/hardware/sensors/lidar/livox/cpp/module.py deleted file mode 100644 index 8ed346a672..0000000000 --- a/dimos/hardware/sensors/lidar/livox/cpp/module.py +++ /dev/null @@ -1,129 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Python NativeModule wrapper for the C++ Livox Mid-360 driver. - -Declares the same ports as LivoxLidarModule (pointcloud, imu) but delegates -all real work to the ``mid360_native`` C++ binary, which talks directly to -the Livox SDK2 C API and publishes on LCM. - -Usage:: - - from dimos.hardware.sensors.lidar.livox.cpp.module import Mid360CppModule - from dimos.core.blueprints import autoconnect - - autoconnect( - Mid360CppModule.blueprint(host_ip="192.168.1.5"), - SomeConsumer.blueprint(), - ).build().loop() -""" - -from __future__ import annotations - -from dataclasses import dataclass -from pathlib import Path -from typing import TYPE_CHECKING - -from dimos.core.native_module import NativeModule, NativeModuleConfig -from dimos.spec import perception - -if TYPE_CHECKING: - from dimos.core import Out - from dimos.msgs.sensor_msgs.Imu import Imu - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 - -_DEFAULT_EXECUTABLE = str(Path(__file__).parent / "build" / "mid360_native") - - -@dataclass(kw_only=True) -class Mid360CppConfig(NativeModuleConfig): - """Config for the C++ Mid-360 native module.""" - - executable: str = _DEFAULT_EXECUTABLE - host_ip: str = "192.168.1.5" - lidar_ip: str = "192.168.1.155" - frequency: float = 10.0 - enable_imu: bool = True - frame_id: str = "lidar_link" - imu_frame_id: str = "imu_link" - - # SDK port configuration (match defaults in LivoxMid360Config) - cmd_data_port: int = 56100 - push_msg_port: int = 56200 - point_data_port: int = 56300 - imu_data_port: int = 56400 - log_data_port: int = 56500 - host_cmd_data_port: int = 56101 - host_push_msg_port: int = 56201 - host_point_data_port: int = 56301 - host_imu_data_port: int = 56401 - host_log_data_port: int = 56501 - - -class Mid360CppModule(NativeModule, perception.Lidar, perception.IMU): - """Livox Mid-360 LiDAR module backed by a native C++ binary. - - Ports: - pointcloud (Out[PointCloud2]): Point cloud frames at configured frequency. - imu (Out[Imu]): IMU data at ~200 Hz (if enabled). - """ - - default_config: type[Mid360CppConfig] = Mid360CppConfig # type: ignore[assignment] - pointcloud: Out[PointCloud2] - imu: Out[Imu] - - def _build_extra_args(self) -> list[str]: - """Pass hardware config to the C++ binary as CLI args.""" - cfg: Mid360CppConfig = self.config # type: ignore[assignment] - return [ - "--host_ip", - cfg.host_ip, - "--lidar_ip", - cfg.lidar_ip, - "--frequency", - str(cfg.frequency), - "--frame_id", - cfg.frame_id, - "--imu_frame_id", - cfg.imu_frame_id, - "--cmd_data_port", - str(cfg.cmd_data_port), - "--push_msg_port", - str(cfg.push_msg_port), - "--point_data_port", - str(cfg.point_data_port), - "--imu_data_port", - str(cfg.imu_data_port), - "--log_data_port", - str(cfg.log_data_port), - "--host_cmd_data_port", - str(cfg.host_cmd_data_port), - "--host_push_msg_port", - str(cfg.host_push_msg_port), - "--host_point_data_port", - str(cfg.host_point_data_port), - "--host_imu_data_port", - str(cfg.host_imu_data_port), - "--host_log_data_port", - str(cfg.host_log_data_port), - ] - - -mid360_cpp_module = Mid360CppModule.blueprint - -__all__ = [ - "Mid360CppConfig", - "Mid360CppModule", - "mid360_cpp_module", -] diff --git a/dimos/hardware/sensors/lidar/livox/livox_blueprints.py b/dimos/hardware/sensors/lidar/livox/livox_blueprints.py index d93dc62a41..a4cbae37fb 100644 --- a/dimos/hardware/sensors/lidar/livox/livox_blueprints.py +++ b/dimos/hardware/sensors/lidar/livox/livox_blueprints.py @@ -13,7 +13,7 @@ # limitations under the License. from dimos.core.blueprints import autoconnect -from dimos.hardware.sensors.lidar.livox.cpp.module import Mid360CppModule +from dimos.hardware.sensors.lidar.livox.module import Mid360CppModule from dimos.visualization.rerun.bridge import rerun_bridge mid360cpp = autoconnect( diff --git a/dimos/hardware/sensors/lidar/livox/mid360.py b/dimos/hardware/sensors/lidar/livox/mid360.py deleted file mode 100644 index dbf8a5d81b..0000000000 --- a/dimos/hardware/sensors/lidar/livox/mid360.py +++ /dev/null @@ -1,500 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Livox Mid-360 LiDAR hardware driver using Livox SDK2 ctypes bindings.""" - -from __future__ import annotations - -from dataclasses import dataclass, field -from functools import cache -import json -import logging -from queue import Empty, Queue -import tempfile -import threading -import time -from typing import TYPE_CHECKING, Any - -import numpy as np -import open3d as o3d # type: ignore[import-untyped] -import open3d.core as o3c # type: ignore[import-untyped] -from reactivex import create - -from dimos.hardware.sensors.lidar.livox.sdk import ( - AsyncControlCallbackType, - ImuDataCallbackType, - InfoChangeCallbackType, - LivoxLidarDeviceType, - LivoxLidarEthernetPacket, - LivoxLidarInfo, - LivoxLidarPointDataType, - LivoxLidarWorkMode, - LivoxSDK, - PointCloudCallbackType, - get_packet_timestamp_ns, - parse_cartesian_high_points, - parse_cartesian_low_points, - parse_imu_data, -) -from dimos.hardware.sensors.lidar.spec import LidarConfig, LidarHardware -from dimos.msgs.geometry_msgs import Vector3 -from dimos.msgs.sensor_msgs.Imu import Imu -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.utils.reactive import backpressure - -if TYPE_CHECKING: - from reactivex.observable import Observable - -logger = logging.getLogger(__name__) - -# Gravity constant for converting accelerometer data from g to m/s^2 -GRAVITY_MS2 = 9.80665 - - -@dataclass -class LivoxMid360Config(LidarConfig): - """Configuration for the Livox Mid-360 LiDAR.""" - - host_ip: str = "192.168.1.5" - lidar_ips: list[str] = field(default_factory=lambda: ["192.168.1.155"]) - frequency: float = 5.0 # Hz, point cloud output rate - frame_id: str = "lidar_link" - frame_id_prefix: str | None = None - enable_imu: bool = True - sdk_lib_path: str | None = None - - # SDK port configuration - cmd_data_port: int = 56100 - push_msg_port: int = 56200 - point_data_port: int = 56300 - imu_data_port: int = 56400 - log_data_port: int = 56500 - host_cmd_data_port: int = 56101 - host_push_msg_port: int = 56201 - host_point_data_port: int = 56301 - host_imu_data_port: int = 56401 - host_log_data_port: int = 56501 - - # Voxel downsampling size in meters (None = no downsampling) - voxel_size: float | None = None - - # Socket buffer size (bytes) to avoid packet loss at high data rates - recv_buf_size: int = 4 * 1024 * 1024 # 4 MB - - -class LivoxMid360(LidarHardware["LivoxMid360Config"]): - """Livox Mid-360 LiDAR driver using SDK2 ctypes bindings. - - Produces Observable[PointCloud2] at the configured frame rate (~10 Hz), - and optionally Observable[Imu] at ~200 Hz. - - Uses a thread-safe queue to pass data from SDK C-thread callbacks - to Python consumer threads that build messages. - """ - - default_config = LivoxMid360Config - - def __init__(self, *args: object, **kwargs: object) -> None: - super().__init__(*args, **kwargs) - self._sdk: LivoxSDK | None = None - self._config_path: str | None = None - self._stop_event = threading.Event() - - # Point cloud frame accumulator state - self._pc_queue: Queue[tuple[int, int, np.ndarray, np.ndarray]] = Queue(maxsize=4096) - self._pc_consumer_thread: threading.Thread | None = None - self._pc_observer: Any[PointCloud2] | None = None - - # IMU state - self._imu_queue: Queue[tuple[int, np.ndarray]] = Queue(maxsize=4096) - self._imu_consumer_thread: threading.Thread | None = None - self._imu_observer: Any[Imu] | None = None - - # Device tracking - self._connected_handles: dict[int, LivoxLidarDeviceType] = {} - - def _frame_id(self, suffix: str) -> str: - prefix = self.config.frame_id_prefix - if prefix: - return f"{prefix}/{suffix}" - return suffix - - # ------------------------------------------------------------------ - # SDK config generation - # ------------------------------------------------------------------ - - def _write_sdk_config(self) -> str: - """Generate a temporary JSON config file for the SDK.""" - config = { - "MID360": { - "lidar_net_info": { - "cmd_data_port": self.config.cmd_data_port, - "push_msg_port": self.config.push_msg_port, - "point_data_port": self.config.point_data_port, - "imu_data_port": self.config.imu_data_port, - "log_data_port": self.config.log_data_port, - }, - "host_net_info": [ - { - "host_ip": self.config.host_ip, - "multicast_ip": "224.1.1.5", - "cmd_data_port": self.config.host_cmd_data_port, - "push_msg_port": self.config.host_push_msg_port, - "point_data_port": self.config.host_point_data_port, - "imu_data_port": self.config.host_imu_data_port, - "log_data_port": self.config.host_log_data_port, - } - ], - } - } - fd, path = tempfile.mkstemp(suffix=".json", prefix="livox_mid360_") - with open(fd, "w") as f: - json.dump(config, f) - return path - - # ------------------------------------------------------------------ - # SDK callbacks (called from C threads - keep minimal, just enqueue) - # ------------------------------------------------------------------ - - def _on_point_cloud( - self, - handle: int, - dev_type: int, - packet_ptr: LivoxLidarEthernetPacket, # type: ignore[override] - client_data: object, - ) -> None: - """SDK point cloud callback. Copies data and enqueues for processing.""" - if self._stop_event.is_set(): - return - try: - packet = packet_ptr.contents if hasattr(packet_ptr, "contents") else packet_ptr - data_type = packet.data_type - - if data_type == LivoxLidarPointDataType.CARTESIAN_HIGH: - xyz, reflectivities, _tags = parse_cartesian_high_points(packet) - elif data_type == LivoxLidarPointDataType.CARTESIAN_LOW: - xyz, reflectivities, _tags = parse_cartesian_low_points(packet) - else: - return # skip spherical for now - - ts_ns = get_packet_timestamp_ns(packet) - frame_cnt = packet.frame_cnt - - self._pc_queue.put_nowait((frame_cnt, ts_ns, xyz, reflectivities)) - except Exception: - logger.debug("Error in point cloud callback", exc_info=True) - - def _on_imu_data( - self, - handle: int, - dev_type: int, - packet_ptr: LivoxLidarEthernetPacket, # type: ignore[override] - client_data: object, - ) -> None: - """SDK IMU callback. Copies data and enqueues.""" - if self._stop_event.is_set(): - return - try: - packet = packet_ptr.contents if hasattr(packet_ptr, "contents") else packet_ptr - ts_ns = get_packet_timestamp_ns(packet) - imu_points = parse_imu_data(packet) - self._imu_queue.put_nowait((ts_ns, imu_points)) - except Exception: - logger.debug("Error in IMU callback", exc_info=True) - - def _on_info_change( - self, - handle: int, - info_ptr: LivoxLidarInfo, # type: ignore[override] - client_data: object, - ) -> None: - """SDK device info change callback. Tracks connected devices.""" - try: - info = info_ptr.contents if hasattr(info_ptr, "contents") else info_ptr - dev_type = LivoxLidarDeviceType(info.dev_type) - sn = info.sn.decode("utf-8", errors="replace").rstrip("\x00") - ip = info.lidar_ip.decode("utf-8", errors="replace").rstrip("\x00") - self._connected_handles[handle] = dev_type - logger.info( - "Livox device connected: handle=%d type=%s sn=%s ip=%s", - handle, - dev_type.name, - sn, - ip, - ) - - # Set to normal work mode - self._sdk_set_work_mode(handle) - - # Enable/disable IMU based on config - if self.config.enable_imu: - self._sdk_enable_imu(handle) - except Exception: - logger.debug("Error in info change callback", exc_info=True) - - def _sdk_set_work_mode(self, handle: int) -> None: - if self._sdk is None: - return - - def _on_work_mode(status: int, handle: int, response: object, client_data: object) -> None: - if status == 0: - logger.info("Work mode set to NORMAL for handle %d", handle) - else: - logger.warning("Failed to set work mode for handle %d: status=%d", handle, status) - - _cb = AsyncControlCallbackType(_on_work_mode) - self._sdk._callbacks[f"work_mode_cb_{handle}"] = _cb - self._sdk.lib.SetLivoxLidarWorkMode(handle, int(LivoxLidarWorkMode.NORMAL), _cb, None) - - def _sdk_enable_imu(self, handle: int) -> None: - if self._sdk is None: - return - - def _on_imu_enable(status: int, handle: int, response: object, client_data: object) -> None: - if status == 0: - logger.info("IMU enabled for handle %d", handle) - else: - logger.warning("Failed to enable IMU for handle %d: status=%d", handle, status) - - _cb = AsyncControlCallbackType(_on_imu_enable) - self._sdk._callbacks[f"imu_enable_cb_{handle}"] = _cb - self._sdk.lib.EnableLivoxLidarImuData(handle, _cb, None) - - # ------------------------------------------------------------------ - # Consumer threads (Python-side, build messages from queued data) - # ------------------------------------------------------------------ - - def _pointcloud_consumer_loop(self) -> None: - """Drain the point cloud queue, accumulate by time window, emit PointCloud2.""" - frame_points: list[np.ndarray] = [] - frame_reflectivities: list[np.ndarray] = [] - frame_timestamp: float | None = None - frame_interval = 1.0 / self.config.frequency if self.config.frequency > 0 else 0.1 - last_emit = time.monotonic() - - while not self._stop_event.is_set(): - try: - _frame_cnt, ts_ns, xyz, reflectivities = self._pc_queue.get(timeout=0.05) - except Empty: - # Check if we should emit on timeout - if frame_points and (time.monotonic() - last_emit) >= frame_interval: - self._emit_pointcloud(frame_points, frame_reflectivities, frame_timestamp) - frame_points = [] - frame_reflectivities = [] - frame_timestamp = None - last_emit = time.monotonic() - continue - - frame_points.append(xyz) - frame_reflectivities.append(reflectivities) - if frame_timestamp is None: - frame_timestamp = ts_ns / 1e9 - - # Emit when time window is full - if (time.monotonic() - last_emit) >= frame_interval: - self._emit_pointcloud(frame_points, frame_reflectivities, frame_timestamp) - frame_points = [] - frame_reflectivities = [] - frame_timestamp = None - last_emit = time.monotonic() - - # Emit any remaining data - if frame_points: - self._emit_pointcloud(frame_points, frame_reflectivities, frame_timestamp) - - def _emit_pointcloud( - self, - frame_points: list[np.ndarray], - frame_reflectivities: list[np.ndarray], - frame_timestamp: float | None, - ) -> None: - if not self._pc_observer or self._stop_event.is_set(): - return - - all_points = np.concatenate(frame_points, axis=0) - all_reflectivities = np.concatenate(frame_reflectivities, axis=0) - - if len(all_points) == 0: - return - - # Build Open3D tensor point cloud - pcd_t = o3d.t.geometry.PointCloud() - pcd_t.point["positions"] = o3c.Tensor(all_points, dtype=o3c.float32) - # Store reflectivity as intensity (normalized to 0-1 range) - pcd_t.point["intensities"] = o3c.Tensor( - all_reflectivities.astype(np.float32).reshape(-1, 1) / 255.0, - dtype=o3c.float32, - ) - - # Optional voxel downsampling - if self.config.voxel_size is not None: - pcd_t = pcd_t.voxel_down_sample(self.config.voxel_size) - - pc2 = PointCloud2( - pointcloud=pcd_t, - frame_id=self._frame_id(self.config.frame_id), - ts=frame_timestamp if frame_timestamp else time.time(), - ) - - try: - self._pc_observer.on_next(pc2) - except Exception: - logger.debug("Error emitting point cloud", exc_info=True) - - def _imu_consumer_loop(self) -> None: - """Drain the IMU queue and emit Imu messages.""" - while not self._stop_event.is_set(): - try: - ts_ns, imu_points = self._imu_queue.get(timeout=0.5) - except Empty: - continue - - if not self._imu_observer or self._stop_event.is_set(): - continue - - ts = ts_ns / 1e9 - for i in range(len(imu_points)): - pt = imu_points[i] - imu_msg = Imu( - angular_velocity=Vector3( - float(pt["gyro_x"]), - float(pt["gyro_y"]), - float(pt["gyro_z"]), - ), - linear_acceleration=Vector3( - float(pt["acc_x"]) * GRAVITY_MS2, - float(pt["acc_y"]) * GRAVITY_MS2, - float(pt["acc_z"]) * GRAVITY_MS2, - ), - frame_id=self._frame_id("imu_link"), - ts=ts, - ) - try: - self._imu_observer.on_next(imu_msg) - except Exception: - logger.debug("Error emitting IMU data", exc_info=True) - - # ------------------------------------------------------------------ - # Public API: Observable streams - # ------------------------------------------------------------------ - - @cache - def pointcloud_stream(self) -> Observable[PointCloud2]: - """Observable stream of PointCloud2 messages (~10 Hz full-frame scans).""" - - def subscribe(observer, scheduler=None): # type: ignore[no-untyped-def] - self._pc_observer = observer - try: - self._start_sdk() - except Exception as e: - observer.on_error(e) - return - - def dispose() -> None: - self._pc_observer = None - self.stop() - - return dispose - - return backpressure(create(subscribe)) - - @cache - def imu_stream(self) -> Observable[Imu]: - """Observable stream of Imu messages (~200 Hz).""" - - def subscribe(observer, scheduler=None): # type: ignore[no-untyped-def] - self._imu_observer = observer - - def dispose() -> None: - self._imu_observer = None - - return dispose - - return backpressure(create(subscribe)) - - # ------------------------------------------------------------------ - # Lifecycle - # ------------------------------------------------------------------ - - def _start_sdk(self) -> None: - """Initialize and start the Livox SDK.""" - if self._sdk is not None: - return # already running - - self._stop_event.clear() - - # Write temp config - self._config_path = self._write_sdk_config() - - # Load and init SDK - self._sdk = LivoxSDK(self.config.sdk_lib_path) - self._sdk.init(self._config_path, self.config.host_ip) - - # Register callbacks (must keep references to prevent GC) - pc_cb = PointCloudCallbackType(self._on_point_cloud) - imu_cb = ImuDataCallbackType(self._on_imu_data) - info_cb = InfoChangeCallbackType(self._on_info_change) - - self._sdk.set_point_cloud_callback(pc_cb) - if self.config.enable_imu: - self._sdk.set_imu_callback(imu_cb) - self._sdk.set_info_change_callback(info_cb) - - # Start SDK background threads - self._sdk.start() - logger.info( - "Livox SDK started (host_ip=%s, lidar_ips=%s)", - self.config.host_ip, - self.config.lidar_ips, - ) - - # Start consumer threads - self._pc_consumer_thread = threading.Thread( - target=self._pointcloud_consumer_loop, daemon=True, name="livox-pc-consumer" - ) - self._pc_consumer_thread.start() - - if self.config.enable_imu: - self._imu_consumer_thread = threading.Thread( - target=self._imu_consumer_loop, daemon=True, name="livox-imu-consumer" - ) - self._imu_consumer_thread.start() - - def stop(self) -> None: - """Stop the SDK and all consumer threads.""" - self._stop_event.set() - - if self._sdk is not None: - self._sdk.uninit() - self._sdk = None - - for thread in [self._pc_consumer_thread, self._imu_consumer_thread]: - if thread is not None and thread.is_alive(): - thread.join(timeout=3.0) - - self._pc_consumer_thread = None - self._imu_consumer_thread = None - self._connected_handles.clear() - - # Cleanup temp config - if self._config_path: - import os - - try: - os.unlink(self._config_path) - except OSError: - pass - self._config_path = None - - logger.info("Livox Mid-360 stopped") diff --git a/dimos/hardware/sensors/lidar/livox/module.py b/dimos/hardware/sensors/lidar/livox/module.py index bdde07225f..dd4d8c2cc5 100644 --- a/dimos/hardware/sensors/lidar/livox/module.py +++ b/dimos/hardware/sensors/lidar/livox/module.py @@ -1,4 +1,4 @@ -# Copyright 2025-2026 Dimensional Inc. +# Copyright 2026 Dimensional Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -12,48 +12,118 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Livox-specific LiDAR module with IMU support.""" +"""Python NativeModule wrapper for the C++ Livox Mid-360 driver. + +Declares the same ports as LivoxLidarModule (pointcloud, imu) but delegates +all real work to the ``mid360_native`` C++ binary, which talks directly to +the Livox SDK2 C API and publishes on LCM. + +Usage:: + + from dimos.hardware.sensors.lidar.livox.module import Mid360CppModule + from dimos.core.blueprints import autoconnect + + autoconnect( + Mid360CppModule.blueprint(host_ip="192.168.1.5"), + SomeConsumer.blueprint(), + ).build().loop() +""" + +from __future__ import annotations from dataclasses import dataclass +from pathlib import Path +from typing import TYPE_CHECKING -from dimos.core import Out, rpc -from dimos.hardware.sensors.lidar.module import LidarModule, LidarModuleConfig -from dimos.msgs.sensor_msgs.Imu import Imu +from dimos.core.native_module import NativeModule, NativeModuleConfig from dimos.spec import perception +if TYPE_CHECKING: + from dimos.core import Out + from dimos.msgs.sensor_msgs.Imu import Imu + from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + +_DEFAULT_EXECUTABLE = str(Path(__file__).parent / "cpp" / "build" / "mid360_native") -@dataclass -class LivoxLidarModuleConfig(LidarModuleConfig): + +@dataclass(kw_only=True) +class Mid360CppConfig(NativeModuleConfig): + """Config for the C++ Mid-360 native module.""" + + executable: str = _DEFAULT_EXECUTABLE + host_ip: str = "192.168.1.5" + lidar_ip: str = "192.168.1.155" + frequency: float = 10.0 enable_imu: bool = True + frame_id: str = "lidar_link" + imu_frame_id: str = "imu_link" + + # SDK port configuration (match defaults in LivoxMid360Config) + cmd_data_port: int = 56100 + push_msg_port: int = 56200 + point_data_port: int = 56300 + imu_data_port: int = 56400 + log_data_port: int = 56500 + host_cmd_data_port: int = 56101 + host_push_msg_port: int = 56201 + host_point_data_port: int = 56301 + host_imu_data_port: int = 56401 + host_log_data_port: int = 56501 -class LivoxLidarModule(LidarModule, perception.IMU): - """Livox LiDAR module — pointcloud + IMU. +class Mid360CppModule(NativeModule, perception.Lidar, perception.IMU): + """Livox Mid-360 LiDAR module backed by a native C++ binary. - Extends LidarModule with IMU stream support for sensors like the Mid-360. + Ports: + pointcloud (Out[PointCloud2]): Point cloud frames at configured frequency. + imu (Out[Imu]): IMU data at ~200 Hz (if enabled). """ + default_config: type[Mid360CppConfig] = Mid360CppConfig # type: ignore[assignment] + pointcloud: Out[PointCloud2] imu: Out[Imu] - config: LivoxLidarModuleConfig # type: ignore[assignment] - default_config = LivoxLidarModuleConfig # type: ignore[assignment] - - @rpc - def start(self) -> None: - super().start() - - if self.config.enable_imu: - imu_stream = self.hardware.imu_stream() - if imu_stream is not None: - self._disposables.add( - imu_stream.subscribe(lambda imu_msg: self.imu.publish(imu_msg)), - ) + def _build_extra_args(self) -> list[str]: + """Pass hardware config to the C++ binary as CLI args.""" + cfg: Mid360CppConfig = self.config # type: ignore[assignment] + return [ + "--host_ip", + cfg.host_ip, + "--lidar_ip", + cfg.lidar_ip, + "--frequency", + str(cfg.frequency), + "--frame_id", + cfg.frame_id, + "--imu_frame_id", + cfg.imu_frame_id, + "--cmd_data_port", + str(cfg.cmd_data_port), + "--push_msg_port", + str(cfg.push_msg_port), + "--point_data_port", + str(cfg.point_data_port), + "--imu_data_port", + str(cfg.imu_data_port), + "--log_data_port", + str(cfg.log_data_port), + "--host_cmd_data_port", + str(cfg.host_cmd_data_port), + "--host_push_msg_port", + str(cfg.host_push_msg_port), + "--host_point_data_port", + str(cfg.host_point_data_port), + "--host_imu_data_port", + str(cfg.host_imu_data_port), + "--host_log_data_port", + str(cfg.host_log_data_port), + ] -livox_lidar_module = LivoxLidarModule.blueprint +mid360_cpp_module = Mid360CppModule.blueprint __all__ = [ - "LivoxLidarModule", - "LivoxLidarModuleConfig", - "livox_lidar_module", + "Mid360CppConfig", + "Mid360CppModule", + "mid360_cpp_module", ] diff --git a/dimos/hardware/sensors/lidar/livox/sdk.py b/dimos/hardware/sensors/lidar/livox/sdk.py deleted file mode 100644 index eb5fbcbccb..0000000000 --- a/dimos/hardware/sensors/lidar/livox/sdk.py +++ /dev/null @@ -1,534 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""ctypes bindings for Livox SDK2 (liblivox_lidar_sdk_shared.so). - -Provides Python access to the Livox LiDAR SDK2 C API for device -communication, point cloud streaming, and IMU data. - -The SDK .so must be pre-built from https://github.com/Livox-SDK/Livox-SDK2. -Set LIVOX_SDK2_LIB_PATH env var or install to /usr/local/lib. -""" - -from __future__ import annotations - -import ctypes -import ctypes.util -from enum import IntEnum -import logging -import os -from pathlib import Path -from typing import Any - -import numpy as np - -logger = logging.getLogger(__name__) - - -# --------------------------------------------------------------------------- -# Enums -# --------------------------------------------------------------------------- - - -class LivoxLidarDeviceType(IntEnum): - HUB = 0 - MID40 = 1 - TELE = 2 - HORIZON = 3 - MID70 = 6 - AVIA = 7 - MID360 = 9 - INDUSTRIAL_HAP = 10 - HAP = 15 - PA = 16 - - -class LivoxLidarPointDataType(IntEnum): - IMU = 0x00 - CARTESIAN_HIGH = 0x01 - CARTESIAN_LOW = 0x02 - SPHERICAL = 0x03 - - -class LivoxLidarWorkMode(IntEnum): - NORMAL = 0x01 - WAKE_UP = 0x02 - SLEEP = 0x03 - ERROR = 0x04 - POWER_ON_SELF_TEST = 0x05 - MOTOR_STARTING = 0x06 - MOTOR_STOPPING = 0x07 - UPGRADE = 0x08 - - -class LivoxLidarScanPattern(IntEnum): - NON_REPETITIVE = 0x00 - REPETITIVE = 0x01 - REPETITIVE_LOW_FRAME_RATE = 0x02 - - -class LivoxLidarStatus(IntEnum): - SEND_FAILED = -9 - HANDLER_IMPL_NOT_EXIST = -8 - INVALID_HANDLE = -7 - CHANNEL_NOT_EXIST = -6 - NOT_ENOUGH_MEMORY = -5 - TIMEOUT = -4 - NOT_SUPPORTED = -3 - NOT_CONNECTED = -2 - FAILURE = -1 - SUCCESS = 0 - - -# --------------------------------------------------------------------------- -# Packed C structures (all #pragma pack(1) in the SDK header) -# --------------------------------------------------------------------------- - - -class LivoxLidarCartesianHighRawPoint(ctypes.LittleEndianStructure): - """High-resolution cartesian point (14 bytes). Coordinates in mm.""" - - _pack_ = 1 - _fields_ = [ - ("x", ctypes.c_int32), - ("y", ctypes.c_int32), - ("z", ctypes.c_int32), - ("reflectivity", ctypes.c_uint8), - ("tag", ctypes.c_uint8), - ] - - -class LivoxLidarCartesianLowRawPoint(ctypes.LittleEndianStructure): - """Low-resolution cartesian point (8 bytes). Coordinates in cm.""" - - _pack_ = 1 - _fields_ = [ - ("x", ctypes.c_int16), - ("y", ctypes.c_int16), - ("z", ctypes.c_int16), - ("reflectivity", ctypes.c_uint8), - ("tag", ctypes.c_uint8), - ] - - -class LivoxLidarSpherPoint(ctypes.LittleEndianStructure): - """Spherical coordinate point (10 bytes).""" - - _pack_ = 1 - _fields_ = [ - ("depth", ctypes.c_uint32), - ("theta", ctypes.c_uint16), - ("phi", ctypes.c_uint16), - ("reflectivity", ctypes.c_uint8), - ("tag", ctypes.c_uint8), - ] - - -class LivoxLidarImuRawPoint(ctypes.LittleEndianStructure): - """IMU data point (24 bytes). Gyro in rad/s, accel in g.""" - - _pack_ = 1 - _fields_ = [ - ("gyro_x", ctypes.c_float), - ("gyro_y", ctypes.c_float), - ("gyro_z", ctypes.c_float), - ("acc_x", ctypes.c_float), - ("acc_y", ctypes.c_float), - ("acc_z", ctypes.c_float), - ] - - -class LivoxLidarEthernetPacket(ctypes.LittleEndianStructure): - """Point cloud / IMU ethernet packet header (32 bytes + variable data).""" - - _pack_ = 1 - _fields_ = [ - ("version", ctypes.c_uint8), - ("length", ctypes.c_uint16), - ("time_interval", ctypes.c_uint16), # unit: 0.1 us - ("dot_num", ctypes.c_uint16), - ("udp_cnt", ctypes.c_uint16), - ("frame_cnt", ctypes.c_uint8), - ("data_type", ctypes.c_uint8), - ("time_type", ctypes.c_uint8), - ("rsvd", ctypes.c_uint8 * 12), - ("crc32", ctypes.c_uint32), - ("timestamp", ctypes.c_uint8 * 8), - ("data", ctypes.c_uint8 * 1), # variable-length payload - ] - - -class LivoxLidarInfo(ctypes.LittleEndianStructure): - """Device info received on connection.""" - - _pack_ = 1 - _fields_ = [ - ("dev_type", ctypes.c_uint8), - ("sn", ctypes.c_char * 16), - ("lidar_ip", ctypes.c_char * 16), - ] - - -class LivoxLidarAsyncControlResponse(ctypes.LittleEndianStructure): - _pack_ = 1 - _fields_ = [ - ("ret_code", ctypes.c_uint8), - ("error_key", ctypes.c_uint16), - ] - - -# --------------------------------------------------------------------------- -# numpy dtypes for efficient batch parsing of point data -# --------------------------------------------------------------------------- - -CART_HIGH_DTYPE = np.dtype( - [("x", " tuple[np.ndarray, np.ndarray, np.ndarray]: - """Parse high-res cartesian points from a packet. - - Returns: - (xyz_meters, reflectivities, tags) where xyz is (N,3) float32 in meters. - """ - dot_num = packet.dot_num - if dot_num == 0: - empty = np.empty((0, 3), dtype=np.float32) - return empty, np.empty(0, dtype=np.uint8), np.empty(0, dtype=np.uint8) - - data_ptr = ctypes.addressof(packet) + LivoxLidarEthernetPacket.data.offset - buf = (ctypes.c_uint8 * (dot_num * CART_HIGH_DTYPE.itemsize)).from_address(data_ptr) - points = np.frombuffer(buf, dtype=CART_HIGH_DTYPE, count=dot_num) - - xyz = np.column_stack([points["x"], points["y"], points["z"]]).astype(np.float32) / 1000.0 - return xyz, points["reflectivity"].copy(), points["tag"].copy() - - -def parse_cartesian_low_points( - packet: LivoxLidarEthernetPacket, -) -> tuple[np.ndarray, np.ndarray, np.ndarray]: - """Parse low-res cartesian points. Returns xyz in meters.""" - dot_num = packet.dot_num - if dot_num == 0: - empty = np.empty((0, 3), dtype=np.float32) - return empty, np.empty(0, dtype=np.uint8), np.empty(0, dtype=np.uint8) - - data_ptr = ctypes.addressof(packet) + LivoxLidarEthernetPacket.data.offset - buf = (ctypes.c_uint8 * (dot_num * CART_LOW_DTYPE.itemsize)).from_address(data_ptr) - points = np.frombuffer(buf, dtype=CART_LOW_DTYPE, count=dot_num) - - xyz = np.column_stack([points["x"], points["y"], points["z"]]).astype(np.float32) / 100.0 - return xyz, points["reflectivity"].copy(), points["tag"].copy() - - -def parse_imu_data(packet: LivoxLidarEthernetPacket) -> np.ndarray: - """Parse IMU data from a packet. Returns structured array with gyro/accel fields.""" - dot_num = packet.dot_num - if dot_num == 0: - return np.empty(0, dtype=IMU_DTYPE) - - data_ptr = ctypes.addressof(packet) + LivoxLidarEthernetPacket.data.offset - buf = (ctypes.c_uint8 * (dot_num * IMU_DTYPE.itemsize)).from_address(data_ptr) - return np.frombuffer(buf, dtype=IMU_DTYPE, count=dot_num).copy() - - -def get_packet_timestamp_ns(packet: LivoxLidarEthernetPacket) -> int: - """Extract the 64-bit nanosecond timestamp from a packet.""" - return int.from_bytes(bytes(packet.timestamp), byteorder="little") - - -# --------------------------------------------------------------------------- -# SDK library loader -# --------------------------------------------------------------------------- - -_SDK_LIB_NAMES = [ - "livox_lidar_sdk_shared", - "liblivox_lidar_sdk_shared.so", - "liblivox_lidar_sdk_shared.so.2", -] - -_SDK_SEARCH_PATHS = [ - "/usr/local/lib", - "/usr/lib", - "/usr/lib/x86_64-linux-gnu", - "/usr/lib/aarch64-linux-gnu", -] - - -def _find_sdk_library(lib_path: str | None = None) -> str: - """Find the Livox SDK2 shared library. - - Search order: - 1. Explicit lib_path argument - 2. LIVOX_SDK2_LIB_PATH environment variable - 3. ctypes.util.find_library - 4. Well-known system paths - """ - if lib_path and Path(lib_path).exists(): - return lib_path - - env_path = os.environ.get("LIVOX_SDK2_LIB_PATH") - if env_path and Path(env_path).exists(): - return env_path - - for name in _SDK_LIB_NAMES: - found = ctypes.util.find_library(name) - if found: - return found - - for search_dir in _SDK_SEARCH_PATHS: - for name in _SDK_LIB_NAMES: - candidate = Path(search_dir) / name - if candidate.exists(): - return str(candidate) - - raise RuntimeError( - "Livox SDK2 shared library not found. " - "Build from https://github.com/Livox-SDK/Livox-SDK2 and install, " - "or set LIVOX_SDK2_LIB_PATH to the .so file path." - ) - - -def load_sdk(lib_path: str | None = None) -> ctypes.CDLL: - """Load the Livox SDK2 shared library and set up function signatures.""" - path = _find_sdk_library(lib_path) - lib = ctypes.CDLL(path) - logger.info("Loaded Livox SDK2 from %s", path) - - # --- SDK lifecycle --- - lib.LivoxLidarSdkInit.argtypes = [ctypes.c_char_p, ctypes.c_char_p, ctypes.c_void_p] - lib.LivoxLidarSdkInit.restype = ctypes.c_bool - - lib.LivoxLidarSdkStart.argtypes = [] - lib.LivoxLidarSdkStart.restype = ctypes.c_bool - - lib.LivoxLidarSdkUninit.argtypes = [] - lib.LivoxLidarSdkUninit.restype = None - - # --- Callbacks --- - lib.SetLivoxLidarPointCloudCallBack.argtypes = [PointCloudCallbackType, ctypes.c_void_p] - lib.SetLivoxLidarPointCloudCallBack.restype = None - - lib.SetLivoxLidarImuDataCallback.argtypes = [ImuDataCallbackType, ctypes.c_void_p] - lib.SetLivoxLidarImuDataCallback.restype = None - - lib.SetLivoxLidarInfoChangeCallback.argtypes = [InfoChangeCallbackType, ctypes.c_void_p] - lib.SetLivoxLidarInfoChangeCallback.restype = None - - lib.SetLivoxLidarInfoCallback.argtypes = [InfoCallbackType, ctypes.c_void_p] - lib.SetLivoxLidarInfoCallback.restype = None - - # --- Device configuration --- - lib.SetLivoxLidarWorkMode.argtypes = [ - ctypes.c_uint32, - ctypes.c_int32, - AsyncControlCallbackType, - ctypes.c_void_p, - ] - lib.SetLivoxLidarWorkMode.restype = ctypes.c_int32 - - lib.EnableLivoxLidarImuData.argtypes = [ - ctypes.c_uint32, - AsyncControlCallbackType, - ctypes.c_void_p, - ] - lib.EnableLivoxLidarImuData.restype = ctypes.c_int32 - - lib.DisableLivoxLidarImuData.argtypes = [ - ctypes.c_uint32, - AsyncControlCallbackType, - ctypes.c_void_p, - ] - lib.DisableLivoxLidarImuData.restype = ctypes.c_int32 - - lib.SetLivoxLidarPclDataType.argtypes = [ - ctypes.c_uint32, - ctypes.c_int32, - AsyncControlCallbackType, - ctypes.c_void_p, - ] - lib.SetLivoxLidarPclDataType.restype = ctypes.c_int32 - - lib.SetLivoxLidarScanPattern.argtypes = [ - ctypes.c_uint32, - ctypes.c_int32, - AsyncControlCallbackType, - ctypes.c_void_p, - ] - lib.SetLivoxLidarScanPattern.restype = ctypes.c_int32 - - # --- Console logging --- - lib.DisableLivoxSdkConsoleLogger.argtypes = [] - lib.DisableLivoxSdkConsoleLogger.restype = None - - return lib - - -class LivoxSDK: - """Convenience wrapper managing SDK lifecycle and callback registration. - - Prevents garbage collection of ctypes callback references (which would - cause segfaults when the SDK tries to call them from C threads). - """ - - def __init__(self, lib_path: str | None = None) -> None: - self._lib = load_sdk(lib_path) - self._initialized = False - # prevent GC of callback C function pointers - self._callbacks: dict[str, Any] = {} - - @property - def lib(self) -> ctypes.CDLL: - return self._lib - - def init(self, config_path: str, host_ip: str, quiet: bool = True) -> None: - """Initialize the SDK with a JSON config file and host IP.""" - if self._initialized: - raise RuntimeError("SDK already initialized. Call uninit() first.") - if quiet: - self._lib.DisableLivoxSdkConsoleLogger() - ok = self._lib.LivoxLidarSdkInit( - config_path.encode(), - host_ip.encode(), - None, - ) - if not ok: - raise RuntimeError( - f"LivoxLidarSdkInit failed (config={config_path}, host_ip={host_ip})" - ) - self._initialized = True - - def start(self) -> None: - """Start the SDK (begins device communication on background threads).""" - if not self._initialized: - raise RuntimeError("SDK not initialized. Call init() first.") - ok = self._lib.LivoxLidarSdkStart() - if not ok: - raise RuntimeError("LivoxLidarSdkStart failed") - - def uninit(self) -> None: - """Shut down the SDK and release resources.""" - if self._initialized: - self._lib.LivoxLidarSdkUninit() - self._initialized = False - self._callbacks.clear() - - def set_point_cloud_callback(self, callback: Any) -> None: - self._callbacks["pointcloud"] = callback # prevent GC - self._lib.SetLivoxLidarPointCloudCallBack(callback, None) - - def set_imu_callback(self, callback: Any) -> None: - self._callbacks["imu"] = callback - self._lib.SetLivoxLidarImuDataCallback(callback, None) - - def set_info_change_callback(self, callback: Any) -> None: - self._callbacks["info_change"] = callback - self._lib.SetLivoxLidarInfoChangeCallback(callback, None) - - def set_info_callback(self, callback: Any) -> None: - self._callbacks["info"] = callback - self._lib.SetLivoxLidarInfoCallback(callback, None) - - def set_work_mode(self, handle: int, mode: LivoxLidarWorkMode, callback: Any = None) -> int: - cb = callback or AsyncControlCallbackType(0) # type: ignore[arg-type] - if callback: - self._callbacks[f"work_mode_{handle}"] = callback - return self._lib.SetLivoxLidarWorkMode(handle, int(mode), cb, None) # type: ignore[no-any-return] - - def enable_imu_data(self, handle: int, callback: Any = None) -> int: - cb = callback or AsyncControlCallbackType(0) # type: ignore[arg-type] - if callback: - self._callbacks[f"imu_enable_{handle}"] = callback - return self._lib.EnableLivoxLidarImuData(handle, cb, None) # type: ignore[no-any-return] - - def disable_imu_data(self, handle: int, callback: Any = None) -> int: - cb = callback or AsyncControlCallbackType(0) # type: ignore[arg-type] - if callback: - self._callbacks[f"imu_disable_{handle}"] = callback - return self._lib.DisableLivoxLidarImuData(handle, cb, None) # type: ignore[no-any-return] diff --git a/dimos/hardware/sensors/lidar/livox/test_spec_compliance.py b/dimos/hardware/sensors/lidar/livox/test_spec_compliance.py index 79abb58dbd..92bf0e7c6e 100644 --- a/dimos/hardware/sensors/lidar/livox/test_spec_compliance.py +++ b/dimos/hardware/sensors/lidar/livox/test_spec_compliance.py @@ -16,14 +16,14 @@ from __future__ import annotations -from dimos.hardware.sensors.lidar.livox.module import LivoxLidarModule +from dimos.hardware.sensors.lidar.livox.module import Mid360CppModule from dimos.spec import perception from dimos.spec.utils import assert_implements_protocol def test_livox_module_implements_lidar_spec() -> None: - assert_implements_protocol(LivoxLidarModule, perception.Lidar) + assert_implements_protocol(Mid360CppModule, perception.Lidar) def test_livox_module_implements_imu_spec() -> None: - assert_implements_protocol(LivoxLidarModule, perception.IMU) + assert_implements_protocol(Mid360CppModule, perception.IMU) From e24ea8e561be959ac84511667f627b13dd05a72f Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Thu, 12 Feb 2026 20:06:57 +0800 Subject: [PATCH 09/15] correct config defaults --- dimos/hardware/sensors/lidar/livox/module.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/dimos/hardware/sensors/lidar/livox/module.py b/dimos/hardware/sensors/lidar/livox/module.py index dd4d8c2cc5..d6c4eb4744 100644 --- a/dimos/hardware/sensors/lidar/livox/module.py +++ b/dimos/hardware/sensors/lidar/livox/module.py @@ -79,7 +79,9 @@ class Mid360CppModule(NativeModule, perception.Lidar, perception.IMU): imu (Out[Imu]): IMU data at ~200 Hz (if enabled). """ - default_config: type[Mid360CppConfig] = Mid360CppConfig # type: ignore[assignment] + config: Mid360CppConfig + default_config = Mid360CppConfig + pointcloud: Out[PointCloud2] imu: Out[Imu] From 71ee68d369c8d4e4ece2096dbf3fdf4feed6f69a Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Thu, 12 Feb 2026 21:33:08 +0800 Subject: [PATCH 10/15] Add FAST-LIO2 native module with Livox Mid-360 SLAM integration Integrates FAST-LIO-NON-ROS directly with Livox SDK2 as a dimos NativeModule. The C++ binary feeds live LiDAR/IMU data into FAST-LIO's EKF-LOAM SLAM and publishes aggregated world-frame point clouds and odometry over LCM. Includes rate-limited output (pointcloud_freq, odom_freq), Odometry.to_rerun() for visualization, and nix flake deps. --- .../sensors/lidar/fastlio2/__init__.py | 0 .../sensors/lidar/fastlio2/cpp/CMakeLists.txt | 91 +++ .../lidar/fastlio2/cpp/config/mid360.json | 38 ++ .../fastlio2/cpp/dimos_native_module.hpp | 64 ++ .../sensors/lidar/fastlio2/cpp/main.cpp | 578 ++++++++++++++++++ .../lidar/fastlio2/fastlio_blueprints.py | 36 ++ .../hardware/sensors/lidar/fastlio2/module.py | 191 ++++++ .../lidar/fastlio2/test_spec_compliance.py | 29 + .../sensors/lidar/test_spec_compliance.py | 34 ++ dimos/msgs/nav_msgs/Odometry.py | 19 + dimos/robot/all_blueprints.py | 6 +- flake.nix | 5 + 12 files changed, 1089 insertions(+), 2 deletions(-) create mode 100644 dimos/hardware/sensors/lidar/fastlio2/__init__.py create mode 100644 dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt create mode 100644 dimos/hardware/sensors/lidar/fastlio2/cpp/config/mid360.json create mode 100644 dimos/hardware/sensors/lidar/fastlio2/cpp/dimos_native_module.hpp create mode 100644 dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp create mode 100644 dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py create mode 100644 dimos/hardware/sensors/lidar/fastlio2/module.py create mode 100644 dimos/hardware/sensors/lidar/fastlio2/test_spec_compliance.py create mode 100644 dimos/hardware/sensors/lidar/test_spec_compliance.py diff --git a/dimos/hardware/sensors/lidar/fastlio2/__init__.py b/dimos/hardware/sensors/lidar/fastlio2/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt b/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt new file mode 100644 index 0000000000..301c257ddd --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt @@ -0,0 +1,91 @@ +cmake_minimum_required(VERSION 3.14) +project(fastlio2_native CXX) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3") + +# FAST-LIO-NON-ROS source directory (local reference) +set(FASTLIO_DIR "$ENV{HOME}/coding/FAST-LIO-NON-ROS") + +# OpenMP for parallel processing +find_package(OpenMP QUIET) +if(OpenMP_CXX_FOUND) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") +endif() + +# MP defines (same logic as FAST-LIO) +message("CPU architecture: ${CMAKE_SYSTEM_PROCESSOR}") +if(CMAKE_SYSTEM_PROCESSOR MATCHES "(x86)|(X86)|(amd64)|(AMD64)") + include(ProcessorCount) + ProcessorCount(N) + if(N GREATER 4) + add_definitions(-DMP_EN -DMP_PROC_NUM=3) + elseif(N GREATER 3) + add_definitions(-DMP_EN -DMP_PROC_NUM=2) + else() + add_definitions(-DMP_PROC_NUM=1) + endif() +else() + add_definitions(-DMP_PROC_NUM=1) +endif() + +# Fetch dimos-lcm for C++ message headers +include(FetchContent) +FetchContent_Declare(dimos_lcm + GIT_REPOSITORY https://github.com/dimensionalOS/dimos-lcm.git + GIT_TAG main + GIT_SHALLOW TRUE +) +FetchContent_MakeAvailable(dimos_lcm) + +# LCM +find_package(PkgConfig REQUIRED) +pkg_check_modules(LCM REQUIRED lcm) + +# Eigen3 +find_package(Eigen3 REQUIRED) + +# PCL +find_package(PCL 1.8 REQUIRED) + +# nlohmann_json (FAST-LIO config parsing) +find_package(nlohmann_json 3.2.0 REQUIRED) + +# Livox SDK2 (installed to /usr/local) +find_library(LIVOX_SDK livox_lidar_sdk_shared HINTS /usr/local/lib) +if(NOT LIVOX_SDK) + message(FATAL_ERROR "Livox SDK2 not found. Install from https://github.com/Livox-SDK/Livox-SDK2") +endif() + +add_executable(fastlio2_native + main.cpp + ${FASTLIO_DIR}/src/preprocess.cpp + ${FASTLIO_DIR}/include/ikd-Tree/ikd_Tree.cpp +) + +target_include_directories(fastlio2_native PRIVATE + ${FASTLIO_DIR}/include + ${FASTLIO_DIR}/src + ${dimos_lcm_SOURCE_DIR}/generated/cpp_lcm_msgs + ${LCM_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} + ${PCL_INCLUDE_DIRS} + ${CMAKE_CURRENT_SOURCE_DIR} + /usr/local/include # Livox SDK2 headers +) + +target_link_libraries(fastlio2_native PRIVATE + ${LCM_LIBRARIES} + ${LIVOX_SDK} + ${PCL_LIBRARIES} + nlohmann_json::nlohmann_json +) + +if(OpenMP_CXX_FOUND) + target_link_libraries(fastlio2_native PRIVATE OpenMP::OpenMP_CXX) +endif() + +target_link_directories(fastlio2_native PRIVATE + ${LCM_LIBRARY_DIRS} +) diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/config/mid360.json b/dimos/hardware/sensors/lidar/fastlio2/cpp/config/mid360.json new file mode 100644 index 0000000000..ff6cc6dbf6 --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/config/mid360.json @@ -0,0 +1,38 @@ +{ + "common": { + "time_sync_en": false, + "time_offset_lidar_to_imu": 0.0, + "msr_freq": 50.0, + "main_freq": 5000.0 + }, + "preprocess": { + "lidar_type": 1, + "scan_line": 1, + "blind": 1 + }, + "mapping": { + "acc_cov": 0.1, + "gyr_cov": 0.1, + "b_acc_cov": 0.0001, + "b_gyr_cov": 0.0001, + "fov_degree": 360, + "det_range": 100.0, + "extrinsic_est_en": true, + "extrinsic_T": [ + 0.04165, + 0.02326, + -0.0284 + ], + "extrinsic_R": [ + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 1.0 + ] + } +} diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/dimos_native_module.hpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/dimos_native_module.hpp new file mode 100644 index 0000000000..73ee618c34 --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/dimos_native_module.hpp @@ -0,0 +1,64 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 +// +// Lightweight header-only helper for dimos NativeModule C++ binaries. +// Parses -- CLI args passed by the Python NativeModule wrapper. + +#pragma once + +#include +#include +#include + +namespace dimos { + +class NativeModule { +public: + NativeModule(int argc, char** argv) { + for (int i = 1; i < argc; ++i) { + std::string arg(argv[i]); + if (arg.size() > 2 && arg[0] == '-' && arg[1] == '-' && i + 1 < argc) { + args_[arg.substr(2)] = argv[++i]; + } + } + } + + /// Get the full LCM channel string for a declared port. + /// Format is "#", e.g. "/pointcloud#sensor_msgs.PointCloud2". + /// This is the exact channel name used by Python LCMTransport subscribers. + const std::string& topic(const std::string& port) const { + auto it = args_.find(port); + if (it == args_.end()) { + throw std::runtime_error("NativeModule: no topic for port '" + port + "'"); + } + return it->second; + } + + /// Get a string arg value, or a default if not present. + std::string arg(const std::string& key, const std::string& default_val = "") const { + auto it = args_.find(key); + return it != args_.end() ? it->second : default_val; + } + + /// Get a float arg value, or a default if not present. + float arg_float(const std::string& key, float default_val = 0.0f) const { + auto it = args_.find(key); + return it != args_.end() ? std::stof(it->second) : default_val; + } + + /// Get an int arg value, or a default if not present. + int arg_int(const std::string& key, int default_val = 0) const { + auto it = args_.find(key); + return it != args_.end() ? std::stoi(it->second) : default_val; + } + + /// Check if a port/arg was provided. + bool has(const std::string& key) const { + return args_.count(key) > 0; + } + +private: + std::map args_; +}; + +} // namespace dimos diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp new file mode 100644 index 0000000000..d931ba27b4 --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp @@ -0,0 +1,578 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 +// +// FAST-LIO2 + Livox Mid-360 native module for dimos NativeModule framework. +// +// Binds Livox SDK2 directly into FAST-LIO-NON-ROS: SDK callbacks feed +// CustomMsg/Imu to FastLio, which performs EKF-LOAM SLAM. Registered +// (world-frame) point clouds and odometry are published on LCM. +// +// Usage: +// ./fastlio2_native \ +// --lidar '/lidar#sensor_msgs.PointCloud2' \ +// --odometry '/odometry#nav_msgs.Odometry' \ +// --config_path /path/to/mid360.json \ +// --host_ip 192.168.1.5 --lidar_ip 192.168.1.155 + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "dimos_native_module.hpp" + +// dimos LCM message headers +#include "geometry_msgs/Quaternion.hpp" +#include "geometry_msgs/Vector3.hpp" +#include "nav_msgs/Odometry.hpp" +#include "sensor_msgs/Imu.hpp" +#include "sensor_msgs/PointCloud2.hpp" +#include "sensor_msgs/PointField.hpp" +#include "std_msgs/Header.hpp" +#include "std_msgs/Time.hpp" + +// FAST-LIO (header-only core, compiled sources linked via CMake) +#include "fast_lio.hpp" + +// Gravity constant for converting accelerometer data from g to m/s^2 +static constexpr double GRAVITY_MS2 = 9.80665; + +// Livox data_type values +static constexpr uint8_t DATA_TYPE_IMU = 0x00; +static constexpr uint8_t DATA_TYPE_CARTESIAN_HIGH = 0x01; +static constexpr uint8_t DATA_TYPE_CARTESIAN_LOW = 0x02; + +// --------------------------------------------------------------------------- +// Global state +// --------------------------------------------------------------------------- + +static std::atomic g_running{true}; +static lcm::LCM* g_lcm = nullptr; +static FastLio* g_fastlio = nullptr; + +static std::string g_lidar_topic; +static std::string g_odometry_topic; +static std::string g_frame_id = "map"; +static std::string g_child_frame_id = "body"; +static float g_frequency = 10.0f; + +// Frame accumulator (Livox SDK raw → CustomMsg) +static std::mutex g_pc_mutex; +static std::vector g_accumulated_points; +static uint64_t g_frame_start_ns = 0; +static bool g_frame_has_timestamp = false; + +// Track whether FastLio has produced a valid result this cycle +static std::atomic g_has_new_result{false}; + +// --------------------------------------------------------------------------- +// Helpers +// --------------------------------------------------------------------------- + +static std_msgs::Time time_from_seconds(double t) { + std_msgs::Time ts; + ts.sec = static_cast(t); + ts.nsec = static_cast((t - ts.sec) * 1e9); + return ts; +} + +static uint64_t get_timestamp_ns(const LivoxLidarEthernetPacket* pkt) { + uint64_t ns = 0; + std::memcpy(&ns, pkt->timestamp, sizeof(uint64_t)); + return ns; +} + +static std_msgs::Header make_header(const std::string& frame_id, double ts) { + static std::atomic seq{0}; + std_msgs::Header h; + h.seq = seq.fetch_add(1, std::memory_order_relaxed); + h.stamp = time_from_seconds(ts); + h.frame_id = frame_id; + return h; +} + +// --------------------------------------------------------------------------- +// Publish lidar (world-frame point cloud) +// --------------------------------------------------------------------------- + +static void publish_lidar(PointCloudXYZI::Ptr cloud, double timestamp) { + if (!g_lcm || !cloud || cloud->empty()) return; + + int num_points = static_cast(cloud->size()); + + sensor_msgs::PointCloud2 pc; + pc.header = make_header(g_frame_id, timestamp); + pc.height = 1; + pc.width = num_points; + pc.is_bigendian = 0; + pc.is_dense = 1; + + // Fields: x, y, z, intensity (float32 each) + pc.fields_length = 4; + pc.fields.resize(4); + + auto make_field = [](const std::string& name, int32_t offset) { + sensor_msgs::PointField f; + f.name = name; + f.offset = offset; + f.datatype = sensor_msgs::PointField::FLOAT32; + f.count = 1; + return f; + }; + + pc.fields[0] = make_field("x", 0); + pc.fields[1] = make_field("y", 4); + pc.fields[2] = make_field("z", 8); + pc.fields[3] = make_field("intensity", 12); + + pc.point_step = 16; + pc.row_step = pc.point_step * num_points; + + pc.data_length = pc.row_step; + pc.data.resize(pc.data_length); + + for (int i = 0; i < num_points; ++i) { + float* dst = reinterpret_cast(pc.data.data() + i * 16); + dst[0] = cloud->points[i].x; + dst[1] = cloud->points[i].y; + dst[2] = cloud->points[i].z; + dst[3] = cloud->points[i].intensity; + } + + g_lcm->publish(g_lidar_topic, &pc); +} + +// --------------------------------------------------------------------------- +// Publish odometry +// --------------------------------------------------------------------------- + +static void publish_odometry(const custom_messages::Odometry& odom, double timestamp) { + if (!g_lcm) return; + + nav_msgs::Odometry msg; + msg.header = make_header(g_frame_id, timestamp); + msg.child_frame_id = g_child_frame_id; + + // Pose + msg.pose.pose.position.x = odom.pose.pose.position.x; + msg.pose.pose.position.y = odom.pose.pose.position.y; + msg.pose.pose.position.z = odom.pose.pose.position.z; + msg.pose.pose.orientation.x = odom.pose.pose.orientation.x; + msg.pose.pose.orientation.y = odom.pose.pose.orientation.y; + msg.pose.pose.orientation.z = odom.pose.pose.orientation.z; + msg.pose.pose.orientation.w = odom.pose.pose.orientation.w; + + // Covariance (fixed-size double[36]) + for (int i = 0; i < 36; ++i) { + msg.pose.covariance[i] = odom.pose.covariance[i]; + } + + // Twist (zero — FAST-LIO doesn't output velocity directly) + msg.twist.twist.linear.x = 0; + msg.twist.twist.linear.y = 0; + msg.twist.twist.linear.z = 0; + msg.twist.twist.angular.x = 0; + msg.twist.twist.angular.y = 0; + msg.twist.twist.angular.z = 0; + std::memset(msg.twist.covariance, 0, sizeof(msg.twist.covariance)); + + g_lcm->publish(g_odometry_topic, &msg); +} + +// --------------------------------------------------------------------------- +// Livox SDK callbacks +// --------------------------------------------------------------------------- + +static void on_point_cloud(const uint32_t /*handle*/, const uint8_t /*dev_type*/, + LivoxLidarEthernetPacket* data, void* /*client_data*/) { + if (!g_running.load() || data == nullptr) return; + + uint64_t ts_ns = get_timestamp_ns(data); + uint16_t dot_num = data->dot_num; + + std::lock_guard lock(g_pc_mutex); + + if (!g_frame_has_timestamp) { + g_frame_start_ns = ts_ns; + g_frame_has_timestamp = true; + } + + if (data->data_type == DATA_TYPE_CARTESIAN_HIGH) { + auto* pts = reinterpret_cast(data->data); + for (uint16_t i = 0; i < dot_num; ++i) { + custom_messages::CustomPoint cp; + cp.x = static_cast(pts[i].x) / 1000.0; // mm → m + cp.y = static_cast(pts[i].y) / 1000.0; + cp.z = static_cast(pts[i].z) / 1000.0; + cp.reflectivity = pts[i].reflectivity; + cp.tag = pts[i].tag; + cp.line = 0; // Mid-360: non-repetitive, single "line" + cp.offset_time = static_cast(ts_ns - g_frame_start_ns); + g_accumulated_points.push_back(cp); + } + } else if (data->data_type == DATA_TYPE_CARTESIAN_LOW) { + auto* pts = reinterpret_cast(data->data); + for (uint16_t i = 0; i < dot_num; ++i) { + custom_messages::CustomPoint cp; + cp.x = static_cast(pts[i].x) / 100.0; // cm → m + cp.y = static_cast(pts[i].y) / 100.0; + cp.z = static_cast(pts[i].z) / 100.0; + cp.reflectivity = pts[i].reflectivity; + cp.tag = pts[i].tag; + cp.line = 0; + cp.offset_time = static_cast(ts_ns - g_frame_start_ns); + g_accumulated_points.push_back(cp); + } + } +} + +static void on_imu_data(const uint32_t /*handle*/, const uint8_t /*dev_type*/, + LivoxLidarEthernetPacket* data, void* /*client_data*/) { + if (!g_running.load() || data == nullptr || !g_fastlio) return; + + double ts = static_cast(get_timestamp_ns(data)) / 1e9; + auto* imu_pts = reinterpret_cast(data->data); + uint16_t dot_num = data->dot_num; + + for (uint16_t i = 0; i < dot_num; ++i) { + auto imu_msg = boost::make_shared(); + imu_msg->header.stamp = custom_messages::Time().fromSec(ts); + imu_msg->header.seq = 0; + imu_msg->header.frame_id = "livox_frame"; + + imu_msg->orientation.x = 0.0; + imu_msg->orientation.y = 0.0; + imu_msg->orientation.z = 0.0; + imu_msg->orientation.w = 1.0; + for (int j = 0; j < 9; ++j) + imu_msg->orientation_covariance[j] = 0.0; + + imu_msg->angular_velocity.x = static_cast(imu_pts[i].gyro_x); + imu_msg->angular_velocity.y = static_cast(imu_pts[i].gyro_y); + imu_msg->angular_velocity.z = static_cast(imu_pts[i].gyro_z); + for (int j = 0; j < 9; ++j) + imu_msg->angular_velocity_covariance[j] = 0.0; + + imu_msg->linear_acceleration.x = static_cast(imu_pts[i].acc_x) * GRAVITY_MS2; + imu_msg->linear_acceleration.y = static_cast(imu_pts[i].acc_y) * GRAVITY_MS2; + imu_msg->linear_acceleration.z = static_cast(imu_pts[i].acc_z) * GRAVITY_MS2; + for (int j = 0; j < 9; ++j) + imu_msg->linear_acceleration_covariance[j] = 0.0; + + g_fastlio->feed_imu(imu_msg); + } +} + +static void on_info_change(const uint32_t handle, const LivoxLidarInfo* info, + void* /*client_data*/) { + if (info == nullptr) return; + + char sn[17] = {}; + std::memcpy(sn, info->sn, 16); + char ip[17] = {}; + std::memcpy(ip, info->lidar_ip, 16); + + printf("[fastlio2] Device connected: handle=%u type=%u sn=%s ip=%s\n", + handle, info->dev_type, sn, ip); + + SetLivoxLidarWorkMode(handle, kLivoxLidarNormal, nullptr, nullptr); + EnableLivoxLidarImuData(handle, nullptr, nullptr); +} + +// --------------------------------------------------------------------------- +// Signal handling +// --------------------------------------------------------------------------- + +static void signal_handler(int /*sig*/) { + g_running.store(false); +} + +// --------------------------------------------------------------------------- +// Livox SDK config file generation (same pattern as mid360_native) +// --------------------------------------------------------------------------- + +struct SdkPorts { + int cmd_data = 56100; + int push_msg = 56200; + int point_data = 56300; + int imu_data = 56400; + int log_data = 56500; + int host_cmd_data = 56101; + int host_push_msg = 56201; + int host_point_data = 56301; + int host_imu_data = 56401; + int host_log_data = 56501; +}; + +static std::string write_sdk_config(const std::string& host_ip, + const std::string& lidar_ip, + const SdkPorts& ports) { + char tmpl[] = "/tmp/livox_fastlio2_XXXXXX"; + int fd = mkstemp(tmpl); + if (fd < 0) { + perror("mkstemp"); + return ""; + } + + FILE* fp = fdopen(fd, "w"); + if (!fp) { + perror("fdopen"); + close(fd); + return ""; + } + + fprintf(fp, + "{\n" + " \"MID360\": {\n" + " \"lidar_net_info\": {\n" + " \"cmd_data_port\": %d,\n" + " \"push_msg_port\": %d,\n" + " \"point_data_port\": %d,\n" + " \"imu_data_port\": %d,\n" + " \"log_data_port\": %d\n" + " },\n" + " \"host_net_info\": [\n" + " {\n" + " \"host_ip\": \"%s\",\n" + " \"multicast_ip\": \"224.1.1.5\",\n" + " \"cmd_data_port\": %d,\n" + " \"push_msg_port\": %d,\n" + " \"point_data_port\": %d,\n" + " \"imu_data_port\": %d,\n" + " \"log_data_port\": %d\n" + " }\n" + " ]\n" + " }\n" + "}\n", + ports.cmd_data, ports.push_msg, ports.point_data, + ports.imu_data, ports.log_data, + host_ip.c_str(), + ports.host_cmd_data, ports.host_push_msg, ports.host_point_data, + ports.host_imu_data, ports.host_log_data); + fclose(fp); + + return tmpl; +} + +// --------------------------------------------------------------------------- +// Main +// --------------------------------------------------------------------------- + +int main(int argc, char** argv) { + dimos::NativeModule mod(argc, argv); + + // Required: LCM topics for output ports + g_lidar_topic = mod.has("lidar") ? mod.topic("lidar") : ""; + g_odometry_topic = mod.has("odometry") ? mod.topic("odometry") : ""; + + if (g_lidar_topic.empty() && g_odometry_topic.empty()) { + fprintf(stderr, "Error: at least one of --lidar or --odometry is required\n"); + return 1; + } + + // FAST-LIO config path + std::string config_path = mod.arg("config_path", ""); + if (config_path.empty()) { + fprintf(stderr, "Error: --config_path is required\n"); + return 1; + } + + // Livox hardware config + std::string host_ip = mod.arg("host_ip", "192.168.1.5"); + std::string lidar_ip = mod.arg("lidar_ip", "192.168.1.155"); + g_frequency = mod.arg_float("frequency", 10.0f); + g_frame_id = mod.arg("frame_id", "map"); + g_child_frame_id = mod.arg("child_frame_id", "body"); + float pointcloud_freq = mod.arg_float("pointcloud_freq", 5.0f); + float odom_freq = mod.arg_float("odom_freq", 50.0f); + + // SDK network ports + SdkPorts ports; + ports.cmd_data = mod.arg_int("cmd_data_port", 56100); + ports.push_msg = mod.arg_int("push_msg_port", 56200); + ports.point_data = mod.arg_int("point_data_port", 56300); + ports.imu_data = mod.arg_int("imu_data_port", 56400); + ports.log_data = mod.arg_int("log_data_port", 56500); + ports.host_cmd_data = mod.arg_int("host_cmd_data_port", 56101); + ports.host_push_msg = mod.arg_int("host_push_msg_port", 56201); + ports.host_point_data = mod.arg_int("host_point_data_port", 56301); + ports.host_imu_data = mod.arg_int("host_imu_data_port", 56401); + ports.host_log_data = mod.arg_int("host_log_data_port", 56501); + + printf("[fastlio2] Starting FAST-LIO2 + Livox Mid-360 native module\n"); + printf("[fastlio2] lidar topic: %s\n", + g_lidar_topic.empty() ? "(disabled)" : g_lidar_topic.c_str()); + printf("[fastlio2] odometry topic: %s\n", + g_odometry_topic.empty() ? "(disabled)" : g_odometry_topic.c_str()); + printf("[fastlio2] config: %s\n", config_path.c_str()); + printf("[fastlio2] host_ip: %s lidar_ip: %s frequency: %.1f Hz\n", + host_ip.c_str(), lidar_ip.c_str(), g_frequency); + printf("[fastlio2] pointcloud_freq: %.1f Hz odom_freq: %.1f Hz\n", + pointcloud_freq, odom_freq); + + // Signal handlers + signal(SIGTERM, signal_handler); + signal(SIGINT, signal_handler); + + // Init LCM + lcm::LCM lcm; + if (!lcm.good()) { + fprintf(stderr, "Error: LCM init failed\n"); + return 1; + } + g_lcm = &lcm; + + // Init FAST-LIO with config + printf("[fastlio2] Initializing FAST-LIO...\n"); + FastLio fast_lio(config_path); + g_fastlio = &fast_lio; + printf("[fastlio2] FAST-LIO initialized.\n"); + + // Write Livox SDK config + std::string sdk_config_path = write_sdk_config(host_ip, lidar_ip, ports); + if (sdk_config_path.empty()) { + fprintf(stderr, "Error: failed to write SDK config\n"); + return 1; + } + + // Init Livox SDK + if (!LivoxLidarSdkInit(sdk_config_path.c_str(), host_ip.c_str())) { + fprintf(stderr, "Error: LivoxLidarSdkInit failed\n"); + std::remove(sdk_config_path.c_str()); + return 1; + } + + // Register SDK callbacks + SetLivoxLidarPointCloudCallBack(on_point_cloud, nullptr); + SetLivoxLidarImuDataCallback(on_imu_data, nullptr); + SetLivoxLidarInfoChangeCallback(on_info_change, nullptr); + + // Start SDK + if (!LivoxLidarSdkStart()) { + fprintf(stderr, "Error: LivoxLidarSdkStart failed\n"); + LivoxLidarSdkUninit(); + std::remove(sdk_config_path.c_str()); + return 1; + } + + printf("[fastlio2] SDK started, waiting for device...\n"); + + // Main loop + auto frame_interval = std::chrono::microseconds( + static_cast(1e6 / g_frequency)); + auto last_emit = std::chrono::steady_clock::now(); + const double process_period_ms = 1000.0 / 5000.0; // FAST-LIO processes at ~5kHz + + // Rate limiters for output publishing + auto pc_interval = std::chrono::microseconds( + static_cast(1e6 / pointcloud_freq)); + auto odom_interval = std::chrono::microseconds( + static_cast(1e6 / odom_freq)); + auto last_pc_publish = std::chrono::steady_clock::now(); + auto last_odom_publish = std::chrono::steady_clock::now(); + + // Accumulate world-frame scans between pointcloud publishes + PointCloudXYZI::Ptr accumulated_scan(new PointCloudXYZI()); + + while (g_running.load()) { + auto loop_start = std::chrono::high_resolution_clock::now(); + + // At frame rate: build CustomMsg from accumulated points and feed to FAST-LIO + auto now = std::chrono::steady_clock::now(); + if (now - last_emit >= frame_interval) { + std::vector points; + uint64_t frame_start = 0; + + { + std::lock_guard lock(g_pc_mutex); + if (!g_accumulated_points.empty()) { + points.swap(g_accumulated_points); + frame_start = g_frame_start_ns; + g_frame_has_timestamp = false; + } + } + + if (!points.empty()) { + // Build CustomMsg + auto lidar_msg = boost::make_shared(); + lidar_msg->header.seq = 0; + lidar_msg->header.stamp = custom_messages::Time().fromSec( + static_cast(frame_start) / 1e9); + lidar_msg->header.frame_id = "livox_frame"; + lidar_msg->timebase = frame_start; + lidar_msg->lidar_id = 0; + for (int i = 0; i < 3; i++) + lidar_msg->rsvd[i] = 0; + lidar_msg->point_num = static_cast(points.size()); + lidar_msg->points = std::move(points); + + fast_lio.feed_lidar(lidar_msg); + } + + last_emit = now; + } + + // Run FAST-LIO processing step (high frequency) + fast_lio.process(); + + // Check for new results and accumulate/publish (rate-limited) + auto pose = fast_lio.get_pose(); + if (!pose.empty() && (pose[0] != 0.0 || pose[1] != 0.0 || pose[2] != 0.0)) { + double ts = std::chrono::duration( + std::chrono::system_clock::now().time_since_epoch()).count(); + + // Accumulate registered scans between publishes + if (!g_lidar_topic.empty()) { + auto world_cloud = fast_lio.get_world_cloud(); + if (world_cloud && !world_cloud->empty()) { + *accumulated_scan += *world_cloud; + } + + // Publish aggregated cloud at pointcloud_freq + if (now - last_pc_publish >= pc_interval && !accumulated_scan->empty()) { + publish_lidar(accumulated_scan, ts); + accumulated_scan->clear(); + last_pc_publish = now; + } + } + + // Publish odometry (rate-limited to odom_freq) + if (!g_odometry_topic.empty() && (now - last_odom_publish >= odom_interval)) { + publish_odometry(fast_lio.get_odometry(), ts); + last_odom_publish = now; + } + } + + // Handle LCM messages + lcm.handleTimeout(0); + + // Rate control (~5kHz processing) + auto loop_end = std::chrono::high_resolution_clock::now(); + auto elapsed_ms = std::chrono::duration(loop_end - loop_start).count(); + if (elapsed_ms < process_period_ms) { + std::this_thread::sleep_for(std::chrono::microseconds( + static_cast((process_period_ms - elapsed_ms) * 1000))); + } + } + + // Cleanup + printf("[fastlio2] Shutting down...\n"); + g_fastlio = nullptr; + LivoxLidarSdkUninit(); + std::remove(sdk_config_path.c_str()); + g_lcm = nullptr; + + printf("[fastlio2] Done.\n"); + return 0; +} diff --git a/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py b/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py new file mode 100644 index 0000000000..2dded02a32 --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py @@ -0,0 +1,36 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from dimos.core.blueprints import autoconnect +from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2Module +from dimos.mapping.voxels import VoxelGridMapper +from dimos.visualization.rerun.bridge import rerun_bridge + +mid360_fastlio = autoconnect( + FastLio2Module.blueprint(), + rerun_bridge(), +).global_config(n_dask_workers=2, robot_model="mid360_fastlio2") + + +rerun_config = {} + +mid360_fastlio_mapper = autoconnect( + FastLio2Module.blueprint(), + VoxelGridMapper.blueprint(publish_interval=0.5, voxel_size=0.25, carve_columns=False), + rerun_bridge( + visual_override={ + "world/global_map": lambda grid: grid.to_rerun(voxel_size=0.25, mode="boxes"), + } + ), +).global_config(n_dask_workers=2, robot_model="mid360_fastlio2_voxels") diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py new file mode 100644 index 0000000000..5d97ee2cdd --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -0,0 +1,191 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Python NativeModule wrapper for the FAST-LIO2 + Livox Mid-360 binary. + +Binds Livox SDK2 directly into FAST-LIO-NON-ROS for real-time LiDAR SLAM. +Outputs registered (world-frame) point clouds and odometry with covariance. + +Usage:: + + from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2Module + from dimos.core.blueprints import autoconnect + + autoconnect( + FastLio2Module.blueprint(host_ip="192.168.1.5"), + SomeConsumer.blueprint(), + ).build().loop() +""" + +from __future__ import annotations + +from dataclasses import dataclass, field +import json +from pathlib import Path +import tempfile +from typing import TYPE_CHECKING + +from dimos.core.native_module import NativeModule, NativeModuleConfig + +if TYPE_CHECKING: + from dimos.core import Out + from dimos.msgs.nav_msgs.Odometry import Odometry + from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + +_DEFAULT_EXECUTABLE = str(Path(__file__).parent / "cpp" / "build" / "fastlio2_native") +_DEFAULT_CONFIG = str(Path(__file__).parent / "cpp" / "config" / "mid360.json") + + +@dataclass(kw_only=True) +class FastLio2Config(NativeModuleConfig): + """Config for the FAST-LIO2 + Livox Mid-360 native module.""" + + executable: str = _DEFAULT_EXECUTABLE + + # Livox SDK hardware config + host_ip: str = "192.168.1.5" + lidar_ip: str = "192.168.1.155" + frequency: float = 10.0 + + # Frame IDs for output messages + frame_id: str = "map" + child_frame_id: str = "body" + + # Output publish rates (Hz) + pointcloud_freq: float = 10.0 + odom_freq: float = 30.0 + + # FAST-LIO config (written to JSON, passed as --config_path) + scan_line: int = 1 + blind: float = 1.0 + fov_degree: int = 360 + det_range: float = 100.0 + acc_cov: float = 0.1 + gyr_cov: float = 0.1 + b_acc_cov: float = 0.0001 + b_gyr_cov: float = 0.0001 + extrinsic_est_en: bool = True + extrinsic_t: list[float] = field(default_factory=lambda: [0.04165, 0.02326, -0.0284]) + extrinsic_r: list[float] = field( + default_factory=lambda: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] + ) + + # SDK port configuration (for multi-sensor setups) + cmd_data_port: int = 56100 + push_msg_port: int = 56200 + point_data_port: int = 56300 + imu_data_port: int = 56400 + log_data_port: int = 56500 + host_cmd_data_port: int = 56101 + host_push_msg_port: int = 56201 + host_point_data_port: int = 56301 + host_imu_data_port: int = 56401 + host_log_data_port: int = 56501 + + +class FastLio2Module(NativeModule): + """FAST-LIO2 SLAM module with integrated Livox Mid-360 driver. + + Ports: + lidar (Out[PointCloud2]): World-frame registered point cloud. + odometry (Out[Odometry]): Pose with covariance at LiDAR scan rate. + """ + + default_config: type[FastLio2Config] = FastLio2Config # type: ignore[assignment] + lidar: Out[PointCloud2] + odometry: Out[Odometry] + + def _build_extra_args(self) -> list[str]: + """Pass hardware and SLAM config to the C++ binary as CLI args.""" + cfg: FastLio2Config = self.config # type: ignore[assignment] + config_path = self._write_fastlio_config(cfg) + return [ + "--config_path", + config_path, + "--host_ip", + cfg.host_ip, + "--lidar_ip", + cfg.lidar_ip, + "--frequency", + str(cfg.frequency), + "--frame_id", + cfg.frame_id, + "--child_frame_id", + cfg.child_frame_id, + "--pointcloud_freq", + str(cfg.pointcloud_freq), + "--odom_freq", + str(cfg.odom_freq), + "--cmd_data_port", + str(cfg.cmd_data_port), + "--push_msg_port", + str(cfg.push_msg_port), + "--point_data_port", + str(cfg.point_data_port), + "--imu_data_port", + str(cfg.imu_data_port), + "--log_data_port", + str(cfg.log_data_port), + "--host_cmd_data_port", + str(cfg.host_cmd_data_port), + "--host_push_msg_port", + str(cfg.host_push_msg_port), + "--host_point_data_port", + str(cfg.host_point_data_port), + "--host_imu_data_port", + str(cfg.host_imu_data_port), + "--host_log_data_port", + str(cfg.host_log_data_port), + ] + + @staticmethod + def _write_fastlio_config(cfg: FastLio2Config) -> str: + """Write FAST-LIO JSON config to a temp file, return path.""" + config = { + "common": { + "time_sync_en": False, + "time_offset_lidar_to_imu": 0.0, + "msr_freq": 50.0, + "main_freq": 5000.0, + }, + "preprocess": { + "lidar_type": 1, + "scan_line": cfg.scan_line, + "blind": cfg.blind, + }, + "mapping": { + "acc_cov": cfg.acc_cov, + "gyr_cov": cfg.gyr_cov, + "b_acc_cov": cfg.b_acc_cov, + "b_gyr_cov": cfg.b_gyr_cov, + "fov_degree": cfg.fov_degree, + "det_range": cfg.det_range, + "extrinsic_est_en": cfg.extrinsic_est_en, + "extrinsic_T": list(cfg.extrinsic_t), + "extrinsic_R": list(cfg.extrinsic_r), + }, + } + f = tempfile.NamedTemporaryFile(mode="w", suffix=".json", prefix="fastlio2_", delete=False) + json.dump(config, f, indent=2) + f.close() + return f.name + + +fastlio2_module = FastLio2Module.blueprint + +__all__ = [ + "FastLio2Config", + "FastLio2Module", + "fastlio2_module", +] diff --git a/dimos/hardware/sensors/lidar/fastlio2/test_spec_compliance.py b/dimos/hardware/sensors/lidar/fastlio2/test_spec_compliance.py new file mode 100644 index 0000000000..8707d7c0af --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/test_spec_compliance.py @@ -0,0 +1,29 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Spec compliance tests for the FAST-LIO2 module.""" + +from __future__ import annotations + +from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2Module + + +def test_fastlio2_has_lidar_port() -> None: + hints = FastLio2Module.__annotations__ + assert "lidar" in hints + + +def test_fastlio2_has_odometry_port() -> None: + hints = FastLio2Module.__annotations__ + assert "odometry" in hints diff --git a/dimos/hardware/sensors/lidar/test_spec_compliance.py b/dimos/hardware/sensors/lidar/test_spec_compliance.py new file mode 100644 index 0000000000..14d7a2e61e --- /dev/null +++ b/dimos/hardware/sensors/lidar/test_spec_compliance.py @@ -0,0 +1,34 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Spec compliance tests for LidarModule.""" + +from __future__ import annotations + +import typing + +from dimos.hardware.sensors.lidar.module import LidarModule +from dimos.spec import perception +from dimos.spec.utils import assert_implements_protocol + + +def test_lidar_module_implements_lidar_spec() -> None: + assert_implements_protocol(LidarModule, perception.Lidar) + + +def test_lidar_spec_does_not_require_imu() -> None: + """Not all LiDARs have IMU — Lidar spec should only require pointcloud.""" + hints = typing.get_type_hints(perception.Lidar, include_extras=True) + assert "pointcloud" in hints + assert "imu" not in hints diff --git a/dimos/msgs/nav_msgs/Odometry.py b/dimos/msgs/nav_msgs/Odometry.py index e6d0844f46..14d26455f8 100644 --- a/dimos/msgs/nav_msgs/Odometry.py +++ b/dimos/msgs/nav_msgs/Odometry.py @@ -17,6 +17,9 @@ import time from typing import TYPE_CHECKING, TypeAlias +if TYPE_CHECKING: + from rerun._baseclasses import Archetype + from dimos_lcm.nav_msgs import Odometry as LCMOdometry import numpy as np from plum import dispatch @@ -252,6 +255,22 @@ def __eq__(self, other) -> bool: # type: ignore[no-untyped-def] and self.twist == other.twist ) + def to_rerun(self) -> Archetype: + """Convert to rerun Transform3D for visualizing the pose.""" + import rerun as rr + + return rr.Transform3D( + translation=[self.x, self.y, self.z], + rotation=rr.Quaternion( + xyzw=[ + self.orientation.x, + self.orientation.y, + self.orientation.z, + self.orientation.w, + ] + ), + ) + def lcm_encode(self) -> bytes: """Encode to LCM binary format.""" lcm_msg = LCMOdometry() diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index d3a6a58993..7f3b2f07d3 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -40,6 +40,8 @@ "demo-osm": "dimos.mapping.osm.demo_osm:demo_osm", "demo-skill": "dimos.agents.skills.demo_skill:demo_skill", "dual-xarm6-planner": "dimos.manipulation.manipulation_blueprints:dual_xarm6_planner", + "mid360-fastlio": "dimos.hardware.sensors.lidar.fastlio2.fastlio_blueprints:mid360_fastlio", + "mid360-fastlio-mapper": "dimos.hardware.sensors.lidar.fastlio2.fastlio_blueprints:mid360_fastlio_mapper", "mid360cpp": "dimos.hardware.sensors.lidar.livox.livox_blueprints:mid360cpp", "unitree-g1": "dimos.robot.unitree_webrtc.unitree_g1_blueprints:unitree_g1", "unitree-g1-agentic": "dimos.robot.unitree_webrtc.unitree_g1_blueprints:unitree_g1_agentic", @@ -78,6 +80,7 @@ "depth_module": "dimos.robot.unitree_webrtc.depth_module", "detection3d_module": "dimos.perception.detection.module3D", "detection_db_module": "dimos.perception.detection.moduleDB", + "fastlio2_module": "dimos.hardware.sensors.lidar.fastlio2.module", "foxglove_bridge": "dimos.robot.foxglove_bridge", "g1_connection": "dimos.robot.unitree.connection.g1", "g1_sim_connection": "dimos.robot.unitree.connection.g1sim", @@ -90,11 +93,10 @@ "joint_trajectory_controller": "dimos.manipulation.control.trajectory_controller.joint_trajectory_controller", "keyboard_teleop": "dimos.robot.unitree_webrtc.keyboard_teleop", "lidar_module": "dimos.hardware.sensors.lidar.module", - "livox_lidar_module": "dimos.hardware.sensors.lidar.livox.module", "llm_agent": "dimos.agents.agent", "manipulation_module": "dimos.manipulation.manipulation_module", "mapper": "dimos.robot.unitree_webrtc.type.map", - "mid360_cpp_module": "dimos.hardware.sensors.lidar.livox.cpp.module", + "mid360_cpp_module": "dimos.hardware.sensors.lidar.livox.module", "navigation_skill": "dimos.agents.skills.navigation", "object_scene_registration_module": "dimos.perception.object_scene_registration", "object_tracking": "dimos.perception.object_tracker", diff --git a/flake.nix b/flake.nix index 50738effc0..ec3e919a90 100644 --- a/flake.nix +++ b/flake.nix @@ -129,6 +129,11 @@ { vals.pkg=pkgs.libjpeg_turbo; flags.ldLibraryGroup=true; } { vals.pkg=pkgs.libpng; flags={}; } + ### FAST-LIO2 native module deps + { vals.pkg=pkgs.pcl; flags.ldLibraryGroup=true; } + { vals.pkg=pkgs.nlohmann_json; flags={}; } + { vals.pkg=pkgs.llvmPackages.openmp; flags.ldLibraryGroup=true; } + ### Docs generators { vals.pkg=pkgs.pikchr; flags={}; } { vals.pkg=pkgs.graphviz; flags={}; } From a84cc090c3867fc380e6dca02ccb9df78497f712 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Thu, 12 Feb 2026 21:52:53 +0800 Subject: [PATCH 11/15] Fetch FAST-LIO-NON-ROS from GitHub instead of hardcoded local path --- .../sensors/lidar/fastlio2/cpp/CMakeLists.txt | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt b/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt index 301c257ddd..9ddf08bade 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt @@ -5,9 +5,6 @@ set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3") -# FAST-LIO-NON-ROS source directory (local reference) -set(FASTLIO_DIR "$ENV{HOME}/coding/FAST-LIO-NON-ROS") - # OpenMP for parallel processing find_package(OpenMP QUIET) if(OpenMP_CXX_FOUND) @@ -30,8 +27,22 @@ else() add_definitions(-DMP_PROC_NUM=1) endif() -# Fetch dimos-lcm for C++ message headers +# Fetch dependencies include(FetchContent) + +# FAST-LIO-NON-ROS +FetchContent_Declare(fastlio + GIT_REPOSITORY https://github.com/leshy/FAST-LIO-NON-ROS.git + GIT_TAG dimos-integration + GIT_SHALLOW TRUE +) +FetchContent_GetProperties(fastlio) +if(NOT fastlio_POPULATED) + FetchContent_Populate(fastlio) +endif() +set(FASTLIO_DIR ${fastlio_SOURCE_DIR}) + +# dimos-lcm C++ message headers FetchContent_Declare(dimos_lcm GIT_REPOSITORY https://github.com/dimensionalOS/dimos-lcm.git GIT_TAG main From cb8fcdc0ba5619ae0008082321821d638b987bf7 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Thu, 12 Feb 2026 23:02:22 +0800 Subject: [PATCH 12/15] ruff bugfix --- .../sensors/lidar/fastlio2/fastlio_blueprints.py | 2 +- dimos/hardware/sensors/lidar/fastlio2/module.py | 9 +++------ dimos/hardware/sensors/lidar/livox/livox_blueprints.py | 4 ++-- dimos/hardware/sensors/lidar/livox/module.py | 9 +++------ dimos/robot/all_blueprints.py | 4 ++-- 5 files changed, 11 insertions(+), 17 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py b/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py index 2dded02a32..7337a41ef4 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py +++ b/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py @@ -25,7 +25,7 @@ rerun_config = {} -mid360_fastlio_mapper = autoconnect( +mid360_fastlio_voxels = autoconnect( FastLio2Module.blueprint(), VoxelGridMapper.blueprint(publish_interval=0.5, voxel_size=0.25, carve_columns=False), rerun_bridge( diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py index 5d97ee2cdd..936ecf8e5c 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -34,14 +34,11 @@ import json from pathlib import Path import tempfile -from typing import TYPE_CHECKING +from dimos.core import Out # noqa: TC001 from dimos.core.native_module import NativeModule, NativeModuleConfig - -if TYPE_CHECKING: - from dimos.core import Out - from dimos.msgs.nav_msgs.Odometry import Odometry - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.nav_msgs.Odometry import Odometry # noqa: TC001 +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 # noqa: TC001 _DEFAULT_EXECUTABLE = str(Path(__file__).parent / "cpp" / "build" / "fastlio2_native") _DEFAULT_CONFIG = str(Path(__file__).parent / "cpp" / "config" / "mid360.json") diff --git a/dimos/hardware/sensors/lidar/livox/livox_blueprints.py b/dimos/hardware/sensors/lidar/livox/livox_blueprints.py index a4cbae37fb..29f404b23f 100644 --- a/dimos/hardware/sensors/lidar/livox/livox_blueprints.py +++ b/dimos/hardware/sensors/lidar/livox/livox_blueprints.py @@ -16,7 +16,7 @@ from dimos.hardware.sensors.lidar.livox.module import Mid360CppModule from dimos.visualization.rerun.bridge import rerun_bridge -mid360cpp = autoconnect( +mid360 = autoconnect( Mid360CppModule.blueprint(), rerun_bridge(), -).global_config(n_dask_workers=2, robot_model="mid360_livox_cpp") +).global_config(n_dask_workers=2, robot_model="mid360") diff --git a/dimos/hardware/sensors/lidar/livox/module.py b/dimos/hardware/sensors/lidar/livox/module.py index d6c4eb4744..3959810ef4 100644 --- a/dimos/hardware/sensors/lidar/livox/module.py +++ b/dimos/hardware/sensors/lidar/livox/module.py @@ -33,16 +33,13 @@ from dataclasses import dataclass from pathlib import Path -from typing import TYPE_CHECKING +from dimos.core import Out # noqa: TC001 from dimos.core.native_module import NativeModule, NativeModuleConfig +from dimos.msgs.sensor_msgs.Imu import Imu # noqa: TC001 +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 # noqa: TC001 from dimos.spec import perception -if TYPE_CHECKING: - from dimos.core import Out - from dimos.msgs.sensor_msgs.Imu import Imu - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 - _DEFAULT_EXECUTABLE = str(Path(__file__).parent / "cpp" / "build" / "mid360_native") diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 7f3b2f07d3..eabf588388 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -40,9 +40,9 @@ "demo-osm": "dimos.mapping.osm.demo_osm:demo_osm", "demo-skill": "dimos.agents.skills.demo_skill:demo_skill", "dual-xarm6-planner": "dimos.manipulation.manipulation_blueprints:dual_xarm6_planner", + "mid360": "dimos.hardware.sensors.lidar.livox.livox_blueprints:mid360", "mid360-fastlio": "dimos.hardware.sensors.lidar.fastlio2.fastlio_blueprints:mid360_fastlio", - "mid360-fastlio-mapper": "dimos.hardware.sensors.lidar.fastlio2.fastlio_blueprints:mid360_fastlio_mapper", - "mid360cpp": "dimos.hardware.sensors.lidar.livox.livox_blueprints:mid360cpp", + "mid360-fastlio-voxels": "dimos.hardware.sensors.lidar.fastlio2.fastlio_blueprints:mid360_fastlio_voxels", "unitree-g1": "dimos.robot.unitree_webrtc.unitree_g1_blueprints:unitree_g1", "unitree-g1-agentic": "dimos.robot.unitree_webrtc.unitree_g1_blueprints:unitree_g1_agentic", "unitree-g1-agentic-sim": "dimos.robot.unitree_webrtc.unitree_g1_blueprints:unitree_g1_agentic_sim", From d345e8276dad036d714a2a4476df12728cc96087 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Thu, 12 Feb 2026 23:19:20 +0800 Subject: [PATCH 13/15] nicer fastlio config --- .../sensors/lidar/fastlio2/fastlio_blueprints.py | 11 ++++++----- dimos/hardware/sensors/lidar/fastlio2/module.py | 6 +++--- dimos/visualization/rerun/bridge.py | 4 ++++ 3 files changed, 13 insertions(+), 8 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py b/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py index 7337a41ef4..f1a25c0d0b 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py +++ b/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py @@ -13,12 +13,12 @@ # limitations under the License. from dimos.core.blueprints import autoconnect -from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2Module +from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 from dimos.mapping.voxels import VoxelGridMapper from dimos.visualization.rerun.bridge import rerun_bridge mid360_fastlio = autoconnect( - FastLio2Module.blueprint(), + FastLio2.blueprint(), rerun_bridge(), ).global_config(n_dask_workers=2, robot_model="mid360_fastlio2") @@ -27,10 +27,11 @@ mid360_fastlio_voxels = autoconnect( FastLio2Module.blueprint(), - VoxelGridMapper.blueprint(publish_interval=0.5, voxel_size=0.25, carve_columns=False), + VoxelGridMapper.blueprint(publish_interval=0.5, voxel_size=0.1, carve_columns=False), rerun_bridge( visual_override={ - "world/global_map": lambda grid: grid.to_rerun(voxel_size=0.25, mode="boxes"), + "world/global_map": lambda grid: grid.to_rerun(voxel_size=0.1, mode="boxes"), + "world/lidar": None, } ), -).global_config(n_dask_workers=2, robot_model="mid360_fastlio2_voxels") +).global_config(n_dask_workers=3, robot_model="mid360_fastlio2_voxels") diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py index 936ecf8e5c..fa2a5c4e11 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -91,7 +91,7 @@ class FastLio2Config(NativeModuleConfig): host_log_data_port: int = 56501 -class FastLio2Module(NativeModule): +class FastLio2(NativeModule): """FAST-LIO2 SLAM module with integrated Livox Mid-360 driver. Ports: @@ -179,10 +179,10 @@ def _write_fastlio_config(cfg: FastLio2Config) -> str: return f.name -fastlio2_module = FastLio2Module.blueprint +fastlio2_module = FastLio2.blueprint __all__ = [ + "FastLio2", "FastLio2Config", - "FastLio2Module", "fastlio2_module", ] diff --git a/dimos/visualization/rerun/bridge.py b/dimos/visualization/rerun/bridge.py index 4e39f0b64c..c236b5c543 100644 --- a/dimos/visualization/rerun/bridge.py +++ b/dimos/visualization/rerun/bridge.py @@ -202,6 +202,10 @@ def _visual_override_for_entity_path( if pattern_matches(pattern, entity_path) ] + # None means "suppress this topic entirely" + if any(fn is None for fn in matches): + return lambda msg: None + # final step (ensures we return Archetype or None) def final_convert(msg: Any) -> RerunData | None: if isinstance(msg, Archetype): From bcda438aa52eefd870dddf88f6d559887c99df73 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Thu, 12 Feb 2026 23:42:42 +0800 Subject: [PATCH 14/15] correct mid360 config --- .../sensors/lidar/fastlio2/cpp/CMakeLists.txt | 23 ++++++++++--------- .../lidar/fastlio2/fastlio_blueprints.py | 2 +- .../hardware/sensors/lidar/fastlio2/module.py | 6 ++--- 3 files changed, 16 insertions(+), 15 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt b/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt index 9ddf08bade..b10847b9a6 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt @@ -30,17 +30,18 @@ endif() # Fetch dependencies include(FetchContent) -# FAST-LIO-NON-ROS -FetchContent_Declare(fastlio - GIT_REPOSITORY https://github.com/leshy/FAST-LIO-NON-ROS.git - GIT_TAG dimos-integration - GIT_SHALLOW TRUE -) -FetchContent_GetProperties(fastlio) -if(NOT fastlio_POPULATED) - FetchContent_Populate(fastlio) -endif() -set(FASTLIO_DIR ${fastlio_SOURCE_DIR}) +# FAST-LIO-NON-ROS (local checkout for development) +set(FASTLIO_DIR $ENV{HOME}/coding/FAST-LIO-NON-ROS) +# FetchContent_Declare(fastlio +# GIT_REPOSITORY https://github.com/leshy/FAST-LIO-NON-ROS.git +# GIT_TAG dimos-integration +# GIT_SHALLOW TRUE +# ) +# FetchContent_GetProperties(fastlio) +# if(NOT fastlio_POPULATED) +# FetchContent_Populate(fastlio) +# endif() +# set(FASTLIO_DIR ${fastlio_SOURCE_DIR}) # dimos-lcm C++ message headers FetchContent_Declare(dimos_lcm diff --git a/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py b/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py index f1a25c0d0b..f22af3b4b0 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py +++ b/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py @@ -26,7 +26,7 @@ rerun_config = {} mid360_fastlio_voxels = autoconnect( - FastLio2Module.blueprint(), + FastLio2.blueprint(), VoxelGridMapper.blueprint(publish_interval=0.5, voxel_size=0.1, carve_columns=False), rerun_bridge( visual_override={ diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py index fa2a5c4e11..95b8c6ef29 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -64,8 +64,8 @@ class FastLio2Config(NativeModuleConfig): odom_freq: float = 30.0 # FAST-LIO config (written to JSON, passed as --config_path) - scan_line: int = 1 - blind: float = 1.0 + scan_line: int = 4 + blind: float = 0.5 fov_degree: int = 360 det_range: float = 100.0 acc_cov: float = 0.1 @@ -73,7 +73,7 @@ class FastLio2Config(NativeModuleConfig): b_acc_cov: float = 0.0001 b_gyr_cov: float = 0.0001 extrinsic_est_en: bool = True - extrinsic_t: list[float] = field(default_factory=lambda: [0.04165, 0.02326, -0.0284]) + extrinsic_t: list[float] = field(default_factory=lambda: [-0.011, -0.02329, 0.04412]) extrinsic_r: list[float] = field( default_factory=lambda: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] ) From 28c80ac679a5f4893da4e37b716878b2b6edac82 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Fri, 13 Feb 2026 00:01:17 +0800 Subject: [PATCH 15/15] fastlio filtering, tuning --- .../lidar/fastlio2/cpp/cloud_filter.hpp | 51 +++++++++++++++++++ .../sensors/lidar/fastlio2/cpp/main.cpp | 10 +++- .../lidar/fastlio2/fastlio_blueprints.py | 14 +++-- .../hardware/sensors/lidar/fastlio2/module.py | 11 ++++ 4 files changed, 81 insertions(+), 5 deletions(-) create mode 100644 dimos/hardware/sensors/lidar/fastlio2/cpp/cloud_filter.hpp diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/cloud_filter.hpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/cloud_filter.hpp new file mode 100644 index 0000000000..352ba9bef5 --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/cloud_filter.hpp @@ -0,0 +1,51 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 +// +// Point cloud filtering utilities: voxel grid downsampling and +// statistical outlier removal using PCL. + +#ifndef CLOUD_FILTER_HPP_ +#define CLOUD_FILTER_HPP_ + +#include +#include +#include +#include + +struct CloudFilterConfig { + float voxel_size = 0.1f; + int sor_mean_k = 50; + float sor_stddev = 1.0f; +}; + +/// Apply voxel grid downsample + statistical outlier removal in-place. +/// Returns the filtered cloud (new allocation). +template +typename pcl::PointCloud::Ptr filter_cloud( + const typename pcl::PointCloud::Ptr& input, + const CloudFilterConfig& cfg) { + + if (!input || input->empty()) return input; + + // Voxel grid downsample + typename pcl::PointCloud::Ptr voxelized(new pcl::PointCloud()); + pcl::VoxelGrid vg; + vg.setInputCloud(input); + vg.setLeafSize(cfg.voxel_size, cfg.voxel_size, cfg.voxel_size); + vg.filter(*voxelized); + + // Statistical outlier removal + if (cfg.sor_mean_k > 0 && voxelized->size() > static_cast(cfg.sor_mean_k)) { + typename pcl::PointCloud::Ptr cleaned(new pcl::PointCloud()); + pcl::StatisticalOutlierRemoval sor; + sor.setInputCloud(voxelized); + sor.setMeanK(cfg.sor_mean_k); + sor.setStddevMulThresh(cfg.sor_stddev); + sor.filter(*cleaned); + return cleaned; + } + + return voxelized; +} + +#endif diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp index d931ba27b4..01ce74bc86 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp @@ -30,6 +30,7 @@ #include #include +#include "cloud_filter.hpp" #include "dimos_native_module.hpp" // dimos LCM message headers @@ -396,6 +397,10 @@ int main(int argc, char** argv) { g_child_frame_id = mod.arg("child_frame_id", "body"); float pointcloud_freq = mod.arg_float("pointcloud_freq", 5.0f); float odom_freq = mod.arg_float("odom_freq", 50.0f); + CloudFilterConfig filter_cfg; + filter_cfg.voxel_size = mod.arg_float("voxel_size", 0.1f); + filter_cfg.sor_mean_k = mod.arg_int("sor_mean_k", 50); + filter_cfg.sor_stddev = mod.arg_float("sor_stddev", 1.0f); // SDK network ports SdkPorts ports; @@ -420,6 +425,8 @@ int main(int argc, char** argv) { host_ip.c_str(), lidar_ip.c_str(), g_frequency); printf("[fastlio2] pointcloud_freq: %.1f Hz odom_freq: %.1f Hz\n", pointcloud_freq, odom_freq); + printf("[fastlio2] voxel_size: %.3f sor_mean_k: %d sor_stddev: %.1f\n", + filter_cfg.voxel_size, filter_cfg.sor_mean_k, filter_cfg.sor_stddev); // Signal handlers signal(SIGTERM, signal_handler); @@ -541,7 +548,8 @@ int main(int argc, char** argv) { // Publish aggregated cloud at pointcloud_freq if (now - last_pc_publish >= pc_interval && !accumulated_scan->empty()) { - publish_lidar(accumulated_scan, ts); + auto filtered = filter_cloud(accumulated_scan, filter_cfg); + publish_lidar(filtered, ts); accumulated_scan->clear(); last_pc_publish = now; } diff --git a/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py b/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py index f22af3b4b0..0ed3cddd05 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py +++ b/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py @@ -17,9 +17,15 @@ from dimos.mapping.voxels import VoxelGridMapper from dimos.visualization.rerun.bridge import rerun_bridge +voxel_size = 0.05 + mid360_fastlio = autoconnect( - FastLio2.blueprint(), - rerun_bridge(), + FastLio2.blueprint(voxel_size=voxel_size), + rerun_bridge( + visual_override={ + "world/lidar": lambda grid: grid.to_rerun(voxel_size=voxel_size, mode="boxes"), + } + ), ).global_config(n_dask_workers=2, robot_model="mid360_fastlio2") @@ -27,10 +33,10 @@ mid360_fastlio_voxels = autoconnect( FastLio2.blueprint(), - VoxelGridMapper.blueprint(publish_interval=0.5, voxel_size=0.1, carve_columns=False), + VoxelGridMapper.blueprint(publish_interval=1.0, voxel_size=voxel_size, carve_columns=False), rerun_bridge( visual_override={ - "world/global_map": lambda grid: grid.to_rerun(voxel_size=0.1, mode="boxes"), + "world/global_map": lambda grid: grid.to_rerun(voxel_size=voxel_size, mode="boxes"), "world/lidar": None, } ), diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py index 95b8c6ef29..1440a03ccc 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -63,6 +63,11 @@ class FastLio2Config(NativeModuleConfig): pointcloud_freq: float = 10.0 odom_freq: float = 30.0 + # Point cloud filtering + voxel_size: float = 0.1 + sor_mean_k: int = 50 + sor_stddev: float = 1.0 + # FAST-LIO config (written to JSON, passed as --config_path) scan_line: int = 4 blind: float = 0.5 @@ -124,6 +129,12 @@ def _build_extra_args(self) -> list[str]: str(cfg.pointcloud_freq), "--odom_freq", str(cfg.odom_freq), + "--voxel_size", + str(cfg.voxel_size), + "--sor_mean_k", + str(cfg.sor_mean_k), + "--sor_stddev", + str(cfg.sor_stddev), "--cmd_data_port", str(cfg.cmd_data_port), "--push_msg_port",