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..833f95a7a2 --- /dev/null +++ b/dimos/core/native_module.py @@ -0,0 +1,240 @@ +# 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, fields +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 + + # Field names from base classes that should not be converted to CLI args + _BASE_FIELDS: frozenset[str] = field(default=frozenset(), init=False, repr=False, compare=False) + + def __post_init__(self): + # Collect all field names from NativeModuleConfig and its parents + object.__setattr__( + self, + "_BASE_FIELDS", + frozenset(f.name for f in fields(NativeModuleConfig)), + ) + + # Override in subclasses to exclude fields from CLI arg generation + cli_exclude: frozenset[str] = frozenset() + + def to_cli_args(self) -> list[str]: + """Auto-convert subclass config fields to CLI args. + + Iterates fields defined on the concrete subclass (not NativeModuleConfig + or its parents) and converts them to ``["--name", str(value)]`` pairs. + Skips fields whose values are ``None`` and fields in ``cli_exclude``. + """ + args: list[str] = [] + for f in fields(self): + if f.name in self._BASE_FIELDS or f.name.startswith("_"): + continue + if f.name in self.cli_exclude: + continue + val = getattr(self, f.name) + if val is None: + continue + if isinstance(val, bool): + args.extend([f"--{f.name}", str(val).lower()]) + elif isinstance(val, list): + args.extend([f"--{f.name}", ",".join(str(v) for v in val)]) + else: + args.extend([f"--{f.name}", str(val)]) + return args + + +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", + } 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/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..b10847b9a6 --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt @@ -0,0 +1,103 @@ +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") + +# 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 dependencies +include(FetchContent) + +# 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 + 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/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/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..07a45ea5ca --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp @@ -0,0 +1,616 @@ +// 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 "cloud_filter.hpp" +#include "dimos_native_module.hpp" +#include "voxel_map.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_map_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, + const std::string& topic = "") { + const std::string& chan = topic.empty() ? g_lidar_topic : topic; + if (!g_lcm || !cloud || cloud->empty() || chan.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(chan, &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") : ""; + g_map_topic = mod.has("global_map") ? mod.topic("global_map") : ""; + + 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); + 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); + float map_voxel_size = mod.arg_float("map_voxel_size", 0.1f); + float map_max_range = mod.arg_float("map_max_range", 100.0f); + float map_freq = mod.arg_float("map_freq", 0.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] global_map topic: %s\n", + g_map_topic.empty() ? "(disabled)" : g_map_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); + 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); + if (!g_map_topic.empty()) + printf("[fastlio2] map_voxel_size: %.3f map_max_range: %.1f map_freq: %.1f Hz\n", + map_voxel_size, map_max_range, map_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(); + + // Global voxel map (only if map topic is configured AND map_freq > 0) + std::unique_ptr global_map; + std::chrono::microseconds map_interval{0}; + auto last_map_publish = std::chrono::steady_clock::now(); + if (!g_map_topic.empty() && map_freq > 0.0f) { + global_map = std::make_unique(map_voxel_size, map_max_range); + map_interval = std::chrono::microseconds( + static_cast(1e6 / map_freq)); + } + + 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(); + + auto world_cloud = fast_lio.get_world_cloud(); + if (world_cloud && !world_cloud->empty()) { + auto filtered = filter_cloud(world_cloud, filter_cfg); + + // Per-scan publish at pointcloud_freq + if (!g_lidar_topic.empty() && now - last_pc_publish >= pc_interval) { + publish_lidar(filtered, ts); + last_pc_publish = now; + } + + // Global map: insert, prune, and publish at map_freq + if (global_map) { + global_map->insert(filtered); + + if (now - last_map_publish >= map_interval) { + global_map->prune( + static_cast(pose[0]), + static_cast(pose[1]), + static_cast(pose[2])); + auto map_cloud = global_map->to_cloud(); + publish_lidar(map_cloud, ts, g_map_topic); + last_map_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..ced2b74374 --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py @@ -0,0 +1,50 @@ +# 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 FastLio2 +from dimos.mapping.voxels import VoxelGridMapper +from dimos.visualization.rerun.bridge import rerun_bridge + +voxel_size = 0.05 + +mid360_fastlio = autoconnect( + FastLio2.blueprint(voxel_size=voxel_size, map_voxel_size=voxel_size, map_freq=-1), + 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") + +mid360_fastlio_voxels_native = autoconnect( + FastLio2.blueprint(voxel_size=voxel_size, map_voxel_size=voxel_size, map_freq=3.0), + rerun_bridge( + visual_override={ + "world/lidar": None, + "world/global_map": lambda grid: grid.to_rerun(voxel_size=voxel_size, mode="boxes"), + } + ), +).global_config(n_dask_workers=2, robot_model="mid360_fastlio2") + +mid360_fastlio_voxels = autoconnect( + FastLio2.blueprint(), + 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=voxel_size, mode="boxes"), + "world/lidar": None, + } + ), +).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 new file mode 100644 index 0000000000..d2fb8fd5ae --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -0,0 +1,178 @@ +# 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 dimos.core import Out # noqa: TC001 +from dimos.core.native_module import NativeModule, NativeModuleConfig +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") + + +@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 + + # Point cloud filtering + voxel_size: float = 0.1 + sor_mean_k: int = 50 + sor_stddev: float = 1.0 + + # Global voxel map (disabled when map_freq <= 0) + map_freq: float = 0.0 + map_voxel_size: float = 0.1 + map_max_range: float = 100.0 + + # FAST-LIO config (written to JSON, passed as --config_path — excluded from CLI) + cli_exclude: frozenset[str] = frozenset( + { + "scan_line", + "blind", + "fov_degree", + "det_range", + "acc_cov", + "gyr_cov", + "b_acc_cov", + "b_gyr_cov", + "extrinsic_est_en", + "extrinsic_t", + "extrinsic_r", + } + ) + scan_line: int = 4 + blind: float = 0.5 + 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.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] + ) + + # 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 FastLio2(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. + global_map (Out[PointCloud2]): Global voxel map (optional, enable via map_freq > 0). + """ + + default_config: type[FastLio2Config] = FastLio2Config # type: ignore[assignment] + lidar: Out[PointCloud2] + odometry: Out[Odometry] + global_map: Out[PointCloud2] + + 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, *cfg.to_cli_args()] + + @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 = FastLio2.blueprint + +__all__ = [ + "FastLio2", + "FastLio2Config", + "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/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/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..c4f8293008 --- /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 `Mid360Module` 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.module import Mid360Module +from dimos.core.blueprints import autoconnect + +autoconnect( + Mid360Module.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 (`Mid360Module`) | +| `CMakeLists.txt` | Build config, fetches dimos-lcm headers automatically | 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/livox_blueprints.py b/dimos/hardware/sensors/lidar/livox/livox_blueprints.py new file mode 100644 index 0000000000..b95d9fccd5 --- /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.module import Mid360Module +from dimos.visualization.rerun.bridge import rerun_bridge + +mid360 = autoconnect( + Mid360Module.blueprint(), + rerun_bridge(), +).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 new file mode 100644 index 0000000000..cbf266eb36 --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/module.py @@ -0,0 +1,96 @@ +# 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.module import Mid360Module + from dimos.core.blueprints import autoconnect + + autoconnect( + Mid360Module.blueprint(host_ip="192.168.1.5"), + SomeConsumer.blueprint(), + ).build().loop() +""" + +from __future__ import annotations + +from dataclasses import dataclass +from pathlib import Path + +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 + +_DEFAULT_EXECUTABLE = str(Path(__file__).parent / "cpp" / "build" / "mid360_native") + + +@dataclass(kw_only=True) +class Mid360Config(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 Mid360Module(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). + """ + + config: Mid360Config + default_config = Mid360Config + + pointcloud: Out[PointCloud2] + imu: Out[Imu] + + def _build_extra_args(self) -> list[str]: + """Pass hardware config to the C++ binary as CLI args.""" + return self.config.to_cli_args() + + +mid360_module = Mid360Module.blueprint + +__all__ = [ + "Mid360Config", + "Mid360Module", + "mid360_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..5f78c0922a --- /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 Mid360Module +from dimos.spec import perception +from dimos.spec.utils import assert_implements_protocol + + +def test_livox_module_implements_lidar_spec() -> None: + assert_implements_protocol(Mid360Module, perception.Lidar) + + +def test_livox_module_implements_imu_spec() -> None: + assert_implements_protocol(Mid360Module, perception.IMU) diff --git a/dimos/hardware/sensors/lidar/module.py b/dimos/hardware/sensors/lidar/module.py new file mode 100644 index 0000000000..660044174b --- /dev/null +++ b/dimos/hardware/sensors/lidar/module.py @@ -0,0 +1,106 @@ +# 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 collections.abc import Callable +from dataclasses import dataclass, field +import time +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.PointCloud2 import PointCloud2 +from dimos.spec import perception + + +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() + + +lidar_module = LidarModule.blueprint + +__all__ = [ + "LidarModule", + "LidarModuleConfig", + "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/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/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..7b087356ec 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,10 +646,19 @@ 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 + # 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/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/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 70cc455d34..e27d9ea3a7 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -40,6 +40,10 @@ "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-voxels": "dimos.hardware.sensors.lidar.fastlio2.fastlio_blueprints:mid360_fastlio_voxels", + "mid360-fastlio-voxels-native": "dimos.hardware.sensors.lidar.fastlio2.fastlio_blueprints:mid360_fastlio_voxels_native", "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", @@ -77,6 +81,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", @@ -88,9 +93,11 @@ "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", "llm_agent": "dimos.agents.agent", "manipulation_module": "dimos.manipulation.manipulation_module", "mapper": "dimos.robot.unitree_webrtc.type.map", + "mid360_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/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/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..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): @@ -247,6 +251,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 @@ -255,6 +264,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 +319,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 +327,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 +340,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 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={}; } diff --git a/test_livox_hw.py b/test_livox_hw.py new file mode 100644 index 0000000000..75daacff38 --- /dev/null +++ b/test_livox_hw.py @@ -0,0 +1,92 @@ +# 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 +from dimos.visualization.rerun.bridge import RerunBridgeModule + +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(3) + + lidar = dimos.deploy( + LivoxLidarModule, + 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) + listener.pointcloud.transport = LCMTransport("/lidar/pointcloud", PointCloud2) + listener.imu.transport = LCMTransport("/lidar/imu", Imu) + + lidar.start() + listener.start() + bridge.start() + + print("LivoxLidarModule + listener running. Ctrl+C to stop.", flush=True) + try: + while True: + time.sleep(1) + except KeyboardInterrupt: + pass + + dimos.stop()