diff --git a/.gitignore b/.gitignore index 44966af366..24a3dd8919 100644 --- a/.gitignore +++ b/.gitignore @@ -65,6 +65,7 @@ yolo11n.pt *mobileclip* /results +**/cpp/result CLAUDE.MD /assets/teleop_certs/ diff --git a/dimos/core/native_module.py b/dimos/core/native_module.py new file mode 100644 index 0000000000..6a93e6453a --- /dev/null +++ b/dimos/core/native_module.py @@ -0,0 +1,296 @@ +# 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 inspect +import json +import os +from pathlib import Path +import signal +import subprocess +import threading +from typing import IO, Any + +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 + build_command: str | None = None + cwd: str | None = None + extra_args: list[str] = field(default_factory=list) + extra_env: dict[str, str] = field(default_factory=dict) + shutdown_timeout: float = 10.0 + log_format: LogFormat = LogFormat.TEXT + + # 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``. + """ + ignore_fields = {f.name for f in fields(NativeModuleConfig)} + args: list[str] = [] + for f in fields(self): + if f.name in ignore_fields: + 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[NativeModuleConfig]): + """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 + _process: subprocess.Popen[bytes] | None = None + _watchdog: threading.Thread | None = None + _stopping: bool = False + + def __init__(self, *args: Any, **kwargs: Any) -> None: + super().__init__(*args, **kwargs) + self._resolve_paths() + + @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 + + self._maybe_build() + + topics = self._collect_topics() + + cmd = [self.config.executable] + for name, topic_str in topics.items(): + cmd.extend([f"--{name}", topic_str]) + cmd.extend(self.config.to_cli_args()) + cmd.extend(self.config.extra_args) + + env = {**os.environ, **self.config.extra_env} + cwd = self.config.cwd or str(Path(self.config.executable).resolve().parent) + + 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._stopping = False + self._watchdog = threading.Thread(target=self._watch_process, daemon=True) + self._watchdog.start() + + @rpc + def stop(self) -> None: + self._stopping = True + 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) + if self._watchdog is not None and self._watchdog is not threading.current_thread(): + self._watchdog.join(timeout=2) + self._watchdog = None + self._process = None + super().stop() + + def _watch_process(self) -> None: + """Block until the native process exits; trigger stop() if it crashed.""" + if self._process is None: + return + + stdout_t = self._start_reader(self._process.stdout, "info") + stderr_t = self._start_reader(self._process.stderr, "warning") + rc = self._process.wait() + stdout_t.join(timeout=2) + stderr_t.join(timeout=2) + + if self._stopping: + return + logger.error( + "Native process died unexpectedly", + pid=self._process.pid, + returncode=rc, + ) + self.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): + logger.warning("malformed JSON from native module", raw=line) + log_fn(line, pid=self._process.pid if self._process else None) + stream.close() + + def _resolve_paths(self) -> None: + """Resolve relative ``cwd`` and ``executable`` against the subclass's source file.""" + if self.config.cwd is not None and not Path(self.config.cwd).is_absolute(): + source_file = inspect.getfile(type(self)) + base_dir = Path(source_file).resolve().parent + self.config.cwd = str(base_dir / self.config.cwd) + if not Path(self.config.executable).is_absolute() and self.config.cwd is not None: + self.config.executable = str(Path(self.config.cwd) / self.config.executable) + + def _maybe_build(self) -> None: + """Run ``build_command`` if the executable does not exist.""" + exe = Path(self.config.executable) + if exe.exists(): + return + if self.config.build_command is None: + raise FileNotFoundError( + f"Executable not found: {exe}. " + "Set build_command in config to auto-build, or build it manually." + ) + logger.info( + "Executable not found, running build", + executable=str(exe), + build_command=self.config.build_command, + ) + proc = subprocess.Popen( + self.config.build_command, + shell=True, + cwd=self.config.cwd, + env={**os.environ, **self.config.extra_env}, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + ) + stdout, stderr = proc.communicate() + for line in stdout.decode("utf-8", errors="replace").splitlines(): + if line.strip(): + logger.info(line) + for line in stderr.decode("utf-8", errors="replace").splitlines(): + if line.strip(): + logger.warning(line) + if proc.returncode != 0: + raise RuntimeError( + f"Build command failed (exit {proc.returncode}): {self.config.build_command}" + ) + if not exe.exists(): + raise FileNotFoundError( + f"Build command succeeded but executable still not found: {exe}" + ) + + 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 + + +__all__ = [ + "LogFormat", + "NativeModule", + "NativeModuleConfig", +] diff --git a/dimos/core/test_native_module.py b/dimos/core/test_native_module.py new file mode 100644 index 0000000000..8af63b0bf4 --- /dev/null +++ b/dimos/core/test_native_module.py @@ -0,0 +1,176 @@ +# 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 +from pathlib import Path +import time + +import pytest + +from dimos.core import DimosCluster +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 / "tests" / "native_echo.py") + + +@pytest.fixture +def args_file(tmp_path: Path) -> str: + """Temp file path where native_echo.py writes the CLI args it received.""" + return str(tmp_path / "native_echo_args.json") + + +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 + + +@dataclass(kw_only=True) +class StubNativeConfig(NativeModuleConfig): + executable: str = _ECHO + log_format: LogFormat = LogFormat.TEXT + output_file: str | None = None + die_after: float | None = None + some_param: float = 1.5 + + +class StubNativeModule(NativeModule): + default_config = StubNativeConfig + pointcloud: Out[PointCloud2] + imu: Out[Imu] + cmd_vel: In[Twist] + + +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_process_crash_triggers_stop() -> None: + """When the native process dies unexpectedly, the watchdog calls stop().""" + mod = StubNativeModule(die_after=0.2) + mod.pointcloud.transport = LCMTransport("/pc", PointCloud2) + mod.start() + + assert mod._process is not None + pid = mod._process.pid + + # Wait for the process to die and the watchdog to call stop() + for _ in range(30): + time.sleep(0.1) + if mod._process is None: + break + + assert mod._process is None, f"Watchdog did not clean up after process {pid} died" + + +def test_manual(dimos_cluster: DimosCluster, args_file: str) -> None: + native_module = dimos_cluster.deploy( # type: ignore[attr-defined] + StubNativeModule, + some_param=2.5, + output_file=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", + "output_file": args_file, + "some_param": "2.5", + } + + +@pytest.mark.heavy +def test_autoconnect(args_file: str) -> None: + """autoconnect passes correct topic args to the native subprocess.""" + blueprint = autoconnect( + StubNativeModule.blueprint( + some_param=2.5, + output_file=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) # type: ignore[type-var] + 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.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", + "output_file": args_file, + "some_param": "2.5", + } diff --git a/dimos/core/tests/native_echo.py b/dimos/core/tests/native_echo.py new file mode 100755 index 0000000000..6723b0b0d1 --- /dev/null +++ b/dimos/core/tests/native_echo.py @@ -0,0 +1,48 @@ +#!/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. + +Parses --output_file and --die_after from CLI args, writes remaining +args as JSON to the output file, then waits for SIGTERM. +""" + +import argparse +import json +import signal +import sys +import time + +print("this message goes to stdout") +print("this message goes to stderr", file=sys.stderr) + +signal.signal(signal.SIGTERM, lambda *_: sys.exit(0)) + +parser = argparse.ArgumentParser() +parser.add_argument("--output_file", default=None) +parser.add_argument("--die_after", type=float, default=None) +args, _ = parser.parse_known_args() + +if args.output_file: + with open(args.output_file, "w") as f: + json.dump(sys.argv[1:], f) + +print("my args:", json.dumps(sys.argv[1:])) + +if args.die_after is not None: + time.sleep(args.die_after) + sys.exit(42) + +signal.pause() 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/common/dimos_native_module.hpp b/dimos/hardware/sensors/lidar/common/dimos_native_module.hpp new file mode 100644 index 0000000000..cdd5d85914 --- /dev/null +++ b/dimos/hardware/sensors/lidar/common/dimos_native_module.hpp @@ -0,0 +1,86 @@ +// 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 +#include + +#include "std_msgs/Header.hpp" +#include "std_msgs/Time.hpp" + +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_; +}; + +/// Convert seconds (double) to a ROS-style Time message. +inline 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; +} + +/// Build a stamped Header with auto-incrementing sequence number. +inline 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; +} + +} // namespace dimos diff --git a/dimos/hardware/sensors/lidar/common/livox_sdk_config.hpp b/dimos/hardware/sensors/lidar/common/livox_sdk_config.hpp new file mode 100644 index 0000000000..d7101c850e --- /dev/null +++ b/dimos/hardware/sensors/lidar/common/livox_sdk_config.hpp @@ -0,0 +1,116 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 +// +// Shared Livox SDK2 configuration utilities for dimos native modules. +// Used by both mid360_native and fastlio2_native. + +#pragma once + +#include +#include + +#include +#include + +#include +#include +#include + +namespace livox_common { + +// Gravity constant for converting accelerometer data from g to m/s^2 +inline constexpr double GRAVITY_MS2 = 9.80665; + +// Livox data_type values (not provided as named constants in SDK2 header) +inline constexpr uint8_t DATA_TYPE_IMU = 0x00; +inline constexpr uint8_t DATA_TYPE_CARTESIAN_HIGH = 0x01; +inline constexpr uint8_t DATA_TYPE_CARTESIAN_LOW = 0x02; + +// SDK network port configuration for Livox Mid-360 +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; +}; + +// Write Livox SDK JSON config to an in-memory file (memfd_create). +// Returns {fd, path} — caller must close(fd) after LivoxLidarSdkInit reads it. +inline std::pair write_sdk_config(const std::string& host_ip, + const std::string& lidar_ip, + const SdkPorts& ports) { + int fd = memfd_create("livox_sdk_config", 0); + if (fd < 0) { + perror("memfd_create"); + return {-1, ""}; + } + + FILE* fp = fdopen(fd, "w"); + if (!fp) { + perror("fdopen"); + close(fd); + return {-1, ""}; + } + + 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); + fflush(fp); // flush but don't fclose — that would close fd + + char path[64]; + snprintf(path, sizeof(path), "/proc/self/fd/%d", fd); + return {fd, path}; +} + +// Initialize Livox SDK from in-memory config. +// Returns true on success. Handles fd lifecycle internally. +inline bool init_livox_sdk(const std::string& host_ip, + const std::string& lidar_ip, + const SdkPorts& ports) { + auto [fd, path] = write_sdk_config(host_ip, lidar_ip, ports); + if (fd < 0) { + fprintf(stderr, "Error: failed to write SDK config\n"); + return false; + } + + bool ok = LivoxLidarSdkInit(path.c_str(), host_ip.c_str()); + close(fd); + + if (!ok) { + fprintf(stderr, "Error: LivoxLidarSdkInit failed\n"); + } + return ok; +} + +} // namespace livox_common 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/config/avia.yaml b/dimos/hardware/sensors/lidar/fastlio2/config/avia.yaml new file mode 100644 index 0000000000..8447b64658 --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/config/avia.yaml @@ -0,0 +1,35 @@ +common: + lid_topic: "/livox/lidar" + imu_topic: "/livox/imu" + time_sync_en: false # ONLY turn on when external time synchronization is really not possible + time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). + # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 + +preprocess: + lidar_type: 1 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, + scan_line: 6 + blind: 4 + +mapping: + acc_cov: 0.1 + gyr_cov: 0.1 + b_acc_cov: 0.0001 + b_gyr_cov: 0.0001 + fov_degree: 90 + det_range: 450.0 + extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic + extrinsic_T: [ 0.04165, 0.02326, -0.0284 ] + extrinsic_R: [ 1, 0, 0, + 0, 1, 0, + 0, 0, 1] + +publish: + path_en: false + scan_publish_en: true # false: close all the point cloud output + dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. + scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame + +pcd_save: + pcd_save_en: true + interval: -1 # how many LiDAR frames saved in each pcd file; + # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. diff --git a/dimos/hardware/sensors/lidar/fastlio2/config/horizon.yaml b/dimos/hardware/sensors/lidar/fastlio2/config/horizon.yaml new file mode 100644 index 0000000000..43db0c3bff --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/config/horizon.yaml @@ -0,0 +1,35 @@ +common: + lid_topic: "/livox/lidar" + imu_topic: "/livox/imu" + time_sync_en: false # ONLY turn on when external time synchronization is really not possible + time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). + # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 + +preprocess: + lidar_type: 1 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, + scan_line: 6 + blind: 4 + +mapping: + acc_cov: 0.1 + gyr_cov: 0.1 + b_acc_cov: 0.0001 + b_gyr_cov: 0.0001 + fov_degree: 100 + det_range: 260.0 + extrinsic_est_en: true # true: enable the online estimation of IMU-LiDAR extrinsic + extrinsic_T: [ 0.05512, 0.02226, -0.0297 ] + extrinsic_R: [ 1, 0, 0, + 0, 1, 0, + 0, 0, 1] + +publish: + path_en: false + scan_publish_en: true # false: close all the point cloud output + dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. + scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame + +pcd_save: + pcd_save_en: true + interval: -1 # how many LiDAR frames saved in each pcd file; + # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. diff --git a/dimos/hardware/sensors/lidar/fastlio2/config/marsim.yaml b/dimos/hardware/sensors/lidar/fastlio2/config/marsim.yaml new file mode 100644 index 0000000000..ad6c89121a --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/config/marsim.yaml @@ -0,0 +1,35 @@ +common: + lid_topic: "/quad0_pcl_render_node/sensor_cloud" + imu_topic: "/quad_0/imu" + time_sync_en: false # ONLY turn on when external time synchronization is really not possible + time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). + # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 + +preprocess: + lidar_type: 4 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, + scan_line: 4 + blind: 0.5 + +mapping: + acc_cov: 0.1 + gyr_cov: 0.1 + b_acc_cov: 0.0001 + b_gyr_cov: 0.0001 + fov_degree: 90 + det_range: 50.0 + extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic + extrinsic_T: [ -0.0, -0.0, 0.0 ] + extrinsic_R: [ 1, 0, 0, + 0, 1, 0, + 0, 0, 1] + +publish: + path_en: false + scan_publish_en: true # false: close all the point cloud output + dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. + scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame + +pcd_save: + pcd_save_en: true + interval: -1 # how many LiDAR frames saved in each pcd file; + # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. diff --git a/dimos/hardware/sensors/lidar/fastlio2/config/mid360.yaml b/dimos/hardware/sensors/lidar/fastlio2/config/mid360.yaml new file mode 100644 index 0000000000..512047ee48 --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/config/mid360.yaml @@ -0,0 +1,35 @@ +common: + lid_topic: "/livox/lidar" + imu_topic: "/livox/imu" + time_sync_en: false # ONLY turn on when external time synchronization is really not possible + time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). + # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 + +preprocess: + lidar_type: 1 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, + scan_line: 4 + blind: 0.5 + +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: false # true: enable the online estimation of IMU-LiDAR extrinsic + extrinsic_T: [ -0.011, -0.02329, 0.04412 ] + extrinsic_R: [ 1, 0, 0, + 0, 1, 0, + 0, 0, 1] + +publish: + path_en: false + scan_publish_en: true # false: close all the point cloud output + dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. + scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame + +pcd_save: + pcd_save_en: true + interval: -1 # how many LiDAR frames saved in each pcd file; + # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. diff --git a/dimos/hardware/sensors/lidar/fastlio2/config/ouster64.yaml b/dimos/hardware/sensors/lidar/fastlio2/config/ouster64.yaml new file mode 100644 index 0000000000..9d891bbeba --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/config/ouster64.yaml @@ -0,0 +1,36 @@ +common: + lid_topic: "/os_cloud_node/points" + imu_topic: "/os_cloud_node/imu" + time_sync_en: false # ONLY turn on when external time synchronization is really not possible + time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). + # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 + +preprocess: + lidar_type: 3 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, + scan_line: 64 + timestamp_unit: 3 # 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. + blind: 4 + +mapping: + acc_cov: 0.1 + gyr_cov: 0.1 + b_acc_cov: 0.0001 + b_gyr_cov: 0.0001 + fov_degree: 180 + det_range: 150.0 + extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic + extrinsic_T: [ 0.0, 0.0, 0.0 ] + extrinsic_R: [1, 0, 0, + 0, 1, 0, + 0, 0, 1] + +publish: + path_en: false + scan_publish_en: true # false: close all the point cloud output + dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. + scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame + +pcd_save: + pcd_save_en: true + interval: -1 # how many LiDAR frames saved in each pcd file; + # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. diff --git a/dimos/hardware/sensors/lidar/fastlio2/config/velodyne.yaml b/dimos/hardware/sensors/lidar/fastlio2/config/velodyne.yaml new file mode 100644 index 0000000000..450eda48b8 --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/config/velodyne.yaml @@ -0,0 +1,37 @@ +common: + lid_topic: "/velodyne_points" + imu_topic: "/imu/data" + time_sync_en: false # ONLY turn on when external time synchronization is really not possible + time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). + # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 + +preprocess: + lidar_type: 2 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, + scan_line: 32 + scan_rate: 10 # only need to be set for velodyne, unit: Hz, + timestamp_unit: 2 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. + blind: 2 + +mapping: + acc_cov: 0.1 + gyr_cov: 0.1 + b_acc_cov: 0.0001 + b_gyr_cov: 0.0001 + fov_degree: 180 + det_range: 100.0 + extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic, + extrinsic_T: [ 0, 0, 0.28] + extrinsic_R: [ 1, 0, 0, + 0, 1, 0, + 0, 0, 1] + +publish: + path_en: false + scan_publish_en: true # false: close all the point cloud output + dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. + scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame + +pcd_save: + pcd_save_en: true + interval: -1 # how many LiDAR frames saved in each pcd file; + # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. 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..39f9f90443 --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt @@ -0,0 +1,117 @@ +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") + +if(CMAKE_INSTALL_PREFIX_INITIALIZED_TO_DEFAULT) + set(CMAKE_INSTALL_PREFIX "${CMAKE_SOURCE_DIR}/result" CACHE PATH "" FORCE) +endif() + +# 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 (pass -DFASTLIO_DIR= or auto-fetched from GitHub) +if(NOT FASTLIO_DIR) + message(STATUS "FASTLIO_DIR not set, fetching FAST-LIO-NON-ROS from GitHub...") + FetchContent_Declare(fast_lio + GIT_REPOSITORY https://github.com/leshy/FAST-LIO-NON-ROS.git + GIT_TAG dimos-integration + GIT_SHALLOW TRUE + ) + FetchContent_MakeAvailable(fast_lio) + set(FASTLIO_DIR ${fast_lio_SOURCE_DIR}) +endif() + +# 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 (only components we need — avoid full PCL which drags in VTK via io) +find_package(PCL 1.8 REQUIRED COMPONENTS common filters) + +# yaml-cpp (FAST-LIO config parsing — standard YAML format) +find_package(yaml-cpp REQUIRED) + +# Livox SDK2 (from nix or /usr/local fallback) +find_library(LIVOX_SDK livox_lidar_sdk_shared) +if(NOT LIVOX_SDK) + message(FATAL_ERROR "Livox SDK2 not found. Available via nix flake in lidar/livox/") +endif() +get_filename_component(LIVOX_SDK_LIB_DIR ${LIVOX_SDK} DIRECTORY) +get_filename_component(LIVOX_SDK_PREFIX ${LIVOX_SDK_LIB_DIR} DIRECTORY) +set(LIVOX_SDK_INCLUDE_DIR ${LIVOX_SDK_PREFIX}/include) + +add_executable(fastlio2_native + main.cpp + ${FASTLIO_DIR}/src/preprocess.cpp + ${FASTLIO_DIR}/include/ikd-Tree/ikd_Tree.cpp +) + +# Shared Livox common headers (livox_sdk_config.hpp etc.) +if(NOT LIVOX_COMMON_DIR) + set(LIVOX_COMMON_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../common) +endif() + +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} + ${LIVOX_COMMON_DIR} + ${LIVOX_SDK_INCLUDE_DIR} +) + +target_link_libraries(fastlio2_native PRIVATE + ${LCM_LIBRARIES} + ${LIVOX_SDK} + ${PCL_LIBRARIES} + yaml-cpp::yaml-cpp +) + +if(OpenMP_CXX_FOUND) + target_link_libraries(fastlio2_native PRIVATE OpenMP::OpenMP_CXX) +endif() + +target_link_directories(fastlio2_native PRIVATE + ${LCM_LIBRARY_DIRS} +) + +install(TARGETS fastlio2_native DESTINATION bin) diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/README.md b/dimos/hardware/sensors/lidar/fastlio2/cpp/README.md new file mode 100644 index 0000000000..da6e7c8803 --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/README.md @@ -0,0 +1,109 @@ +# FAST-LIO2 Native Module (C++) + +Real-time LiDAR SLAM using FAST-LIO2 with integrated Livox Mid-360 driver. +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. + +## Build + +### Nix (recommended) + +```bash +cd dimos/hardware/sensors/lidar/fastlio2/cpp +nix build .#fastlio2_native +``` + +Binary lands at `result/bin/fastlio2_native`. + +The flake pulls Livox SDK2 from the livox sub-flake and +[FAST-LIO-NON-ROS](https://github.com/leshy/FAST-LIO-NON-ROS) from GitHub +automatically. + +### Native (CMake) + +Requires: +- CMake >= 3.14 +- [LCM](https://lcm-proj.github.io/) (`pacman -S lcm` or build from source) +- [Livox SDK2](https://github.com/Livox-SDK/Livox-SDK2) installed to `/usr/local` +- Eigen3, PCL (common, filters), yaml-cpp, Boost, OpenMP +- [FAST-LIO-NON-ROS](https://github.com/leshy/FAST-LIO-NON-ROS) checked out locally + +```bash +cd dimos/hardware/sensors/lidar/fastlio2/cpp +cmake -B build -DFASTLIO_DIR=$HOME/coding/FAST-LIO-NON-ROS +cmake --build build -j$(nproc) +cmake --install build +``` + +Binary lands at `result/bin/fastlio2_native` (same location as nix). + +If `-DFASTLIO_DIR` is omitted, CMake auto-fetches FAST-LIO-NON-ROS from GitHub. + +## Network setup + +The Mid-360 communicates over USB ethernet. Configure the interface: + +```bash +sudo nmcli con add type ethernet ifname usbeth0 con-name livox-mid360 \ + ipv4.addresses 192.168.1.5/24 ipv4.method manual +sudo nmcli con up livox-mid360 +``` + +This persists across reboots. The lidar defaults to `192.168.1.155`. + +## Usage + +Normally launched by `FastLio2` via the NativeModule framework: + +```python +from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 +from dimos.core.blueprints import autoconnect + +autoconnect( + FastLio2.blueprint(host_ip="192.168.1.5"), + SomeConsumer.blueprint(), +).build().loop() +``` + +### Manual invocation (for debugging) + +```bash +./result/bin/fastlio2_native \ + --lidar '/pointcloud#sensor_msgs.PointCloud2' \ + --odometry '/odometry#nav_msgs.Odometry' \ + --host_ip 192.168.1.5 \ + --lidar_ip 192.168.1.155 \ + --config_path ../config/mid360.yaml +``` + +Topic strings must include the `#type` suffix -- this is the actual LCM channel +name used by dimos subscribers. + +For full vis: +```sh +rerun-bridge +``` + +For LCM traffic: +```sh +lcm-spy +``` + +## Configuration + +FAST-LIO2 config files live in `config/`. The YAML config controls filter +parameters, EKF tuning, and point cloud processing settings. + +## File overview + +| File | Description | +|---------------------------|--------------------------------------------------------------| +| `main.cpp` | Livox SDK2 + FAST-LIO2 integration, EKF SLAM, LCM publishing | +| `cloud_filter.hpp` | Point cloud filtering (range, voxel downsampling) | +| `voxel_map.hpp` | Global voxel map accumulation | +| `dimos_native_module.hpp` | Reusable header for parsing NativeModule CLI args | +| `config/` | FAST-LIO2 YAML configuration files | +| `flake.nix` | Nix flake for hermetic builds | +| `CMakeLists.txt` | Build config, fetches dimos-lcm headers automatically | +| `../module.py` | Python NativeModule wrapper (`FastLio2`) | 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/flake.lock b/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.lock new file mode 100644 index 0000000000..2636f00ada --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.lock @@ -0,0 +1,135 @@ +{ + "nodes": { + "dimos-lcm": { + "flake": false, + "locked": { + "lastModified": 1769774949, + "narHash": "sha256-icRK7jerqNlwK1WZBrnIP04I2WozzFqTD7qsmnPxQuo=", + "owner": "dimensionalOS", + "repo": "dimos-lcm", + "rev": "0aa72b7b1bd3a65f50f5c03485ee9b728df56afe", + "type": "github" + }, + "original": { + "owner": "dimensionalOS", + "ref": "main", + "repo": "dimos-lcm", + "type": "github" + } + }, + "dimos-lcm_2": { + "flake": false, + "locked": { + "lastModified": 1769774949, + "narHash": "sha256-icRK7jerqNlwK1WZBrnIP04I2WozzFqTD7qsmnPxQuo=", + "owner": "dimensionalOS", + "repo": "dimos-lcm", + "rev": "0aa72b7b1bd3a65f50f5c03485ee9b728df56afe", + "type": "github" + }, + "original": { + "owner": "dimensionalOS", + "ref": "main", + "repo": "dimos-lcm", + "type": "github" + } + }, + "fast-lio": { + "flake": false, + "locked": { + "lastModified": 1770976391, + "narHash": "sha256-OjSHk6qs3oCZ7XNjDyq4/K/Rb1VhqyADtra2q3F8V5U=", + "owner": "leshy", + "repo": "FAST-LIO-NON-ROS", + "rev": "47606ac6bbafcae9231936b4662b94c84fe87339", + "type": "github" + }, + "original": { + "owner": "leshy", + "ref": "dimos-integration", + "repo": "FAST-LIO-NON-ROS", + "type": "github" + } + }, + "flake-utils": { + "inputs": { + "systems": "systems" + }, + "locked": { + "lastModified": 1731533236, + "narHash": "sha256-l0KFg5HjrsfsO/JpG+r7fRrqm12kzFHyUHqHCVpMMbI=", + "owner": "numtide", + "repo": "flake-utils", + "rev": "11707dc2f618dd54ca8739b309ec4fc024de578b", + "type": "github" + }, + "original": { + "owner": "numtide", + "repo": "flake-utils", + "type": "github" + } + }, + "livox-sdk": { + "inputs": { + "dimos-lcm": "dimos-lcm_2", + "flake-utils": [ + "flake-utils" + ], + "nixpkgs": [ + "nixpkgs" + ] + }, + "locked": { + "path": "../../livox/cpp", + "type": "path" + }, + "original": { + "path": "../../livox/cpp", + "type": "path" + }, + "parent": [] + }, + "nixpkgs": { + "locked": { + "lastModified": 1770841267, + "narHash": "sha256-9xejG0KoqsoKEGp2kVbXRlEYtFFcDTHjidiuX8hGO44=", + "owner": "NixOS", + "repo": "nixpkgs", + "rev": "ec7c70d12ce2fc37cb92aff673dcdca89d187bae", + "type": "github" + }, + "original": { + "owner": "NixOS", + "ref": "nixos-unstable", + "repo": "nixpkgs", + "type": "github" + } + }, + "root": { + "inputs": { + "dimos-lcm": "dimos-lcm", + "fast-lio": "fast-lio", + "flake-utils": "flake-utils", + "livox-sdk": "livox-sdk", + "nixpkgs": "nixpkgs" + } + }, + "systems": { + "locked": { + "lastModified": 1681028828, + "narHash": "sha256-Vy1rq5AaRuLzOxct8nz4T6wlgyUR7zLU309k9mBC768=", + "owner": "nix-systems", + "repo": "default", + "rev": "da67096a3b9bf56a91d16901293e51ba5b49a27e", + "type": "github" + }, + "original": { + "owner": "nix-systems", + "repo": "default", + "type": "github" + } + } + }, + "root": "root", + "version": 7 +} diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix b/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix new file mode 100644 index 0000000000..7a58aceb76 --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix @@ -0,0 +1,59 @@ +{ + description = "FAST-LIO2 + Livox Mid-360 native module"; + + inputs = { + nixpkgs.url = "github:NixOS/nixpkgs/nixos-unstable"; + flake-utils.url = "github:numtide/flake-utils"; + livox-sdk.url = "path:../../livox/cpp"; + livox-sdk.inputs.nixpkgs.follows = "nixpkgs"; + livox-sdk.inputs.flake-utils.follows = "flake-utils"; + dimos-lcm = { + url = "github:dimensionalOS/dimos-lcm/main"; + flake = false; + }; + fast-lio = { + url = "github:leshy/FAST-LIO-NON-ROS/dimos-integration"; + flake = false; + }; + }; + + outputs = { self, nixpkgs, flake-utils, livox-sdk, dimos-lcm, fast-lio, ... }: + flake-utils.lib.eachDefaultSystem (system: + let + pkgs = import nixpkgs { inherit system; }; + livox-sdk2 = livox-sdk.packages.${system}.livox-sdk2; + + livox-common = ../../common; + + fastlio2_native = pkgs.stdenv.mkDerivation { + pname = "fastlio2_native"; + version = "0.1.0"; + + src = ./.; + + nativeBuildInputs = [ pkgs.cmake pkgs.pkg-config ]; + buildInputs = [ + livox-sdk2 + pkgs.lcm + pkgs.glib + pkgs.eigen + pkgs.pcl + pkgs.yaml-cpp + pkgs.boost + pkgs.llvmPackages.openmp + ]; + + cmakeFlags = [ + "-DCMAKE_POLICY_VERSION_MINIMUM=3.5" + "-DFETCHCONTENT_SOURCE_DIR_DIMOS_LCM=${dimos-lcm}" + "-DFASTLIO_DIR=${fast-lio}" + "-DLIVOX_COMMON_DIR=${livox-common}" + ]; + }; + in { + packages = { + default = fastlio2_native; + inherit fastlio2_native; + }; + }); +} 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..60b8d9cdb2 --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp @@ -0,0 +1,522 @@ +// 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.yaml \ +// --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 "livox_sdk_config.hpp" + +#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" + +// FAST-LIO (header-only core, compiled sources linked via CMake) +#include "fast_lio.hpp" + +using livox_common::GRAVITY_MS2; +using livox_common::DATA_TYPE_IMU; +using livox_common::DATA_TYPE_CARTESIAN_HIGH; +using livox_common::DATA_TYPE_CARTESIAN_LOW; + +// --------------------------------------------------------------------------- +// 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; + +// --------------------------------------------------------------------------- +// Helpers +// --------------------------------------------------------------------------- + +static uint64_t get_timestamp_ns(const LivoxLidarEthernetPacket* pkt) { + uint64_t ns = 0; + std::memcpy(&ns, pkt->timestamp, sizeof(uint64_t)); + return ns; +} + +using dimos::time_from_seconds; +using dimos::make_header; + +// --------------------------------------------------------------------------- +// 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); +} + +// --------------------------------------------------------------------------- +// 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; + } + + // FAST-LIO internal processing rates + double msr_freq = mod.arg_float("msr_freq", 50.0f); + double main_freq = mod.arg_float("main_freq", 5000.0f); + + // 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 (defaults from SdkPorts struct in livox_sdk_config.hpp) + livox_common::SdkPorts ports; + const livox_common::SdkPorts port_defaults; + ports.cmd_data = mod.arg_int("cmd_data_port", port_defaults.cmd_data); + ports.push_msg = mod.arg_int("push_msg_port", port_defaults.push_msg); + ports.point_data = mod.arg_int("point_data_port", port_defaults.point_data); + ports.imu_data = mod.arg_int("imu_data_port", port_defaults.imu_data); + ports.log_data = mod.arg_int("log_data_port", port_defaults.log_data); + ports.host_cmd_data = mod.arg_int("host_cmd_data_port", port_defaults.host_cmd_data); + ports.host_push_msg = mod.arg_int("host_push_msg_port", port_defaults.host_push_msg); + ports.host_point_data = mod.arg_int("host_point_data_port", port_defaults.host_point_data); + ports.host_imu_data = mod.arg_int("host_imu_data_port", port_defaults.host_imu_data); + ports.host_log_data = mod.arg_int("host_log_data_port", port_defaults.host_log_data); + + 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, msr_freq, main_freq); + g_fastlio = &fast_lio; + printf("[fastlio2] FAST-LIO initialized.\n"); + + // Init Livox SDK (in-memory config, no temp files) + if (!livox_common::init_livox_sdk(host_ip, lidar_ip, ports)) { + 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(); + 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 / main_freq; + + // 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(); + g_lcm = nullptr; + + printf("[fastlio2] Done.\n"); + return 0; +} diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/voxel_map.hpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/voxel_map.hpp new file mode 100644 index 0000000000..a50740cd04 --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/voxel_map.hpp @@ -0,0 +1,297 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 +// +// Efficient global voxel map using a hash map. +// Supports O(1) insert/update, distance-based pruning, and +// raycasting-based free space clearing via Amanatides & Woo 3D DDA. +// FOV is discovered dynamically from incoming point cloud data. + +#ifndef VOXEL_MAP_HPP_ +#define VOXEL_MAP_HPP_ + +#include +#include +#include + +#include +#include + +struct VoxelKey { + int32_t x, y, z; + bool operator==(const VoxelKey& o) const { return x == o.x && y == o.y && z == o.z; } +}; + +struct VoxelKeyHash { + size_t operator()(const VoxelKey& k) const { + // Fast spatial hash — large primes reduce collisions for grid coords + size_t h = static_cast(k.x) * 73856093u; + h ^= static_cast(k.y) * 19349669u; + h ^= static_cast(k.z) * 83492791u; + return h; + } +}; + +struct Voxel { + float x, y, z; // running centroid + float intensity; + uint32_t count; // points merged into this voxel + uint8_t miss_count; // consecutive scans where a ray passed through without hitting +}; + +/// Config for raycast-based free space clearing. +struct RaycastConfig { + int subsample = 4; // raycast every Nth point + int max_misses = 3; // erase after this many consecutive misses + float fov_margin_rad = 0.035f; // ~2° safety margin added to discovered FOV +}; + +class VoxelMap { +public: + explicit VoxelMap(float voxel_size, float max_range = 100.0f) + : voxel_size_(voxel_size), max_range_(max_range) { + map_.reserve(500000); + } + + /// Insert a point cloud into the map, merging into existing voxels. + /// Resets miss_count for hit voxels. + template + void insert(const typename pcl::PointCloud::Ptr& cloud) { + if (!cloud) return; + float inv = 1.0f / voxel_size_; + for (const auto& pt : cloud->points) { + VoxelKey key{ + static_cast(std::floor(pt.x * inv)), + static_cast(std::floor(pt.y * inv)), + static_cast(std::floor(pt.z * inv))}; + + auto it = map_.find(key); + if (it != map_.end()) { + // Running average update + auto& v = it->second; + float n = static_cast(v.count); + float n1 = n + 1.0f; + v.x = (v.x * n + pt.x) / n1; + v.y = (v.y * n + pt.y) / n1; + v.z = (v.z * n + pt.z) / n1; + v.intensity = (v.intensity * n + pt.intensity) / n1; + v.count++; + v.miss_count = 0; + } else { + map_.emplace(key, Voxel{pt.x, pt.y, pt.z, pt.intensity, 1, 0}); + } + } + } + + /// Cast rays from sensor origin through each point in the cloud. + /// Discovers the sensor FOV from the cloud's elevation angle range, + /// then marks intermediate voxels as missed and erases those exceeding + /// the miss threshold within the discovered FOV. + /// + /// Orientation quaternion (qx,qy,qz,qw) is body→world. + template + void raycast_clear(float ox, float oy, float oz, + float qx, float qy, float qz, float qw, + const typename pcl::PointCloud::Ptr& cloud, + const RaycastConfig& cfg) { + if (!cloud || cloud->empty() || cfg.max_misses <= 0) return; + + // Phase 0: discover FOV from this scan's elevation angles in sensor-local frame + update_fov(ox, oy, oz, qx, qy, qz, qw, cloud); + + // Skip raycasting until we have a valid FOV (need at least a few scans) + if (!fov_valid_) return; + + float inv = 1.0f / voxel_size_; + int n_pts = static_cast(cloud->size()); + float fov_up = fov_up_ + cfg.fov_margin_rad; + float fov_down = fov_down_ - cfg.fov_margin_rad; + + // Phase 1: walk rays, increment miss_count for intermediate voxels + for (int i = 0; i < n_pts; i += cfg.subsample) { + const auto& pt = cloud->points[i]; + raycast_single(ox, oy, oz, pt.x, pt.y, pt.z, inv); + } + + // Phase 2: erase voxels that exceeded miss threshold and are within FOV + for (auto it = map_.begin(); it != map_.end();) { + if (it->second.miss_count > static_cast(cfg.max_misses)) { + if (in_sensor_fov(ox, oy, oz, qx, qy, qz, qw, + it->second.x, it->second.y, it->second.z, + fov_up, fov_down)) { + it = map_.erase(it); + continue; + } + } + ++it; + } + } + + /// Remove voxels farther than max_range from the given position. + void prune(float px, float py, float pz) { + float r2 = max_range_ * max_range_; + for (auto it = map_.begin(); it != map_.end();) { + float dx = it->second.x - px; + float dy = it->second.y - py; + float dz = it->second.z - pz; + if (dx * dx + dy * dy + dz * dz > r2) + it = map_.erase(it); + else + ++it; + } + } + + /// Export all voxel centroids as a point cloud. + template + typename pcl::PointCloud::Ptr to_cloud() const { + typename pcl::PointCloud::Ptr cloud( + new pcl::PointCloud(map_.size(), 1)); + size_t i = 0; + for (const auto& [key, v] : map_) { + auto& pt = cloud->points[i++]; + pt.x = v.x; + pt.y = v.y; + pt.z = v.z; + pt.intensity = v.intensity; + } + return cloud; + } + + size_t size() const { return map_.size(); } + void clear() { map_.clear(); } + void set_max_range(float r) { max_range_ = r; } + float fov_up_deg() const { return fov_up_ * 180.0f / static_cast(M_PI); } + float fov_down_deg() const { return fov_down_ * 180.0f / static_cast(M_PI); } + bool fov_valid() const { return fov_valid_; } + +private: + std::unordered_map map_; + float voxel_size_; + float max_range_; + + // Dynamically discovered sensor FOV (accumulated over scans) + float fov_up_ = -static_cast(M_PI); // start narrow, expand from data + float fov_down_ = static_cast(M_PI); + int fov_scan_count_ = 0; + bool fov_valid_ = false; + static constexpr int FOV_WARMUP_SCANS = 5; // require N scans before trusting FOV + + /// Update discovered FOV from a scan's elevation angles in sensor-local frame. + template + void update_fov(float ox, float oy, float oz, + float qx, float qy, float qz, float qw, + const typename pcl::PointCloud::Ptr& cloud) { + // Inverse quaternion for world→sensor rotation + float nqx = -qx, nqy = -qy, nqz = -qz; + + for (const auto& pt : cloud->points) { + float wx = pt.x - ox, wy = pt.y - oy, wz = pt.z - oz; + + // Rotate to sensor-local frame + float tx = 2.0f * (nqy * wz - nqz * wy); + float ty = 2.0f * (nqz * wx - nqx * wz); + float tz = 2.0f * (nqx * wy - nqy * wx); + float lx = wx + qw * tx + (nqy * tz - nqz * ty); + float ly = wy + qw * ty + (nqz * tx - nqx * tz); + float lz = wz + qw * tz + (nqx * ty - nqy * tx); + + float horiz_dist = std::sqrt(lx * lx + ly * ly); + if (horiz_dist < 1e-6f) continue; + float elevation = std::atan2(lz, horiz_dist); + + if (elevation > fov_up_) fov_up_ = elevation; + if (elevation < fov_down_) fov_down_ = elevation; + } + + if (++fov_scan_count_ >= FOV_WARMUP_SCANS && !fov_valid_) { + fov_valid_ = true; + printf("[voxel_map] FOV discovered: [%.1f, %.1f] deg\n", + fov_down_deg(), fov_up_deg()); + } + } + + /// Amanatides & Woo 3D DDA: walk from (ox,oy,oz) to (px,py,pz), + /// incrementing miss_count for all intermediate voxels. + void raycast_single(float ox, float oy, float oz, + float px, float py, float pz, float inv) { + float dx = px - ox, dy = py - oy, dz = pz - oz; + float len = std::sqrt(dx * dx + dy * dy + dz * dz); + if (len < 1e-6f) return; + dx /= len; dy /= len; dz /= len; + + int32_t cx = static_cast(std::floor(ox * inv)); + int32_t cy = static_cast(std::floor(oy * inv)); + int32_t cz = static_cast(std::floor(oz * inv)); + int32_t ex = static_cast(std::floor(px * inv)); + int32_t ey = static_cast(std::floor(py * inv)); + int32_t ez = static_cast(std::floor(pz * inv)); + + int sx = (dx >= 0) ? 1 : -1; + int sy = (dy >= 0) ? 1 : -1; + int sz = (dz >= 0) ? 1 : -1; + + // tMax: parametric distance along ray to next voxel boundary per axis + // tDelta: parametric distance to cross one full voxel per axis + float tMaxX = (std::abs(dx) < 1e-10f) ? 1e30f + : (((dx > 0 ? cx + 1 : cx) * voxel_size_ - ox) / dx); + float tMaxY = (std::abs(dy) < 1e-10f) ? 1e30f + : (((dy > 0 ? cy + 1 : cy) * voxel_size_ - oy) / dy); + float tMaxZ = (std::abs(dz) < 1e-10f) ? 1e30f + : (((dz > 0 ? cz + 1 : cz) * voxel_size_ - oz) / dz); + + float tDeltaX = (std::abs(dx) < 1e-10f) ? 1e30f : std::abs(voxel_size_ / dx); + float tDeltaY = (std::abs(dy) < 1e-10f) ? 1e30f : std::abs(voxel_size_ / dy); + float tDeltaZ = (std::abs(dz) < 1e-10f) ? 1e30f : std::abs(voxel_size_ / dz); + + // Walk through voxels (skip endpoint — it was hit) + int max_steps = static_cast(len * inv) + 3; // safety bound + for (int step = 0; step < max_steps; ++step) { + if (cx == ex && cy == ey && cz == ez) break; // reached endpoint + + VoxelKey key{cx, cy, cz}; + auto it = map_.find(key); + if (it != map_.end() && it->second.miss_count < 255) { + it->second.miss_count++; + } + + // Step to next voxel on the axis with smallest tMax + if (tMaxX < tMaxY && tMaxX < tMaxZ) { + cx += sx; tMaxX += tDeltaX; + } else if (tMaxY < tMaxZ) { + cy += sy; tMaxY += tDeltaY; + } else { + cz += sz; tMaxZ += tDeltaZ; + } + } + } + + /// Check if a voxel centroid falls within the sensor's vertical FOV. + /// Rotates the vector (sensor→voxel) into sensor-local frame using the + /// inverse of the body→world quaternion, then checks elevation angle. + static bool in_sensor_fov(float ox, float oy, float oz, + float qx, float qy, float qz, float qw, + float vx, float vy, float vz, + float fov_up_rad, float fov_down_rad) { + // Vector from sensor origin to voxel in world frame + float wx = vx - ox, wy = vy - oy, wz = vz - oz; + + // Rotate by quaternion inverse (conjugate): q* = (-qx,-qy,-qz,qw) + float nqx = -qx, nqy = -qy, nqz = -qz; + // t = 2 * cross(q.xyz, v) + float tx = 2.0f * (nqy * wz - nqz * wy); + float ty = 2.0f * (nqz * wx - nqx * wz); + float tz = 2.0f * (nqx * wy - nqy * wx); + // v' = v + qw * t + cross(q.xyz, t) + float lx = wx + qw * tx + (nqy * tz - nqz * ty); + float ly = wy + qw * ty + (nqz * tx - nqx * tz); + float lz = wz + qw * tz + (nqx * ty - nqy * tx); + + // Elevation angle in sensor-local frame + float horiz_dist = std::sqrt(lx * lx + ly * ly); + if (horiz_dist < 1e-6f) return true; // directly above/below, treat as in FOV + float elevation = std::atan2(lz, horiz_dist); + + return elevation >= fov_down_rad && elevation <= fov_up_rad; + } +}; + +#endif 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..05801729e3 --- /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 = 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") + +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") diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py new file mode 100644 index 0000000000..ee9a0783a0 --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -0,0 +1,148 @@ +# 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 FastLio2 + from dimos.core.blueprints import autoconnect + + autoconnect( + FastLio2.blueprint(host_ip="192.168.1.5"), + SomeConsumer.blueprint(), + ).build().loop() +""" + +from __future__ import annotations + +from dataclasses import dataclass +from pathlib import Path +from typing import TYPE_CHECKING + +from dimos.core import Out # noqa: TC001 +from dimos.core.native_module import NativeModule, NativeModuleConfig +from dimos.hardware.sensors.lidar.livox.ports import ( + SDK_CMD_DATA_PORT, + SDK_HOST_CMD_DATA_PORT, + SDK_HOST_IMU_DATA_PORT, + SDK_HOST_LOG_DATA_PORT, + SDK_HOST_POINT_DATA_PORT, + SDK_HOST_PUSH_MSG_PORT, + SDK_IMU_DATA_PORT, + SDK_LOG_DATA_PORT, + SDK_POINT_DATA_PORT, + SDK_PUSH_MSG_PORT, +) +from dimos.msgs.nav_msgs.Odometry import Odometry # noqa: TC001 +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 # noqa: TC001 +from dimos.spec import mapping, perception + +_CONFIG_DIR = Path(__file__).parent / "config" + + +@dataclass(kw_only=True) +class FastLio2Config(NativeModuleConfig): + """Config for the FAST-LIO2 + Livox Mid-360 native module.""" + + cwd: str | None = "cpp" + executable: str = "result/bin/fastlio2_native" + build_command: str | None = "nix build .#fastlio2_native" + + # 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" + + # FAST-LIO internal processing rates + msr_freq: float = 50.0 + main_freq: float = 5000.0 + + # 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 YAML config (relative to config/ dir, or absolute path) + # C++ binary reads YAML directly via yaml-cpp + config: str = "mid360.yaml" + + # SDK port configuration (see livox/ports.py for defaults) + cmd_data_port: int = SDK_CMD_DATA_PORT + push_msg_port: int = SDK_PUSH_MSG_PORT + point_data_port: int = SDK_POINT_DATA_PORT + imu_data_port: int = SDK_IMU_DATA_PORT + log_data_port: int = SDK_LOG_DATA_PORT + host_cmd_data_port: int = SDK_HOST_CMD_DATA_PORT + host_push_msg_port: int = SDK_HOST_PUSH_MSG_PORT + host_point_data_port: int = SDK_HOST_POINT_DATA_PORT + host_imu_data_port: int = SDK_HOST_IMU_DATA_PORT + host_log_data_port: int = SDK_HOST_LOG_DATA_PORT + + # Resolved in __post_init__, passed as --config_path to the binary + config_path: str | None = None + + # config is not a CLI arg (config_path is) + cli_exclude: frozenset[str] = frozenset({"config"}) + + def __post_init__(self) -> None: + if self.config_path is None: + path = Path(self.config) + if not path.is_absolute(): + path = _CONFIG_DIR / path + self.config_path = str(path.resolve()) + + +class FastLio2(NativeModule, perception.Lidar, perception.Odometry, mapping.GlobalPointcloud): + """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] + + +fastlio2_module = FastLio2.blueprint + +__all__ = [ + "FastLio2", + "FastLio2Config", + "fastlio2_module", +] + +# Verify protocol port compliance (mypy will flag missing ports) +if TYPE_CHECKING: + FastLio2() 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..b6641a2fc6 --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/cpp/CMakeLists.txt @@ -0,0 +1,57 @@ +cmake_minimum_required(VERSION 3.14) +project(livox_mid360_native CXX) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +if(CMAKE_INSTALL_PREFIX_INITIALIZED_TO_DEFAULT) + set(CMAKE_INSTALL_PREFIX "${CMAKE_SOURCE_DIR}/result" CACHE PATH "" FORCE) +endif() + +# Fetch dimos-lcm for C++ message headers +include(FetchContent) +FetchContent_Declare(dimos_lcm + GIT_REPOSITORY https://github.com/dimensionalOS/dimos-lcm.git + GIT_TAG main + GIT_SHALLOW TRUE +) +FetchContent_MakeAvailable(dimos_lcm) + +# Find LCM +find_package(PkgConfig REQUIRED) +pkg_check_modules(LCM REQUIRED lcm) + +# Livox SDK2 (from nix or /usr/local fallback) +find_library(LIVOX_SDK livox_lidar_sdk_shared) +if(NOT LIVOX_SDK) + message(FATAL_ERROR "Livox SDK2 not found. Available via nix flake in lidar/livox/") +endif() +get_filename_component(LIVOX_SDK_LIB_DIR ${LIVOX_SDK} DIRECTORY) +get_filename_component(LIVOX_SDK_PREFIX ${LIVOX_SDK_LIB_DIR} DIRECTORY) +set(LIVOX_SDK_INCLUDE_DIR ${LIVOX_SDK_PREFIX}/include) + +add_executable(mid360_native main.cpp) + +# Shared Livox common headers (livox_sdk_config.hpp etc.) +if(NOT LIVOX_COMMON_DIR) + set(LIVOX_COMMON_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../common) +endif() + +target_include_directories(mid360_native PRIVATE + ${dimos_lcm_SOURCE_DIR}/generated/cpp_lcm_msgs + ${LCM_INCLUDE_DIRS} + ${CMAKE_CURRENT_SOURCE_DIR} + ${LIVOX_COMMON_DIR} + ${LIVOX_SDK_INCLUDE_DIR} +) + +target_link_libraries(mid360_native PRIVATE + ${LCM_LIBRARIES} + ${LIVOX_SDK} +) + +target_link_directories(mid360_native PRIVATE + ${LCM_LIBRARY_DIRS} +) + +install(TARGETS mid360_native DESTINATION bin) 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..4db5248ce1 --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/cpp/README.md @@ -0,0 +1,114 @@ +# 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. + +## Build + +### Nix (recommended) + +```bash +cd dimos/hardware/sensors/lidar/livox/cpp +nix build .#mid360_native +``` + +Binary lands at `result/bin/mid360_native`. + +To build just the Livox SDK2 library: + +```bash +nix build .#livox-sdk2 +``` + +### Native (CMake) + +Requires: +- CMake >= 3.14 +- [LCM](https://lcm-proj.github.io/) (`pacman -S lcm` or build from source) +- [Livox SDK2](https://github.com/Livox-SDK/Livox-SDK2) installed to `/usr/local` + +Installing Livox SDK2 manually: + +```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 +``` + +Then build: + +```bash +cd dimos/hardware/sensors/lidar/livox/cpp +cmake -B build +cmake --build build -j$(nproc) +cmake --install build +``` + +Binary lands at `result/bin/mid360_native` (same location as nix). + +CMake automatically fetches [dimos-lcm](https://github.com/dimensionalOS/dimos-lcm) +for the C++ message headers on first configure. + +## Network setup + +The Mid-360 communicates over USB ethernet. Configure the interface: + +```bash +sudo nmcli con add type ethernet ifname usbeth0 con-name livox-mid360 \ + ipv4.addresses 192.168.1.5/24 ipv4.method manual +sudo nmcli con up livox-mid360 +``` + +This persists across reboots. The lidar defaults to `192.168.1.155`. + +## Usage + +Normally launched by `Mid360` via the NativeModule framework: + +```python +from dimos.hardware.sensors.lidar.livox.module import Mid360 +from dimos.core.blueprints import autoconnect + +autoconnect( + Mid360.blueprint(host_ip="192.168.1.5"), + SomeConsumer.blueprint(), +).build().loop() +``` + +### Manual invocation (for debugging) + +```bash +./result/bin/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. + +View data in another terminal: + +For full vis: +```sh +rerun-bridge +``` + +For LCM traffic: +```sh +lcm-spy +``` + +## File overview + +| File | Description | +|---------------------------|----------------------------------------------------------| +| `main.cpp` | Livox SDK2 callbacks, frame accumulation, LCM publishing | +| `dimos_native_module.hpp` | Reusable header for parsing NativeModule CLI args | +| `flake.nix` | Nix flake for hermetic builds | +| `CMakeLists.txt` | Build config, fetches dimos-lcm headers automatically | +| `../module.py` | Python NativeModule wrapper (`Mid360`) | diff --git a/dimos/hardware/sensors/lidar/livox/cpp/flake.lock b/dimos/hardware/sensors/lidar/livox/cpp/flake.lock new file mode 100644 index 0000000000..58e8252be8 --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/cpp/flake.lock @@ -0,0 +1,79 @@ +{ + "nodes": { + "dimos-lcm": { + "flake": false, + "locked": { + "lastModified": 1769774949, + "narHash": "sha256-icRK7jerqNlwK1WZBrnIP04I2WozzFqTD7qsmnPxQuo=", + "owner": "dimensionalOS", + "repo": "dimos-lcm", + "rev": "0aa72b7b1bd3a65f50f5c03485ee9b728df56afe", + "type": "github" + }, + "original": { + "owner": "dimensionalOS", + "ref": "main", + "repo": "dimos-lcm", + "type": "github" + } + }, + "flake-utils": { + "inputs": { + "systems": "systems" + }, + "locked": { + "lastModified": 1731533236, + "narHash": "sha256-l0KFg5HjrsfsO/JpG+r7fRrqm12kzFHyUHqHCVpMMbI=", + "owner": "numtide", + "repo": "flake-utils", + "rev": "11707dc2f618dd54ca8739b309ec4fc024de578b", + "type": "github" + }, + "original": { + "owner": "numtide", + "repo": "flake-utils", + "type": "github" + } + }, + "nixpkgs": { + "locked": { + "lastModified": 1770841267, + "narHash": "sha256-9xejG0KoqsoKEGp2kVbXRlEYtFFcDTHjidiuX8hGO44=", + "owner": "NixOS", + "repo": "nixpkgs", + "rev": "ec7c70d12ce2fc37cb92aff673dcdca89d187bae", + "type": "github" + }, + "original": { + "owner": "NixOS", + "ref": "nixos-unstable", + "repo": "nixpkgs", + "type": "github" + } + }, + "root": { + "inputs": { + "dimos-lcm": "dimos-lcm", + "flake-utils": "flake-utils", + "nixpkgs": "nixpkgs" + } + }, + "systems": { + "locked": { + "lastModified": 1681028828, + "narHash": "sha256-Vy1rq5AaRuLzOxct8nz4T6wlgyUR7zLU309k9mBC768=", + "owner": "nix-systems", + "repo": "default", + "rev": "da67096a3b9bf56a91d16901293e51ba5b49a27e", + "type": "github" + }, + "original": { + "owner": "nix-systems", + "repo": "default", + "type": "github" + } + } + }, + "root": "root", + "version": 7 +} diff --git a/dimos/hardware/sensors/lidar/livox/cpp/flake.nix b/dimos/hardware/sensors/lidar/livox/cpp/flake.nix new file mode 100644 index 0000000000..eeb06b33a6 --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/cpp/flake.nix @@ -0,0 +1,71 @@ +{ + description = "Livox SDK2 and Mid-360 native module"; + + inputs = { + nixpkgs.url = "github:NixOS/nixpkgs/nixos-unstable"; + flake-utils.url = "github:numtide/flake-utils"; + dimos-lcm = { + url = "github:dimensionalOS/dimos-lcm/main"; + flake = false; + }; + }; + + outputs = { self, nixpkgs, flake-utils, dimos-lcm, ... }: + flake-utils.lib.eachDefaultSystem (system: + let + pkgs = import nixpkgs { inherit system; }; + + livox-sdk2 = pkgs.stdenv.mkDerivation rec { + pname = "livox-sdk2"; + version = "1.2.5"; + + src = pkgs.fetchFromGitHub { + owner = "Livox-SDK"; + repo = "Livox-SDK2"; + rev = "v${version}"; + hash = "sha256-NGscO/vLiQ17yQJtdPyFzhhMGE89AJ9kTL5cSun/bpU="; + }; + + nativeBuildInputs = [ pkgs.cmake ]; + + cmakeFlags = [ + "-DBUILD_SHARED_LIBS=ON" + "-DCMAKE_POLICY_VERSION_MINIMUM=3.5" + ]; + + preConfigure = '' + substituteInPlace CMakeLists.txt \ + --replace-fail "add_subdirectory(samples)" "" + sed -i '1i #include ' sdk_core/comm/define.h + sed -i '1i #include ' sdk_core/logger_handler/file_manager.h + ''; + }; + + livox-common = ../../common; + + mid360_native = pkgs.stdenv.mkDerivation { + pname = "mid360_native"; + version = "0.1.0"; + + src = ./.; + + nativeBuildInputs = [ pkgs.cmake pkgs.pkg-config ]; + buildInputs = [ livox-sdk2 pkgs.lcm pkgs.glib ]; + + cmakeFlags = [ + "-DCMAKE_POLICY_VERSION_MINIMUM=3.5" + "-DFETCHCONTENT_SOURCE_DIR_DIMOS_LCM=${dimos-lcm}" + "-DLIVOX_COMMON_DIR=${livox-common}" + ]; + }; + in { + packages = { + default = mid360_native; + inherit livox-sdk2 mid360_native; + }; + + devShells.default = pkgs.mkShell { + buildInputs = [ livox-sdk2 ]; + }; + }); +} 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..cdf083ef3b --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/cpp/main.cpp @@ -0,0 +1,341 @@ +// 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 --lidar --imu [--host_ip ] [--lidar_ip ] ... + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "livox_sdk_config.hpp" + +#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" + +using livox_common::GRAVITY_MS2; +using livox_common::DATA_TYPE_IMU; +using livox_common::DATA_TYPE_CARTESIAN_HIGH; +using livox_common::DATA_TYPE_CARTESIAN_LOW; + +// --------------------------------------------------------------------------- +// Global state +// --------------------------------------------------------------------------- + +static std::atomic g_running{true}; +static lcm::LCM* g_lcm = nullptr; +static std::string g_lidar_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 double get_timestamp_ns(const LivoxLidarEthernetPacket* pkt) { + uint64_t ns = 0; + std::memcpy(&ns, pkt->timestamp, sizeof(uint64_t)); + return static_cast(ns); +} + +using dimos::time_from_seconds; +using dimos::make_header; + +// --------------------------------------------------------------------------- +// 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_lidar_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); +} + +// --------------------------------------------------------------------------- +// Main +// --------------------------------------------------------------------------- + +int main(int argc, char** argv) { + dimos::NativeModule mod(argc, argv); + + // Required: LCM topics for ports + g_lidar_topic = mod.has("lidar") ? mod.topic("lidar") : ""; + g_imu_topic = mod.has("imu") ? mod.topic("imu") : ""; + + if (g_lidar_topic.empty()) { + fprintf(stderr, "Error: --lidar 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 (defaults from SdkPorts struct in livox_sdk_config.hpp) + livox_common::SdkPorts ports; + const livox_common::SdkPorts port_defaults; + ports.cmd_data = mod.arg_int("cmd_data_port", port_defaults.cmd_data); + ports.push_msg = mod.arg_int("push_msg_port", port_defaults.push_msg); + ports.point_data = mod.arg_int("point_data_port", port_defaults.point_data); + ports.imu_data = mod.arg_int("imu_data_port", port_defaults.imu_data); + ports.log_data = mod.arg_int("log_data_port", port_defaults.log_data); + ports.host_cmd_data = mod.arg_int("host_cmd_data_port", port_defaults.host_cmd_data); + ports.host_push_msg = mod.arg_int("host_push_msg_port", port_defaults.host_push_msg); + ports.host_point_data = mod.arg_int("host_point_data_port", port_defaults.host_point_data); + ports.host_imu_data = mod.arg_int("host_imu_data_port", port_defaults.host_imu_data); + ports.host_log_data = mod.arg_int("host_log_data_port", port_defaults.host_log_data); + + printf("[mid360] Starting native Livox Mid-360 module\n"); + printf("[mid360] lidar topic: %s\n", g_lidar_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; + + // Init Livox SDK (in-memory config, no temp files) + if (!livox_common::init_livox_sdk(host_ip, lidar_ip, ports)) { + 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(); + 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(); + 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..0cda912b73 --- /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 Mid360 +from dimos.visualization.rerun.bridge import rerun_bridge + +mid360 = autoconnect( + Mid360.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..672968a0eb --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/module.py @@ -0,0 +1,104 @@ +# 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. + +Usage:: + from dimos.hardware.sensors.lidar.livox.module import Mid360 + from dimos.core.blueprints import autoconnect + + autoconnect( + Mid360.blueprint(host_ip="192.168.1.5"), + SomeConsumer.blueprint(), + ).build().loop() +""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import TYPE_CHECKING + +from dimos.core import Out # noqa: TC001 +from dimos.core.native_module import NativeModule, NativeModuleConfig +from dimos.hardware.sensors.lidar.livox.ports import ( + SDK_CMD_DATA_PORT, + SDK_HOST_CMD_DATA_PORT, + SDK_HOST_IMU_DATA_PORT, + SDK_HOST_LOG_DATA_PORT, + SDK_HOST_POINT_DATA_PORT, + SDK_HOST_PUSH_MSG_PORT, + SDK_IMU_DATA_PORT, + SDK_LOG_DATA_PORT, + SDK_POINT_DATA_PORT, + SDK_PUSH_MSG_PORT, +) +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 + + +@dataclass(kw_only=True) +class Mid360Config(NativeModuleConfig): + """Config for the C++ Mid-360 native module.""" + + cwd: str | None = "cpp" + executable: str = "result/bin/mid360_native" + build_command: str | None = "nix build .#mid360_native" + + 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 (see livox/ports.py for defaults) + cmd_data_port: int = SDK_CMD_DATA_PORT + push_msg_port: int = SDK_PUSH_MSG_PORT + point_data_port: int = SDK_POINT_DATA_PORT + imu_data_port: int = SDK_IMU_DATA_PORT + log_data_port: int = SDK_LOG_DATA_PORT + host_cmd_data_port: int = SDK_HOST_CMD_DATA_PORT + host_push_msg_port: int = SDK_HOST_PUSH_MSG_PORT + host_point_data_port: int = SDK_HOST_POINT_DATA_PORT + host_imu_data_port: int = SDK_HOST_IMU_DATA_PORT + host_log_data_port: int = SDK_HOST_LOG_DATA_PORT + + +class Mid360(NativeModule, perception.Lidar, perception.IMU): + """Livox Mid-360 LiDAR module backed by a native C++ binary. + + Ports: + lidar (Out[PointCloud2]): Point cloud frames at configured frequency. + imu (Out[Imu]): IMU data at ~200 Hz (if enabled). + """ + + config: Mid360Config + default_config = Mid360Config + + lidar: Out[PointCloud2] + imu: Out[Imu] + + +mid360_module = Mid360.blueprint + +__all__ = [ + "Mid360", + "Mid360Config", + "mid360_module", +] + +# Verify protocol port compliance (mypy will flag missing ports) +if TYPE_CHECKING: + Mid360() diff --git a/dimos/hardware/sensors/lidar/livox/ports.py b/dimos/hardware/sensors/lidar/livox/ports.py new file mode 100644 index 0000000000..9ad83251d6 --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/ports.py @@ -0,0 +1,31 @@ +# 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. + +"""Default Livox SDK2 network port constants. + +These match the defaults in ``common/livox_sdk_config.hpp`` (``SdkPorts``). +Both the Mid-360 driver and FAST-LIO2 modules reference this single source +so port numbers are defined in one place on the Python side. +""" + +SDK_CMD_DATA_PORT = 56100 +SDK_PUSH_MSG_PORT = 56200 +SDK_POINT_DATA_PORT = 56300 +SDK_IMU_DATA_PORT = 56400 +SDK_LOG_DATA_PORT = 56500 +SDK_HOST_CMD_DATA_PORT = 56101 +SDK_HOST_PUSH_MSG_PORT = 56201 +SDK_HOST_POINT_DATA_PORT = 56301 +SDK_HOST_IMU_DATA_PORT = 56401 +SDK_HOST_LOG_DATA_PORT = 56501 diff --git a/dimos/msgs/nav_msgs/Odometry.py b/dimos/msgs/nav_msgs/Odometry.py index 584bc42418..a958f8dba0 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 +if TYPE_CHECKING: + from rerun._baseclasses import Archetype + from dimos_lcm.nav_msgs import Odometry as LCMOdometry import numpy as np @@ -214,3 +217,19 @@ def __str__(self) -> str: f" Linear Velocity: [{self.vx:.3f}, {self.vy:.3f}, {self.vz:.3f}]\n" f" Angular Velocity: [{self.wx:.3f}, {self.wy:.3f}, {self.wz:.3f}]" ) + + 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, + ] + ), + ) diff --git a/dimos/msgs/sensor_msgs/Imu.py b/dimos/msgs/sensor_msgs/Imu.py new file mode 100644 index 0000000000..7fe03ce03f --- /dev/null +++ b/dimos/msgs/sensor_msgs/Imu.py @@ -0,0 +1,118 @@ +# 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 + + +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] = self.ros_timestamp() + 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/navigation/rosnav.py b/dimos/navigation/rosnav.py index c9cbcba631..8efabaebee 100644 --- a/dimos/navigation/rosnav.py +++ b/dimos/navigation/rosnav.py @@ -56,14 +56,14 @@ @dataclass class Config(ModuleConfig): local_pointcloud_freq: float = 2.0 - global_pointcloud_freq: float = 1.0 + global_map_freq: float = 1.0 sensor_to_base_link_transform: Transform = field( default_factory=lambda: Transform(frame_id="sensor", child_frame_id="base_link") ) class ROSNav( - Module, NavigationInterface, spec.Nav, spec.Global3DMap, spec.Pointcloud, spec.LocalPlanner + Module, NavigationInterface, spec.Nav, spec.GlobalPointcloud, spec.Pointcloud, spec.LocalPlanner ): config: Config default_config = Config @@ -72,7 +72,7 @@ class ROSNav( goal_req: In[PoseStamped] pointcloud: Out[PointCloud2] - global_pointcloud: Out[PointCloud2] + global_map: Out[PointCloud2] goal_active: Out[PoseStamped] path_active: Out[Path] @@ -83,7 +83,7 @@ class ROSNav( ros_cmd_vel: In[TwistStamped] ros_way_point: In[PoseStamped] ros_registered_scan: In[PointCloud2] - ros_global_pointcloud: In[PointCloud2] + ros_global_map: In[PointCloud2] ros_path: In[Path] ros_tf: In[TFMessage] @@ -95,7 +95,7 @@ class ROSNav( # Using RxPY Subjects for reactive data flow instead of storing state _local_pointcloud_subject: Subject # type: ignore[type-arg] - _global_pointcloud_subject: Subject # type: ignore[type-arg] + _global_map_subject: Subject # type: ignore[type-arg] _current_position_running: bool = False _goal_reach: bool | None = None @@ -112,7 +112,7 @@ def __init__(self, *args, **kwargs) -> None: # type: ignore[no-untyped-def] # Initialize RxPY Subjects for streaming data self._local_pointcloud_subject = Subject() - self._global_pointcloud_subject = Subject() + self._global_map_subject = Subject() # Initialize state tracking self._state_lock = threading.Lock() @@ -135,10 +135,10 @@ def start(self) -> None: ) self._disposables.add( - self._global_pointcloud_subject.pipe( - ops.sample(1.0 / self.config.global_pointcloud_freq), + self._global_map_subject.pipe( + ops.sample(1.0 / self.config.global_map_freq), ).subscribe( - on_next=self.global_pointcloud.publish, + on_next=self.global_map.publish, on_error=lambda e: logger.error(f"Map stream error: {e}"), ) ) @@ -148,7 +148,7 @@ def start(self) -> None: self.ros_cmd_vel.subscribe(self._on_ros_cmd_vel) self.ros_way_point.subscribe(self._on_ros_goal_waypoint) self.ros_registered_scan.subscribe(self._on_ros_registered_scan) - self.ros_global_pointcloud.subscribe(self._on_ros_global_pointcloud) + self.ros_global_map.subscribe(self._on_ros_global_map) self.ros_path.subscribe(self._on_ros_path) self.ros_tf.subscribe(self._on_ros_tf) @@ -171,8 +171,8 @@ def _on_ros_cmd_vel(self, msg: TwistStamped) -> None: def _on_ros_registered_scan(self, msg: PointCloud2) -> None: self._local_pointcloud_subject.on_next(msg) - def _on_ros_global_pointcloud(self, msg: PointCloud2) -> None: - self._global_pointcloud_subject.on_next(msg) + def _on_ros_global_map(self, msg: PointCloud2) -> None: + self._global_map_subject.on_next(msg) def _on_ros_path(self, msg: Path) -> None: msg.frame_id = "base_link" @@ -367,7 +367,7 @@ def stop(self) -> None: self._running = False self._local_pointcloud_subject.on_completed() - self._global_pointcloud_subject.on_completed() + self._global_map_subject.on_completed() except Exception as e: logger.error(f"Error during shutdown: {e}") @@ -383,7 +383,7 @@ def deploy(dimos: DimosCluster): # type: ignore[no-untyped-def] # Existing ports on LCM transports nav.pointcloud.transport = LCMTransport("/lidar", PointCloud2) - nav.global_pointcloud.transport = LCMTransport("/map", PointCloud2) + nav.global_map.transport = LCMTransport("/map", PointCloud2) nav.goal_req.transport = LCMTransport("/goal_req", PoseStamped) nav.goal_active.transport = LCMTransport("/goal_active", PoseStamped) nav.path_active.transport = LCMTransport("/path_active", Path) @@ -394,7 +394,7 @@ def deploy(dimos: DimosCluster): # type: ignore[no-untyped-def] nav.ros_cmd_vel.transport = ROSTransport("/cmd_vel", TwistStamped) nav.ros_way_point.transport = ROSTransport("/way_point", PoseStamped) nav.ros_registered_scan.transport = ROSTransport("/registered_scan", PointCloud2) - nav.ros_global_pointcloud.transport = ROSTransport("/terrain_map_ext", PointCloud2) + nav.ros_global_map.transport = ROSTransport("/terrain_map_ext", PointCloud2) nav.ros_path.transport = ROSTransport("/path", Path) nav.ros_tf.transport = ROSTransport("/tf", TFMessage) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 14e45b230d..b335a2df76 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -47,6 +47,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", "phone-go2-teleop": "dimos.teleop.phone.blueprints:phone_go2_teleop", "simple-phone-teleop": "dimos.teleop.phone.blueprints:simple_phone_teleop", "uintree-g1-primitive-no-nav": "dimos.robot.unitree.g1.blueprints.primitive.uintree_g1_primitive_no_nav:uintree_g1_primitive_no_nav", @@ -91,6 +95,7 @@ "depth_module": "dimos.robot.unitree.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.g1.connection", "g1_sim_connection": "dimos.robot.unitree.g1.sim", @@ -103,6 +108,7 @@ "keyboard_teleop": "dimos.robot.unitree.keyboard_teleop", "manipulation_module": "dimos.manipulation.manipulation_module", "mapper": "dimos.robot.unitree.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/spec/__init__.py b/dimos/spec/__init__.py index 03c1024d12..1423bec9a1 100644 --- a/dimos/spec/__init__.py +++ b/dimos/spec/__init__.py @@ -1,13 +1,12 @@ from dimos.spec.control import LocalPlanner -from dimos.spec.map import Global3DMap, GlobalCostmap, GlobalMap +from dimos.spec.mapping import GlobalCostmap, GlobalPointcloud from dimos.spec.nav import Nav from dimos.spec.perception import Camera, Image, Pointcloud __all__ = [ "Camera", - "Global3DMap", "GlobalCostmap", - "GlobalMap", + "GlobalPointcloud", "Image", "LocalPlanner", "Nav", diff --git a/dimos/spec/map.py b/dimos/spec/mapping.py similarity index 85% rename from dimos/spec/map.py rename to dimos/spec/mapping.py index 438b77a7a6..f8e7e1a04f 100644 --- a/dimos/spec/map.py +++ b/dimos/spec/mapping.py @@ -19,12 +19,8 @@ from dimos.msgs.sensor_msgs import PointCloud2 -class Global3DMap(Protocol): - global_pointcloud: Out[PointCloud2] - - -class GlobalMap(Protocol): - global_map: Out[OccupancyGrid] +class GlobalPointcloud(Protocol): + global_map: Out[PointCloud2] class GlobalCostmap(Protocol): diff --git a/dimos/spec/perception.py b/dimos/spec/perception.py index 4dc682523f..1cecdb4d2f 100644 --- a/dimos/spec/perception.py +++ b/dimos/spec/perception.py @@ -15,7 +15,8 @@ from typing import Protocol from dimos.core import Out -from dimos.msgs.sensor_msgs import CameraInfo, Image as ImageMsg, PointCloud2 +from dimos.msgs.nav_msgs.Odometry import Odometry as OdometryMsg +from dimos.msgs.sensor_msgs import CameraInfo, Image as ImageMsg, Imu, PointCloud2 class Image(Protocol): @@ -34,3 +35,17 @@ class DepthCamera(Camera): class Pointcloud(Protocol): pointcloud: Out[PointCloud2] + + +class IMU(Protocol): + imu: Out[Imu] + + +class Odometry(Protocol): + odometry: Out[OdometryMsg] + + +class Lidar(Protocol): + """LiDAR sensor providing point clouds.""" + + lidar: Out[PointCloud2] diff --git a/dimos/visualization/rerun/bridge.py b/dimos/visualization/rerun/bridge.py index 5263e59a44..1dc104f1b4 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): @@ -255,6 +259,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 +314,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 +322,11 @@ def run_bridge( signal.pause() -def _cli( +app = typer.Typer() + + +@app.command() +def cli( viewer_mode: str = typer.Option( "native", help="Viewer mode: native (desktop), web (browser), none (headless)" ), @@ -359,7 +339,7 @@ def _cli( if __name__ == "__main__": - typer.run(_cli) + app() # 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/docs/usage/lcm.md b/docs/usage/lcm.md index f1b41cfc9f..99437a2458 100644 --- a/docs/usage/lcm.md +++ b/docs/usage/lcm.md @@ -1,9 +1,29 @@ - # LCM Messages -[LCM (Lightweight Communications and Marshalling)](https://github.com/lcm-proj/lcm) is a message-passing system with bindings for many languages (C, C++, Python, Java, Lua, Go). While LCM includes a UDP multicast transport, its real power is the message definition format - classes that can encode themselves to a compact binary representation. +DimOS uses [LCM (Lightweight Communications and Marshalling)](https://github.com/lcm-proj/lcm) for inter-process communication on a local machine (similar to how ROS uses DDS). LCM is a simple [UDP multicast](https://lcm-proj.github.io/lcm/content/udp-multicast-protocol.html#lcm-udp-multicast-protocol-description) pubsub protocol with a straightforward [message definition language](https://lcm-proj.github.io/lcm/content/lcm-type-ref.html#lcm-type-specification-language). + +The LCM project provides pubsub clients and code generators for many languages. For us the power of LCM is its message definition format, multi-language classes that encode themselves to a compact binary format. This means LCM messages can be sent over any transport (WebSocket, SSH, shared memory, etc.) between differnt programming languages. + +Our messages are ported from ROS (they are structurally compatible in order to facilitate easy communication to ROS if needed) +Repo that hosts our message definitions and autogenerators is at [dimos-lcm](https://github.com/dimensionalOS/dimos-lcm/) + +our LCM implementation significantly [outperforms ROS for local communication](/docs/usage/transports.md#benchmarks) + +## Supported languages + +Apart from python, we have examples of LCM integrations for: +- [**C++**](/examples/language-interop/cpp/README.md) +- [**TypeScript**](/examples/language-interop/ts/README.md) +- [**Lua**](/examples/language-interop/lua/README.md) + +In our [/examples/language-interop/](/examples/language-interop/) dir + +Types generated (but no examples yet) for: +[**C#**](https://github.com/dimensionalOS/dimos-lcm/tree/main/generated/csharp) and [**Java**](https://github.com/dimensionalOS/dimos-lcm/tree/main/generated/java) + +### Native Modules -Dimos uses LCM message definitions for all inter-module communication. Because messages serialize to binary, they can be sent over any transport - not just LCM's UDP multicast, but also shared memory, Redis, WebSockets, or any other channel. +Given LCM is so portable, we can easily run dimos [Modules](/docs/usage/modules.md) written in [third party languages](/docs/usage/native_modules.md) ## dimos-lcm Package diff --git a/docs/usage/native_modules.md b/docs/usage/native_modules.md new file mode 100644 index 0000000000..5a9362839f --- /dev/null +++ b/docs/usage/native_modules.md @@ -0,0 +1,282 @@ +# Native Modules + +Prerequisite for this is to understand dimos [Modules](/docs/usage/modules.md) and [Blueprints](/docs/usage/blueprints.md). + +Native modules let you wrap **any executable** as a first-class DimOS module, given it speaks LCM. + +Python will handle blueprint wiring, lifecycle, and logging. Native binary handles the actual computation, publishing and subscribing directly on LCM. + +Python module **never touches the pubsub data**. It just passes configuration and LCM topic to use via CLI args to your executable. + +On how to speak LCM with the rest of dimos, you can read our [LCM intro](/docs/usage/lcm.md) + +## Defining a native module + +Python side native module is just a definition of a **config** dataclass and **module** class specifying pubsub I/O. + +Both the config dataclass and pubsub topics get converted to CLI args passed down to your executable once the module is started. + +```python no-result session=nativemodule +from dataclasses import dataclass +from dimos.core import Out, LCMTransport +from dimos.core.native_module import NativeModule, NativeModuleConfig +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.sensor_msgs.Imu import Imu +import time + +@dataclass(kw_only=True) +class MyLidarConfig(NativeModuleConfig): + executable: str = "./build/my_lidar" + host_ip: str = "192.168.1.5" + frequency: float = 10.0 + +class MyLidar(NativeModule): + default_config = MyLidarConfig + pointcloud: Out[PointCloud2] + imu: Out[Imu] + + +``` + +That's it. `MyLidar` is a full DimOS module. You can use it with `autoconnect`, blueprints, transport overrides, and specs. Once this module is started, your `./build/my_lidar` will get called with specific CLI args. + + +## How it works + +When `start()` is called, NativeModule: + +1. **Builds the executable** if it doesn't exist and `build_command` is set. +2. **Collects topics** from blueprint-assigned transports on each declared port. +3. **Builds the command line**: ` -- ... -- ...` +4. **Launches the subprocess** with `Popen`, piping stdout/stderr. +5. **Starts a watchdog** thread that calls `stop()` if the process crashes. + +For the example above, the launched command would look like: + +```sh +./build/my_lidar \ + --pointcloud '/pointcloud#sensor_msgs.PointCloud2' \ + --imu '/imu#sensor_msgs.Imu' \ + --host_ip 192.168.1.5 \ + --frequency 10.0 +``` + +```python ansi=false session=nativemodule skip +mylidar = MyLidar() +mylidar.pointcloud.transport = LCMTransport("/lidar", PointCloud2) +mylidar.imu.transport = LCMTransport("/imu", Imu) +mylidar.start() +``` + + +``` +2026-02-14T11:22:12.123963Z [info ] Starting native process [dimos/core/native_module.py] cmd='./build/my_lidar --pointcloud /lidar#sensor_msgs.PointCloud2 --imu /imu#sensor_msgs.Imu --host_ip 192.168.1.5 --frequency 10.0' cwd=/home/lesh/coding/dimos/docs/usage/build +``` + +Topic strings use the format `/#`, which is the LCM channel name that Python `LCMTransport` subscribers use. The native binary publishes on these exact channels. + +When `stop()` is called, the process receives SIGTERM. If it doesn't exit within `shutdown_timeout` seconds (default 10), it gets SIGKILL. + +## Config + +`NativeModuleConfig` extends `ModuleConfig` with subprocess fields: + +| Field | Type | Default | Description | +|--------------------|------------------|---------------|-------------------------------------------------------------| +| `executable` | `str` | *(required)* | Path to the native binary (relative to `cwd` if set) | +| `build_command` | `str \| None` | `None` | Shell command to run if executable is missing (auto-build) | +| `cwd` | `str \| None` | `None` | Working directory for build and runtime. Relative paths are resolved against the Python file defining the module | +| `extra_args` | `list[str]` | `[]` | Additional CLI arguments appended after auto-generated ones | +| `extra_env` | `dict[str, str]` | `{}` | Extra environment variables for the subprocess | +| `shutdown_timeout` | `float` | `10.0` | Seconds to wait for SIGTERM before SIGKILL | +| `log_format` | `LogFormat` | `TEXT` | How to parse subprocess output (`TEXT` or `JSON`) | +| `cli_exclude` | `frozenset[str]` | `frozenset()` | Config fields to skip when generating CLI args | + +### Auto CLI arg generation + +Any field you add to your config subclass automatically becomes a `--name value` CLI arg. Fields from `NativeModuleConfig` itself (like `executable`, `extra_args`, `cwd`) are **not** passed — they're for Python-side orchestration only. + +```python skip + +class LogFormat(enum.Enum): + TEXT = "text" + JSON = "json" + +@dataclass(kw_only=True) +class MyConfig(NativeModuleConfig): + executable: str = "./build/my_module" # relative or absolute path to your executable + host_ip: str = "192.168.1.5" # becomes --host_ip 192.168.1.5 + frequency: float = 10.0 # becomes --frequency 10.0 + enable_imu: bool = True # becomes --enable_imu true + filters: list[str] = field(default_factory=lambda: ["a", "b"]) # becomes --filters a,b +``` + +- `None` values are skipped. +- Booleans are lowercased (`true`/`false`). +- Lists are comma-joined. + +### Excluding fields + +If a config field shouldn't be a CLI arg, add it to `cli_exclude`: + +```python skip +@dataclass(kw_only=True) +class FastLio2Config(NativeModuleConfig): + executable: str = "./build/fastlio2" + config: str = "mid360.yaml" # human-friendly name + config_path: str | None = None # resolved absolute path + cli_exclude: frozenset[str] = frozenset({"config"}) # only config_path is passed + + def __post_init__(self) -> None: + if self.config_path is None: + self.config_path = str(Path(self.config).resolve()) +``` + +## Using with blueprints + +Native modules work with `autoconnect` exactly like Python modules: + +```python skip +from dimos.core.blueprints import autoconnect + +class PointCloudConsumer(Module): + pointcloud: In[PointCloud2] + imu: In[Imu] + +autoconnect( + MyLidar.blueprint(host_ip="192.168.1.10"), + PointCloudConsumer.blueprint(), +).build().loop() +``` + +`autoconnect` matches ports by `(name, type)`, assigns LCM topics, and passes them to the native binary as CLI args. You can override transports as usual: + +```python skip +blueprint = autoconnect( + MyLidar.blueprint(), + PointCloudConsumer.blueprint(), +).transports({ + ("pointcloud", PointCloud2): LCMTransport("/my/custom/lidar", PointCloud2), +}) +``` + +## Logging + +NativeModule pipes subprocess stdout and stderr through structlog: + +- **stdout** is logged at `info` level. +- **stderr** is logged at `warning` level. + +### JSON log format + +If your native binary outputs structured JSON lines, set `log_format=LogFormat.JSON`: + +```python skip +@dataclass(kw_only=True) +class MyConfig(NativeModuleConfig): + executable: str = "./build/my_module" + log_format: LogFormat = LogFormat.JSON +``` + +The module will parse each line as JSON and feed the key-value pairs into structlog. The `event` key becomes the log message: + +```json +{"event": "sensor initialized", "device": "/dev/ttyUSB0", "baud": 115200} +``` + +Malformed lines fall back to plain text logging. + +## Writing the C++ side + +A header-only helper is provided at [`dimos/hardware/sensors/lidar/common/dimos_native_module.hpp`](/dimos/hardware/sensors/lidar/common/dimos_native_module.hpp): + +```cpp +#include "dimos_native_module.hpp" +#include "sensor_msgs/PointCloud2.hpp" + +int main(int argc, char** argv) { + dimos::NativeModule mod(argc, argv); + + // Get the LCM channel for a declared port + std::string pc_topic = mod.topic("pointcloud"); + + // Get config values + float freq = mod.arg_float("frequency", 10.0); + std::string ip = mod.arg("host_ip", "192.168.1.5"); + + // Set up LCM publisher and publish on pc_topic... +} +``` + +The helper provides: + +| Method | Description | +|---------------------------|----------------------------------------------------------------| +| `topic(port)` | Get the full LCM channel string (`/topic#msg_type`) for a port | +| `arg(key, default)` | Get a string config value | +| `arg_float(key, default)` | Get a float config value | +| `arg_int(key, default)` | Get an int config value | +| `has(key)` | Check if a port/arg was provided | + +It also includes `make_header()` and `time_from_seconds()` for building ROS-compatible stamped messages. + +## Examples + +For language interop examples (subscribing to DimOS topics from C++, TypeScript, Lua), see [/examples/language-interop/](/examples/language-interop/README.md). + +### Livox Mid-360 Module + +The Livox Mid-360 LiDAR driver is a complete example at [`dimos/hardware/sensors/lidar/livox/module.py`](/dimos/hardware/sensors/lidar/livox/module.py): + +```python skip +from dimos.core import Out +from dimos.core.native_module import NativeModule, NativeModuleConfig +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.sensor_msgs.Imu import Imu +from dimos.spec import perception + +@dataclass(kw_only=True) +class Mid360Config(NativeModuleConfig): + cwd: str | None = "cpp" + executable: str = "result/bin/mid360_native" + build_command: str | None = "nix build .#mid360_native" + 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" + # ... SDK port configuration + +class Mid360(NativeModule, perception.Lidar, perception.IMU): + default_config = Mid360Config + lidar: Out[PointCloud2] + imu: Out[Imu] +``` + +Usage: + +```python skip +from dimos.hardware.sensors.lidar.livox.module import Mid360 + +autoconnect( + Mid360.blueprint(host_ip="192.168.1.5"), + SomeConsumer.blueprint(), +) +``` + +## Auto Building + +If `build_command` is set in the module config, and the executable doesn't exist when `start()` is called, NativeModule runs the build command automatically. +Build output is piped through structlog (stdout at `info`, stderr at `warning`). + +```python skip +@dataclass(kw_only=True) +class MyLidarConfig(NativeModuleConfig): + cwd: str | None = "cpp" + executable: str = "result/bin/my_lidar" + build_command: str | None = "nix build .#my_lidar" +``` + +`cwd` is used for both the build command and the runtime subprocess. Relative paths are resolved against the directory of the Python file that defines the module + +If the executable already exists, the build step is skipped entirely. diff --git a/pyproject.toml b/pyproject.toml index 0420e23e61..1b0c40acd3 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -76,7 +76,7 @@ foxglove-bridge = "dimos.utils.cli.foxglove_bridge.run_foxglove_bridge:main" agentspy = "dimos.utils.cli.agentspy.agentspy:main" humancli = "dimos.utils.cli.human.humanclianim:main" dimos = "dimos.robot.cli.dimos:main" -rerun-bridge = "dimos.visualization.rerun.bridge:main" +rerun-bridge = "dimos.visualization.rerun.bridge:app" doclinks = "dimos.utils.docs.doclinks:main" [project.optional-dependencies]