From f954e86a3768629ae20aa75f12790e797aabfc83 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Thu, 12 Feb 2026 03:25:39 +0800 Subject: [PATCH 01/48] mid360 livox integration --- dimos/hardware/sensors/lidar/__init__.py | 0 .../hardware/sensors/lidar/livox/__init__.py | 0 dimos/hardware/sensors/lidar/livox/mid360.py | 493 ++++++++++++++++ dimos/hardware/sensors/lidar/livox/sdk.py | 534 ++++++++++++++++++ dimos/hardware/sensors/lidar/module.py | 147 +++++ dimos/hardware/sensors/lidar/spec.py | 43 ++ dimos/msgs/sensor_msgs/Imu.py | 123 ++++ dimos/msgs/sensor_msgs/PointCloud2.py | 21 +- dimos/msgs/sensor_msgs/__init__.py | 2 + dimos/spec/perception.py | 12 +- dimos/spec/utils.py | 26 + dimos/visualization/rerun/bridge.py | 34 +- 12 files changed, 1394 insertions(+), 41 deletions(-) create mode 100644 dimos/hardware/sensors/lidar/__init__.py create mode 100644 dimos/hardware/sensors/lidar/livox/__init__.py create mode 100644 dimos/hardware/sensors/lidar/livox/mid360.py create mode 100644 dimos/hardware/sensors/lidar/livox/sdk.py create mode 100644 dimos/hardware/sensors/lidar/module.py create mode 100644 dimos/hardware/sensors/lidar/spec.py create mode 100644 dimos/msgs/sensor_msgs/Imu.py diff --git a/dimos/hardware/sensors/lidar/__init__.py b/dimos/hardware/sensors/lidar/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/dimos/hardware/sensors/lidar/livox/__init__.py b/dimos/hardware/sensors/lidar/livox/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/dimos/hardware/sensors/lidar/livox/mid360.py b/dimos/hardware/sensors/lidar/livox/mid360.py new file mode 100644 index 0000000000..9c06c4af20 --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/mid360.py @@ -0,0 +1,493 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Livox Mid-360 LiDAR hardware driver using Livox SDK2 ctypes bindings.""" + +from __future__ import annotations + +from dataclasses import dataclass, field +from functools import cache +import json +import logging +from queue import Empty, Queue +import tempfile +import threading +import time +from typing import TYPE_CHECKING, Any + +import numpy as np +import open3d as o3d # type: ignore[import-untyped] +import open3d.core as o3c # type: ignore[import-untyped] +from reactivex import create + +from dimos.hardware.sensors.lidar.livox.sdk import ( + AsyncControlCallbackType, + ImuDataCallbackType, + InfoChangeCallbackType, + LivoxLidarDeviceType, + LivoxLidarEthernetPacket, + LivoxLidarInfo, + LivoxLidarPointDataType, + LivoxLidarWorkMode, + LivoxSDK, + PointCloudCallbackType, + get_packet_timestamp_ns, + parse_cartesian_high_points, + parse_cartesian_low_points, + parse_imu_data, +) +from dimos.hardware.sensors.lidar.spec import LidarConfig, LidarHardware +from dimos.msgs.geometry_msgs import Vector3 +from dimos.msgs.sensor_msgs.Imu import Imu +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.utils.reactive import backpressure + +if TYPE_CHECKING: + from reactivex.observable import Observable + +logger = logging.getLogger(__name__) + +# Gravity constant for converting accelerometer data from g to m/s^2 +GRAVITY_MS2 = 9.80665 + + +@dataclass +class LivoxMid360Config(LidarConfig): + """Configuration for the Livox Mid-360 LiDAR.""" + + host_ip: str = "192.168.1.5" + lidar_ips: list[str] = field(default_factory=lambda: ["192.168.1.155"]) + frequency: float = 10.0 # Hz, point cloud output rate + frame_id: str = "lidar_link" + frame_id_prefix: str | None = None + enable_imu: bool = True + sdk_lib_path: str | None = None + + # SDK port configuration + cmd_data_port: int = 56100 + push_msg_port: int = 56200 + point_data_port: int = 56300 + imu_data_port: int = 56400 + log_data_port: int = 56500 + host_cmd_data_port: int = 56101 + host_push_msg_port: int = 56201 + host_point_data_port: int = 56301 + host_imu_data_port: int = 56401 + host_log_data_port: int = 56501 + + # Socket buffer size (bytes) to avoid packet loss at high data rates + recv_buf_size: int = 4 * 1024 * 1024 # 4 MB + + +class LivoxMid360(LidarHardware["LivoxMid360Config"]): + """Livox Mid-360 LiDAR driver using SDK2 ctypes bindings. + + Produces Observable[PointCloud2] at the configured frame rate (~10 Hz), + and optionally Observable[Imu] at ~200 Hz. + + Uses a thread-safe queue to pass data from SDK C-thread callbacks + to Python consumer threads that build messages. + """ + + default_config = LivoxMid360Config + + def __init__(self, *args: object, **kwargs: object) -> None: + super().__init__(*args, **kwargs) + self._sdk: LivoxSDK | None = None + self._config_path: str | None = None + self._stop_event = threading.Event() + + # Point cloud frame accumulator state + self._pc_queue: Queue[tuple[int, int, np.ndarray, np.ndarray]] = Queue(maxsize=4096) + self._pc_consumer_thread: threading.Thread | None = None + self._pc_observer: Any[PointCloud2] | None = None + + # IMU state + self._imu_queue: Queue[tuple[int, np.ndarray]] = Queue(maxsize=4096) + self._imu_consumer_thread: threading.Thread | None = None + self._imu_observer: Any[Imu] | None = None + + # Device tracking + self._connected_handles: dict[int, LivoxLidarDeviceType] = {} + + def _frame_id(self, suffix: str) -> str: + prefix = self.config.frame_id_prefix + if prefix: + return f"{prefix}/{suffix}" + return suffix + + # ------------------------------------------------------------------ + # SDK config generation + # ------------------------------------------------------------------ + + def _write_sdk_config(self) -> str: + """Generate a temporary JSON config file for the SDK.""" + config = { + "MID360": { + "lidar_net_info": { + "cmd_data_port": self.config.cmd_data_port, + "push_msg_port": self.config.push_msg_port, + "point_data_port": self.config.point_data_port, + "imu_data_port": self.config.imu_data_port, + "log_data_port": self.config.log_data_port, + }, + "host_net_info": [ + { + "host_ip": self.config.host_ip, + "multicast_ip": "224.1.1.5", + "cmd_data_port": self.config.host_cmd_data_port, + "push_msg_port": self.config.host_push_msg_port, + "point_data_port": self.config.host_point_data_port, + "imu_data_port": self.config.host_imu_data_port, + "log_data_port": self.config.host_log_data_port, + } + ], + } + } + fd, path = tempfile.mkstemp(suffix=".json", prefix="livox_mid360_") + with open(fd, "w") as f: + json.dump(config, f) + return path + + # ------------------------------------------------------------------ + # SDK callbacks (called from C threads - keep minimal, just enqueue) + # ------------------------------------------------------------------ + + def _on_point_cloud( + self, + handle: int, + dev_type: int, + packet_ptr: LivoxLidarEthernetPacket, # type: ignore[override] + client_data: object, + ) -> None: + """SDK point cloud callback. Copies data and enqueues for processing.""" + if self._stop_event.is_set(): + return + try: + packet = packet_ptr.contents if hasattr(packet_ptr, "contents") else packet_ptr + data_type = packet.data_type + + if data_type == LivoxLidarPointDataType.CARTESIAN_HIGH: + xyz, reflectivities, _tags = parse_cartesian_high_points(packet) + elif data_type == LivoxLidarPointDataType.CARTESIAN_LOW: + xyz, reflectivities, _tags = parse_cartesian_low_points(packet) + else: + return # skip spherical for now + + ts_ns = get_packet_timestamp_ns(packet) + frame_cnt = packet.frame_cnt + + self._pc_queue.put_nowait((frame_cnt, ts_ns, xyz, reflectivities)) + except Exception: + logger.debug("Error in point cloud callback", exc_info=True) + + def _on_imu_data( + self, + handle: int, + dev_type: int, + packet_ptr: LivoxLidarEthernetPacket, # type: ignore[override] + client_data: object, + ) -> None: + """SDK IMU callback. Copies data and enqueues.""" + if self._stop_event.is_set(): + return + try: + packet = packet_ptr.contents if hasattr(packet_ptr, "contents") else packet_ptr + ts_ns = get_packet_timestamp_ns(packet) + imu_points = parse_imu_data(packet) + self._imu_queue.put_nowait((ts_ns, imu_points)) + except Exception: + logger.debug("Error in IMU callback", exc_info=True) + + def _on_info_change( + self, + handle: int, + info_ptr: LivoxLidarInfo, # type: ignore[override] + client_data: object, + ) -> None: + """SDK device info change callback. Tracks connected devices.""" + try: + info = info_ptr.contents if hasattr(info_ptr, "contents") else info_ptr + dev_type = LivoxLidarDeviceType(info.dev_type) + sn = info.sn.decode("utf-8", errors="replace").rstrip("\x00") + ip = info.lidar_ip.decode("utf-8", errors="replace").rstrip("\x00") + self._connected_handles[handle] = dev_type + logger.info( + "Livox device connected: handle=%d type=%s sn=%s ip=%s", + handle, + dev_type.name, + sn, + ip, + ) + + # Set to normal work mode + self._sdk_set_work_mode(handle) + + # Enable/disable IMU based on config + if self.config.enable_imu: + self._sdk_enable_imu(handle) + except Exception: + logger.debug("Error in info change callback", exc_info=True) + + def _sdk_set_work_mode(self, handle: int) -> None: + if self._sdk is None: + return + + def _on_work_mode(status: int, handle: int, response: object, client_data: object) -> None: + if status == 0: + logger.info("Work mode set to NORMAL for handle %d", handle) + else: + logger.warning("Failed to set work mode for handle %d: status=%d", handle, status) + + _cb = AsyncControlCallbackType(_on_work_mode) + self._sdk._callbacks[f"work_mode_cb_{handle}"] = _cb + self._sdk.lib.SetLivoxLidarWorkMode(handle, int(LivoxLidarWorkMode.NORMAL), _cb, None) + + def _sdk_enable_imu(self, handle: int) -> None: + if self._sdk is None: + return + + def _on_imu_enable(status: int, handle: int, response: object, client_data: object) -> None: + if status == 0: + logger.info("IMU enabled for handle %d", handle) + else: + logger.warning("Failed to enable IMU for handle %d: status=%d", handle, status) + + _cb = AsyncControlCallbackType(_on_imu_enable) + self._sdk._callbacks[f"imu_enable_cb_{handle}"] = _cb + self._sdk.lib.EnableLivoxLidarImuData(handle, _cb, None) + + # ------------------------------------------------------------------ + # Consumer threads (Python-side, build messages from queued data) + # ------------------------------------------------------------------ + + def _pointcloud_consumer_loop(self) -> None: + """Drain the point cloud queue, accumulate by time window, emit PointCloud2.""" + frame_points: list[np.ndarray] = [] + frame_reflectivities: list[np.ndarray] = [] + frame_timestamp: float | None = None + frame_interval = 1.0 / self.config.frequency if self.config.frequency > 0 else 0.1 + last_emit = time.monotonic() + + while not self._stop_event.is_set(): + try: + _frame_cnt, ts_ns, xyz, reflectivities = self._pc_queue.get(timeout=0.05) + except Empty: + # Check if we should emit on timeout + if frame_points and (time.monotonic() - last_emit) >= frame_interval: + self._emit_pointcloud(frame_points, frame_reflectivities, frame_timestamp) + frame_points = [] + frame_reflectivities = [] + frame_timestamp = None + last_emit = time.monotonic() + continue + + frame_points.append(xyz) + frame_reflectivities.append(reflectivities) + if frame_timestamp is None: + frame_timestamp = ts_ns / 1e9 + + # Emit when time window is full + if (time.monotonic() - last_emit) >= frame_interval: + self._emit_pointcloud(frame_points, frame_reflectivities, frame_timestamp) + frame_points = [] + frame_reflectivities = [] + frame_timestamp = None + last_emit = time.monotonic() + + # Emit any remaining data + if frame_points: + self._emit_pointcloud(frame_points, frame_reflectivities, frame_timestamp) + + def _emit_pointcloud( + self, + frame_points: list[np.ndarray], + frame_reflectivities: list[np.ndarray], + frame_timestamp: float | None, + ) -> None: + if not self._pc_observer or self._stop_event.is_set(): + return + + all_points = np.concatenate(frame_points, axis=0) + all_reflectivities = np.concatenate(frame_reflectivities, axis=0) + + if len(all_points) == 0: + return + + # Build Open3D tensor point cloud + pcd_t = o3d.t.geometry.PointCloud() + pcd_t.point["positions"] = o3c.Tensor(all_points, dtype=o3c.float32) + # Store reflectivity as intensity (normalized to 0-1 range) + pcd_t.point["intensities"] = o3c.Tensor( + all_reflectivities.astype(np.float32).reshape(-1, 1) / 255.0, + dtype=o3c.float32, + ) + + pc2 = PointCloud2( + pointcloud=pcd_t, + frame_id=self._frame_id(self.config.frame_id), + ts=frame_timestamp if frame_timestamp else time.time(), + ) + + try: + self._pc_observer.on_next(pc2) + except Exception: + logger.debug("Error emitting point cloud", exc_info=True) + + def _imu_consumer_loop(self) -> None: + """Drain the IMU queue and emit Imu messages.""" + while not self._stop_event.is_set(): + try: + ts_ns, imu_points = self._imu_queue.get(timeout=0.5) + except Empty: + continue + + if not self._imu_observer or self._stop_event.is_set(): + continue + + ts = ts_ns / 1e9 + for i in range(len(imu_points)): + pt = imu_points[i] + imu_msg = Imu( + angular_velocity=Vector3( + float(pt["gyro_x"]), + float(pt["gyro_y"]), + float(pt["gyro_z"]), + ), + linear_acceleration=Vector3( + float(pt["acc_x"]) * GRAVITY_MS2, + float(pt["acc_y"]) * GRAVITY_MS2, + float(pt["acc_z"]) * GRAVITY_MS2, + ), + frame_id=self._frame_id("imu_link"), + ts=ts, + ) + try: + self._imu_observer.on_next(imu_msg) + except Exception: + logger.debug("Error emitting IMU data", exc_info=True) + + # ------------------------------------------------------------------ + # Public API: Observable streams + # ------------------------------------------------------------------ + + @cache + def pointcloud_stream(self) -> Observable[PointCloud2]: + """Observable stream of PointCloud2 messages (~10 Hz full-frame scans).""" + + def subscribe(observer, scheduler=None): # type: ignore[no-untyped-def] + self._pc_observer = observer + try: + self._start_sdk() + except Exception as e: + observer.on_error(e) + return + + def dispose() -> None: + self._pc_observer = None + self.stop() + + return dispose + + return backpressure(create(subscribe)) + + @cache + def imu_stream(self) -> Observable[Imu]: + """Observable stream of Imu messages (~200 Hz).""" + + def subscribe(observer, scheduler=None): # type: ignore[no-untyped-def] + self._imu_observer = observer + + def dispose() -> None: + self._imu_observer = None + + return dispose + + return backpressure(create(subscribe)) + + # ------------------------------------------------------------------ + # Lifecycle + # ------------------------------------------------------------------ + + def _start_sdk(self) -> None: + """Initialize and start the Livox SDK.""" + if self._sdk is not None: + return # already running + + self._stop_event.clear() + + # Write temp config + self._config_path = self._write_sdk_config() + + # Load and init SDK + self._sdk = LivoxSDK(self.config.sdk_lib_path) + self._sdk.init(self._config_path, self.config.host_ip) + + # Register callbacks (must keep references to prevent GC) + pc_cb = PointCloudCallbackType(self._on_point_cloud) + imu_cb = ImuDataCallbackType(self._on_imu_data) + info_cb = InfoChangeCallbackType(self._on_info_change) + + self._sdk.set_point_cloud_callback(pc_cb) + if self.config.enable_imu: + self._sdk.set_imu_callback(imu_cb) + self._sdk.set_info_change_callback(info_cb) + + # Start SDK background threads + self._sdk.start() + logger.info( + "Livox SDK started (host_ip=%s, lidar_ips=%s)", + self.config.host_ip, + self.config.lidar_ips, + ) + + # Start consumer threads + self._pc_consumer_thread = threading.Thread( + target=self._pointcloud_consumer_loop, daemon=True, name="livox-pc-consumer" + ) + self._pc_consumer_thread.start() + + if self.config.enable_imu: + self._imu_consumer_thread = threading.Thread( + target=self._imu_consumer_loop, daemon=True, name="livox-imu-consumer" + ) + self._imu_consumer_thread.start() + + def stop(self) -> None: + """Stop the SDK and all consumer threads.""" + self._stop_event.set() + + if self._sdk is not None: + self._sdk.uninit() + self._sdk = None + + for thread in [self._pc_consumer_thread, self._imu_consumer_thread]: + if thread is not None and thread.is_alive(): + thread.join(timeout=3.0) + + self._pc_consumer_thread = None + self._imu_consumer_thread = None + self._connected_handles.clear() + + # Cleanup temp config + if self._config_path: + import os + + try: + os.unlink(self._config_path) + except OSError: + pass + self._config_path = None + + logger.info("Livox Mid-360 stopped") diff --git a/dimos/hardware/sensors/lidar/livox/sdk.py b/dimos/hardware/sensors/lidar/livox/sdk.py new file mode 100644 index 0000000000..eb5fbcbccb --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/sdk.py @@ -0,0 +1,534 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""ctypes bindings for Livox SDK2 (liblivox_lidar_sdk_shared.so). + +Provides Python access to the Livox LiDAR SDK2 C API for device +communication, point cloud streaming, and IMU data. + +The SDK .so must be pre-built from https://github.com/Livox-SDK/Livox-SDK2. +Set LIVOX_SDK2_LIB_PATH env var or install to /usr/local/lib. +""" + +from __future__ import annotations + +import ctypes +import ctypes.util +from enum import IntEnum +import logging +import os +from pathlib import Path +from typing import Any + +import numpy as np + +logger = logging.getLogger(__name__) + + +# --------------------------------------------------------------------------- +# Enums +# --------------------------------------------------------------------------- + + +class LivoxLidarDeviceType(IntEnum): + HUB = 0 + MID40 = 1 + TELE = 2 + HORIZON = 3 + MID70 = 6 + AVIA = 7 + MID360 = 9 + INDUSTRIAL_HAP = 10 + HAP = 15 + PA = 16 + + +class LivoxLidarPointDataType(IntEnum): + IMU = 0x00 + CARTESIAN_HIGH = 0x01 + CARTESIAN_LOW = 0x02 + SPHERICAL = 0x03 + + +class LivoxLidarWorkMode(IntEnum): + NORMAL = 0x01 + WAKE_UP = 0x02 + SLEEP = 0x03 + ERROR = 0x04 + POWER_ON_SELF_TEST = 0x05 + MOTOR_STARTING = 0x06 + MOTOR_STOPPING = 0x07 + UPGRADE = 0x08 + + +class LivoxLidarScanPattern(IntEnum): + NON_REPETITIVE = 0x00 + REPETITIVE = 0x01 + REPETITIVE_LOW_FRAME_RATE = 0x02 + + +class LivoxLidarStatus(IntEnum): + SEND_FAILED = -9 + HANDLER_IMPL_NOT_EXIST = -8 + INVALID_HANDLE = -7 + CHANNEL_NOT_EXIST = -6 + NOT_ENOUGH_MEMORY = -5 + TIMEOUT = -4 + NOT_SUPPORTED = -3 + NOT_CONNECTED = -2 + FAILURE = -1 + SUCCESS = 0 + + +# --------------------------------------------------------------------------- +# Packed C structures (all #pragma pack(1) in the SDK header) +# --------------------------------------------------------------------------- + + +class LivoxLidarCartesianHighRawPoint(ctypes.LittleEndianStructure): + """High-resolution cartesian point (14 bytes). Coordinates in mm.""" + + _pack_ = 1 + _fields_ = [ + ("x", ctypes.c_int32), + ("y", ctypes.c_int32), + ("z", ctypes.c_int32), + ("reflectivity", ctypes.c_uint8), + ("tag", ctypes.c_uint8), + ] + + +class LivoxLidarCartesianLowRawPoint(ctypes.LittleEndianStructure): + """Low-resolution cartesian point (8 bytes). Coordinates in cm.""" + + _pack_ = 1 + _fields_ = [ + ("x", ctypes.c_int16), + ("y", ctypes.c_int16), + ("z", ctypes.c_int16), + ("reflectivity", ctypes.c_uint8), + ("tag", ctypes.c_uint8), + ] + + +class LivoxLidarSpherPoint(ctypes.LittleEndianStructure): + """Spherical coordinate point (10 bytes).""" + + _pack_ = 1 + _fields_ = [ + ("depth", ctypes.c_uint32), + ("theta", ctypes.c_uint16), + ("phi", ctypes.c_uint16), + ("reflectivity", ctypes.c_uint8), + ("tag", ctypes.c_uint8), + ] + + +class LivoxLidarImuRawPoint(ctypes.LittleEndianStructure): + """IMU data point (24 bytes). Gyro in rad/s, accel in g.""" + + _pack_ = 1 + _fields_ = [ + ("gyro_x", ctypes.c_float), + ("gyro_y", ctypes.c_float), + ("gyro_z", ctypes.c_float), + ("acc_x", ctypes.c_float), + ("acc_y", ctypes.c_float), + ("acc_z", ctypes.c_float), + ] + + +class LivoxLidarEthernetPacket(ctypes.LittleEndianStructure): + """Point cloud / IMU ethernet packet header (32 bytes + variable data).""" + + _pack_ = 1 + _fields_ = [ + ("version", ctypes.c_uint8), + ("length", ctypes.c_uint16), + ("time_interval", ctypes.c_uint16), # unit: 0.1 us + ("dot_num", ctypes.c_uint16), + ("udp_cnt", ctypes.c_uint16), + ("frame_cnt", ctypes.c_uint8), + ("data_type", ctypes.c_uint8), + ("time_type", ctypes.c_uint8), + ("rsvd", ctypes.c_uint8 * 12), + ("crc32", ctypes.c_uint32), + ("timestamp", ctypes.c_uint8 * 8), + ("data", ctypes.c_uint8 * 1), # variable-length payload + ] + + +class LivoxLidarInfo(ctypes.LittleEndianStructure): + """Device info received on connection.""" + + _pack_ = 1 + _fields_ = [ + ("dev_type", ctypes.c_uint8), + ("sn", ctypes.c_char * 16), + ("lidar_ip", ctypes.c_char * 16), + ] + + +class LivoxLidarAsyncControlResponse(ctypes.LittleEndianStructure): + _pack_ = 1 + _fields_ = [ + ("ret_code", ctypes.c_uint8), + ("error_key", ctypes.c_uint16), + ] + + +# --------------------------------------------------------------------------- +# numpy dtypes for efficient batch parsing of point data +# --------------------------------------------------------------------------- + +CART_HIGH_DTYPE = np.dtype( + [("x", " tuple[np.ndarray, np.ndarray, np.ndarray]: + """Parse high-res cartesian points from a packet. + + Returns: + (xyz_meters, reflectivities, tags) where xyz is (N,3) float32 in meters. + """ + dot_num = packet.dot_num + if dot_num == 0: + empty = np.empty((0, 3), dtype=np.float32) + return empty, np.empty(0, dtype=np.uint8), np.empty(0, dtype=np.uint8) + + data_ptr = ctypes.addressof(packet) + LivoxLidarEthernetPacket.data.offset + buf = (ctypes.c_uint8 * (dot_num * CART_HIGH_DTYPE.itemsize)).from_address(data_ptr) + points = np.frombuffer(buf, dtype=CART_HIGH_DTYPE, count=dot_num) + + xyz = np.column_stack([points["x"], points["y"], points["z"]]).astype(np.float32) / 1000.0 + return xyz, points["reflectivity"].copy(), points["tag"].copy() + + +def parse_cartesian_low_points( + packet: LivoxLidarEthernetPacket, +) -> tuple[np.ndarray, np.ndarray, np.ndarray]: + """Parse low-res cartesian points. Returns xyz in meters.""" + dot_num = packet.dot_num + if dot_num == 0: + empty = np.empty((0, 3), dtype=np.float32) + return empty, np.empty(0, dtype=np.uint8), np.empty(0, dtype=np.uint8) + + data_ptr = ctypes.addressof(packet) + LivoxLidarEthernetPacket.data.offset + buf = (ctypes.c_uint8 * (dot_num * CART_LOW_DTYPE.itemsize)).from_address(data_ptr) + points = np.frombuffer(buf, dtype=CART_LOW_DTYPE, count=dot_num) + + xyz = np.column_stack([points["x"], points["y"], points["z"]]).astype(np.float32) / 100.0 + return xyz, points["reflectivity"].copy(), points["tag"].copy() + + +def parse_imu_data(packet: LivoxLidarEthernetPacket) -> np.ndarray: + """Parse IMU data from a packet. Returns structured array with gyro/accel fields.""" + dot_num = packet.dot_num + if dot_num == 0: + return np.empty(0, dtype=IMU_DTYPE) + + data_ptr = ctypes.addressof(packet) + LivoxLidarEthernetPacket.data.offset + buf = (ctypes.c_uint8 * (dot_num * IMU_DTYPE.itemsize)).from_address(data_ptr) + return np.frombuffer(buf, dtype=IMU_DTYPE, count=dot_num).copy() + + +def get_packet_timestamp_ns(packet: LivoxLidarEthernetPacket) -> int: + """Extract the 64-bit nanosecond timestamp from a packet.""" + return int.from_bytes(bytes(packet.timestamp), byteorder="little") + + +# --------------------------------------------------------------------------- +# SDK library loader +# --------------------------------------------------------------------------- + +_SDK_LIB_NAMES = [ + "livox_lidar_sdk_shared", + "liblivox_lidar_sdk_shared.so", + "liblivox_lidar_sdk_shared.so.2", +] + +_SDK_SEARCH_PATHS = [ + "/usr/local/lib", + "/usr/lib", + "/usr/lib/x86_64-linux-gnu", + "/usr/lib/aarch64-linux-gnu", +] + + +def _find_sdk_library(lib_path: str | None = None) -> str: + """Find the Livox SDK2 shared library. + + Search order: + 1. Explicit lib_path argument + 2. LIVOX_SDK2_LIB_PATH environment variable + 3. ctypes.util.find_library + 4. Well-known system paths + """ + if lib_path and Path(lib_path).exists(): + return lib_path + + env_path = os.environ.get("LIVOX_SDK2_LIB_PATH") + if env_path and Path(env_path).exists(): + return env_path + + for name in _SDK_LIB_NAMES: + found = ctypes.util.find_library(name) + if found: + return found + + for search_dir in _SDK_SEARCH_PATHS: + for name in _SDK_LIB_NAMES: + candidate = Path(search_dir) / name + if candidate.exists(): + return str(candidate) + + raise RuntimeError( + "Livox SDK2 shared library not found. " + "Build from https://github.com/Livox-SDK/Livox-SDK2 and install, " + "or set LIVOX_SDK2_LIB_PATH to the .so file path." + ) + + +def load_sdk(lib_path: str | None = None) -> ctypes.CDLL: + """Load the Livox SDK2 shared library and set up function signatures.""" + path = _find_sdk_library(lib_path) + lib = ctypes.CDLL(path) + logger.info("Loaded Livox SDK2 from %s", path) + + # --- SDK lifecycle --- + lib.LivoxLidarSdkInit.argtypes = [ctypes.c_char_p, ctypes.c_char_p, ctypes.c_void_p] + lib.LivoxLidarSdkInit.restype = ctypes.c_bool + + lib.LivoxLidarSdkStart.argtypes = [] + lib.LivoxLidarSdkStart.restype = ctypes.c_bool + + lib.LivoxLidarSdkUninit.argtypes = [] + lib.LivoxLidarSdkUninit.restype = None + + # --- Callbacks --- + lib.SetLivoxLidarPointCloudCallBack.argtypes = [PointCloudCallbackType, ctypes.c_void_p] + lib.SetLivoxLidarPointCloudCallBack.restype = None + + lib.SetLivoxLidarImuDataCallback.argtypes = [ImuDataCallbackType, ctypes.c_void_p] + lib.SetLivoxLidarImuDataCallback.restype = None + + lib.SetLivoxLidarInfoChangeCallback.argtypes = [InfoChangeCallbackType, ctypes.c_void_p] + lib.SetLivoxLidarInfoChangeCallback.restype = None + + lib.SetLivoxLidarInfoCallback.argtypes = [InfoCallbackType, ctypes.c_void_p] + lib.SetLivoxLidarInfoCallback.restype = None + + # --- Device configuration --- + lib.SetLivoxLidarWorkMode.argtypes = [ + ctypes.c_uint32, + ctypes.c_int32, + AsyncControlCallbackType, + ctypes.c_void_p, + ] + lib.SetLivoxLidarWorkMode.restype = ctypes.c_int32 + + lib.EnableLivoxLidarImuData.argtypes = [ + ctypes.c_uint32, + AsyncControlCallbackType, + ctypes.c_void_p, + ] + lib.EnableLivoxLidarImuData.restype = ctypes.c_int32 + + lib.DisableLivoxLidarImuData.argtypes = [ + ctypes.c_uint32, + AsyncControlCallbackType, + ctypes.c_void_p, + ] + lib.DisableLivoxLidarImuData.restype = ctypes.c_int32 + + lib.SetLivoxLidarPclDataType.argtypes = [ + ctypes.c_uint32, + ctypes.c_int32, + AsyncControlCallbackType, + ctypes.c_void_p, + ] + lib.SetLivoxLidarPclDataType.restype = ctypes.c_int32 + + lib.SetLivoxLidarScanPattern.argtypes = [ + ctypes.c_uint32, + ctypes.c_int32, + AsyncControlCallbackType, + ctypes.c_void_p, + ] + lib.SetLivoxLidarScanPattern.restype = ctypes.c_int32 + + # --- Console logging --- + lib.DisableLivoxSdkConsoleLogger.argtypes = [] + lib.DisableLivoxSdkConsoleLogger.restype = None + + return lib + + +class LivoxSDK: + """Convenience wrapper managing SDK lifecycle and callback registration. + + Prevents garbage collection of ctypes callback references (which would + cause segfaults when the SDK tries to call them from C threads). + """ + + def __init__(self, lib_path: str | None = None) -> None: + self._lib = load_sdk(lib_path) + self._initialized = False + # prevent GC of callback C function pointers + self._callbacks: dict[str, Any] = {} + + @property + def lib(self) -> ctypes.CDLL: + return self._lib + + def init(self, config_path: str, host_ip: str, quiet: bool = True) -> None: + """Initialize the SDK with a JSON config file and host IP.""" + if self._initialized: + raise RuntimeError("SDK already initialized. Call uninit() first.") + if quiet: + self._lib.DisableLivoxSdkConsoleLogger() + ok = self._lib.LivoxLidarSdkInit( + config_path.encode(), + host_ip.encode(), + None, + ) + if not ok: + raise RuntimeError( + f"LivoxLidarSdkInit failed (config={config_path}, host_ip={host_ip})" + ) + self._initialized = True + + def start(self) -> None: + """Start the SDK (begins device communication on background threads).""" + if not self._initialized: + raise RuntimeError("SDK not initialized. Call init() first.") + ok = self._lib.LivoxLidarSdkStart() + if not ok: + raise RuntimeError("LivoxLidarSdkStart failed") + + def uninit(self) -> None: + """Shut down the SDK and release resources.""" + if self._initialized: + self._lib.LivoxLidarSdkUninit() + self._initialized = False + self._callbacks.clear() + + def set_point_cloud_callback(self, callback: Any) -> None: + self._callbacks["pointcloud"] = callback # prevent GC + self._lib.SetLivoxLidarPointCloudCallBack(callback, None) + + def set_imu_callback(self, callback: Any) -> None: + self._callbacks["imu"] = callback + self._lib.SetLivoxLidarImuDataCallback(callback, None) + + def set_info_change_callback(self, callback: Any) -> None: + self._callbacks["info_change"] = callback + self._lib.SetLivoxLidarInfoChangeCallback(callback, None) + + def set_info_callback(self, callback: Any) -> None: + self._callbacks["info"] = callback + self._lib.SetLivoxLidarInfoCallback(callback, None) + + def set_work_mode(self, handle: int, mode: LivoxLidarWorkMode, callback: Any = None) -> int: + cb = callback or AsyncControlCallbackType(0) # type: ignore[arg-type] + if callback: + self._callbacks[f"work_mode_{handle}"] = callback + return self._lib.SetLivoxLidarWorkMode(handle, int(mode), cb, None) # type: ignore[no-any-return] + + def enable_imu_data(self, handle: int, callback: Any = None) -> int: + cb = callback or AsyncControlCallbackType(0) # type: ignore[arg-type] + if callback: + self._callbacks[f"imu_enable_{handle}"] = callback + return self._lib.EnableLivoxLidarImuData(handle, cb, None) # type: ignore[no-any-return] + + def disable_imu_data(self, handle: int, callback: Any = None) -> int: + cb = callback or AsyncControlCallbackType(0) # type: ignore[arg-type] + if callback: + self._callbacks[f"imu_disable_{handle}"] = callback + return self._lib.DisableLivoxLidarImuData(handle, cb, None) # type: ignore[no-any-return] diff --git a/dimos/hardware/sensors/lidar/module.py b/dimos/hardware/sensors/lidar/module.py new file mode 100644 index 0000000000..d637ea347a --- /dev/null +++ b/dimos/hardware/sensors/lidar/module.py @@ -0,0 +1,147 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""LiDAR module wrappers that convert LidarHardware observables into module streams.""" + +from __future__ import annotations + +from dataclasses import dataclass, field +import time +from typing import TYPE_CHECKING, Any + +import reactivex as rx +from reactivex import operators as ops + +from dimos.core import Module, ModuleConfig, Out, rpc +from dimos.msgs.geometry_msgs import Quaternion, Transform, Vector3 +from dimos.spec import perception + +if TYPE_CHECKING: + from collections.abc import Callable + + from dimos.hardware.sensors.lidar.spec import LidarHardware + from dimos.msgs.sensor_msgs.Imu import Imu + from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + + +def default_lidar_transform() -> Transform: + return Transform( + translation=Vector3(0.0, 0.0, 0.0), + rotation=Quaternion(0.0, 0.0, 0.0, 1.0), + frame_id="base_link", + child_frame_id="lidar_link", + ) + + +@dataclass +class LidarModuleConfig(ModuleConfig): + frame_id: str = "lidar_link" + transform: Transform | None = field(default_factory=default_lidar_transform) + hardware: Callable[[], LidarHardware[Any]] | LidarHardware[Any] | None = None + frequency: float = 0.0 # Hz, 0 means no limit + + +class LidarModule(Module[LidarModuleConfig], perception.Lidar): + """Generic LiDAR module — pointcloud only. + + Publishes PointCloud2 messages and TF transforms. + """ + + pointcloud: Out[PointCloud2] + + hardware: LidarHardware[Any] + + config: LidarModuleConfig + default_config = LidarModuleConfig + + @rpc + def start(self) -> None: + super().start() + self._init_hardware() + + stream = self.hardware.pointcloud_stream() + + if self.config.frequency > 0: + stream = stream.pipe(ops.sample(1.0 / self.config.frequency)) + + self._disposables.add( + stream.subscribe(lambda pc: self.pointcloud.publish(pc)), + ) + + self._disposables.add( + rx.interval(1.0).subscribe(lambda _: self._publish_tf()), + ) + + def _init_hardware(self) -> None: + if callable(self.config.hardware): + self.hardware = self.config.hardware() + else: + self.hardware = self.config.hardware # type: ignore[assignment] + + def _publish_tf(self) -> None: + if not self.config.transform: + return + ts = time.time() + lidar_link = self.config.transform + lidar_link.ts = ts + self.tf.publish(lidar_link) + + def stop(self) -> None: + if self.hardware and hasattr(self.hardware, "stop"): + self.hardware.stop() + super().stop() + + +# --------------------------------------------------------------------------- +# Livox-specific: LiDAR + IMU +# --------------------------------------------------------------------------- + + +@dataclass +class LivoxLidarModuleConfig(LidarModuleConfig): + enable_imu: bool = True + + +class LivoxLidarModule(LidarModule, perception.IMU): + """Livox LiDAR module — pointcloud + IMU. + + Extends LidarModule with IMU stream support for sensors like the Mid-360. + """ + + imu: Out[Imu] + + config: LivoxLidarModuleConfig # type: ignore[assignment] + default_config = LivoxLidarModuleConfig # type: ignore[assignment] + + @rpc + def start(self) -> None: + super().start() + + if self.config.enable_imu: + imu_stream = self.hardware.imu_stream() + if imu_stream is not None: + self._disposables.add( + imu_stream.subscribe(lambda imu_msg: self.imu.publish(imu_msg)), + ) + + +lidar_module = LidarModule.blueprint +livox_lidar_module = LivoxLidarModule.blueprint + +__all__ = [ + "LidarModule", + "LivoxLidarModule", + "lidar_module", + "livox_lidar_module", +] diff --git a/dimos/hardware/sensors/lidar/spec.py b/dimos/hardware/sensors/lidar/spec.py new file mode 100644 index 0000000000..d01b451479 --- /dev/null +++ b/dimos/hardware/sensors/lidar/spec.py @@ -0,0 +1,43 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from abc import ABC, abstractmethod +from typing import Generic, Protocol, TypeVar + +from reactivex.observable import Observable + +from dimos.msgs.sensor_msgs.Imu import Imu +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.protocol.service import Configurable # type: ignore[attr-defined] + + +class LidarConfig(Protocol): + frame_id_prefix: str | None + frequency: float # Hz, point cloud output rate + + +LidarConfigT = TypeVar("LidarConfigT", bound=LidarConfig) + + +class LidarHardware(ABC, Configurable[LidarConfigT], Generic[LidarConfigT]): + """Abstract base class for LiDAR hardware drivers.""" + + @abstractmethod + def pointcloud_stream(self) -> Observable[PointCloud2]: + """Observable stream of point clouds from the LiDAR.""" + pass + + def imu_stream(self) -> Observable[Imu] | None: + """Optional observable stream of IMU data. Returns None if unsupported.""" + return None diff --git a/dimos/msgs/sensor_msgs/Imu.py b/dimos/msgs/sensor_msgs/Imu.py new file mode 100644 index 0000000000..0fadc3f3f7 --- /dev/null +++ b/dimos/msgs/sensor_msgs/Imu.py @@ -0,0 +1,123 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from __future__ import annotations + +import time + +from dimos_lcm.sensor_msgs.Imu import Imu as LCMImu + +from dimos.msgs.geometry_msgs import Quaternion, Vector3 +from dimos.types.timestamped import Timestamped + + +def _sec_nsec(ts: float) -> tuple[int, int]: + s = int(ts) + return s, int((ts - s) * 1_000_000_000) + + +class Imu(Timestamped): + """IMU sensor message mirroring ROS sensor_msgs/Imu. + + Contains orientation, angular velocity, and linear acceleration + with optional covariance matrices (3x3 row-major as flat 9-element lists). + """ + + msg_name = "sensor_msgs.Imu" + + def __init__( + self, + angular_velocity: Vector3 | None = None, + linear_acceleration: Vector3 | None = None, + orientation: Quaternion | None = None, + orientation_covariance: list[float] | None = None, + angular_velocity_covariance: list[float] | None = None, + linear_acceleration_covariance: list[float] | None = None, + frame_id: str = "imu_link", + ts: float | None = None, + ) -> None: + self.ts = ts if ts is not None else time.time() # type: ignore[assignment] + self.frame_id = frame_id + self.angular_velocity = angular_velocity or Vector3(0.0, 0.0, 0.0) + self.linear_acceleration = linear_acceleration or Vector3(0.0, 0.0, 0.0) + self.orientation = orientation or Quaternion(0.0, 0.0, 0.0, 1.0) + self.orientation_covariance = orientation_covariance or [0.0] * 9 + self.angular_velocity_covariance = angular_velocity_covariance or [0.0] * 9 + self.linear_acceleration_covariance = linear_acceleration_covariance or [0.0] * 9 + + def lcm_encode(self) -> bytes: + msg = LCMImu() + msg.header.stamp.sec, msg.header.stamp.nsec = _sec_nsec(self.ts) + msg.header.frame_id = self.frame_id + + msg.orientation.x = self.orientation.x + msg.orientation.y = self.orientation.y + msg.orientation.z = self.orientation.z + msg.orientation.w = self.orientation.w + msg.orientation_covariance = self.orientation_covariance + + msg.angular_velocity.x = self.angular_velocity.x + msg.angular_velocity.y = self.angular_velocity.y + msg.angular_velocity.z = self.angular_velocity.z + msg.angular_velocity_covariance = self.angular_velocity_covariance + + msg.linear_acceleration.x = self.linear_acceleration.x + msg.linear_acceleration.y = self.linear_acceleration.y + msg.linear_acceleration.z = self.linear_acceleration.z + msg.linear_acceleration_covariance = self.linear_acceleration_covariance + + return msg.lcm_encode() # type: ignore[no-any-return] + + @classmethod + def lcm_decode(cls, data: bytes) -> Imu: + msg = LCMImu.lcm_decode(data) + ts = msg.header.stamp.sec + (msg.header.stamp.nsec / 1_000_000_000) + return cls( + angular_velocity=Vector3( + msg.angular_velocity.x, + msg.angular_velocity.y, + msg.angular_velocity.z, + ), + linear_acceleration=Vector3( + msg.linear_acceleration.x, + msg.linear_acceleration.y, + msg.linear_acceleration.z, + ), + orientation=Quaternion( + msg.orientation.x, + msg.orientation.y, + msg.orientation.z, + msg.orientation.w, + ), + orientation_covariance=list(msg.orientation_covariance), + angular_velocity_covariance=list(msg.angular_velocity_covariance), + linear_acceleration_covariance=list(msg.linear_acceleration_covariance), + frame_id=msg.header.frame_id, + ts=ts, + ) + + def __str__(self) -> str: + return ( + f"Imu(frame_id='{self.frame_id}', " + f"gyro=({self.angular_velocity.x:.3f}, {self.angular_velocity.y:.3f}, {self.angular_velocity.z:.3f}), " + f"accel=({self.linear_acceleration.x:.3f}, {self.linear_acceleration.y:.3f}, {self.linear_acceleration.z:.3f}))" + ) + + def __repr__(self) -> str: + return ( + f"Imu(ts={self.ts}, frame_id='{self.frame_id}', " + f"angular_velocity={self.angular_velocity}, " + f"linear_acceleration={self.linear_acceleration}, " + f"orientation={self.orientation})" + ) diff --git a/dimos/msgs/sensor_msgs/PointCloud2.py b/dimos/msgs/sensor_msgs/PointCloud2.py index 39a0146e4c..911c66997e 100644 --- a/dimos/msgs/sensor_msgs/PointCloud2.py +++ b/dimos/msgs/sensor_msgs/PointCloud2.py @@ -611,20 +611,18 @@ def to_rerun( voxel_size: float = 0.05, colormap: str | None = "turbo", colors: list[int] | None = None, - mode: str = "boxes", + mode: str = "points", size: float | None = None, fill_mode: str = "solid", **kwargs: object, ) -> Archetype: - import rerun as rr - - """Convert to Rerun Points3D or Boxes3D archetype. + """Convert to Rerun archetype for visualization. Args: voxel_size: size for visualization colormap: Optional colormap name (e.g., "turbo", "viridis") to color by height colors: Optional RGB color [r, g, b] for all points (0-255) - mode: Visualization mode - "points" for spheres, "boxes" for cubes (default) + mode: "points" for raw points, "boxes" for cubes (default), or "spheres" for sized spheres size: Box size for mode="boxes" (e.g., voxel_size). Defaults to radii*2. fill_mode: Fill mode for boxes - "solid", "majorwireframe", or "densewireframe" **kwargs: Additional args (ignored for compatibility) @@ -632,14 +630,15 @@ def to_rerun( Returns: rr.Points3D or rr.Boxes3D archetype for logging to Rerun """ + import rerun as rr + points, _ = self.as_numpy() if len(points) == 0: - return rr.Points3D([]) if mode == "points" else rr.Boxes3D(centers=[]) + return rr.Points3D([]) if mode != "boxes" else rr.Boxes3D(centers=[]) # Determine colors point_colors = None if colormap is not None: - # Color by height (z-coordinate) z = points[:, 2] z_norm = (z - z.min()) / (z.max() - z.min() + 1e-8) cmap = _get_matplotlib_cmap(colormap) @@ -647,8 +646,12 @@ def to_rerun( elif colors is not None: point_colors = colors - if mode == "boxes": - # Use boxes for voxel visualization + if mode == "points": + return rr.Points3D( + positions=points, + colors=point_colors, + ) + elif mode == "boxes": box_size = size if size is not None else voxel_size half = box_size / 2 return rr.Boxes3D( diff --git a/dimos/msgs/sensor_msgs/__init__.py b/dimos/msgs/sensor_msgs/__init__.py index b58dda8db5..7fec2d2793 100644 --- a/dimos/msgs/sensor_msgs/__init__.py +++ b/dimos/msgs/sensor_msgs/__init__.py @@ -1,5 +1,6 @@ from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos.msgs.sensor_msgs.Imu import Imu from dimos.msgs.sensor_msgs.JointCommand import JointCommand from dimos.msgs.sensor_msgs.JointState import JointState from dimos.msgs.sensor_msgs.Joy import Joy @@ -10,6 +11,7 @@ "CameraInfo", "Image", "ImageFormat", + "Imu", "JointCommand", "JointState", "Joy", diff --git a/dimos/spec/perception.py b/dimos/spec/perception.py index 4dc682523f..33e3d1dc97 100644 --- a/dimos/spec/perception.py +++ b/dimos/spec/perception.py @@ -15,7 +15,7 @@ from typing import Protocol from dimos.core import Out -from dimos.msgs.sensor_msgs import CameraInfo, Image as ImageMsg, PointCloud2 +from dimos.msgs.sensor_msgs import CameraInfo, Image as ImageMsg, Imu, PointCloud2 class Image(Protocol): @@ -34,3 +34,13 @@ class DepthCamera(Camera): class Pointcloud(Protocol): pointcloud: Out[PointCloud2] + + +class IMU(Protocol): + imu: Out[Imu] + + +class Lidar(Pointcloud, Protocol): + """LiDAR sensor providing point clouds.""" + + pass diff --git a/dimos/spec/utils.py b/dimos/spec/utils.py index b9786b91b5..31b6d3e5b5 100644 --- a/dimos/spec/utils.py +++ b/dimos/spec/utils.py @@ -13,6 +13,7 @@ # limitations under the License. import inspect +import typing from typing import Any, Protocol, runtime_checkable from annotation_protocol import AnnotationProtocol # type: ignore[import-not-found,import-untyped] @@ -99,6 +100,31 @@ def foo(self) -> int: ... return isinstance(obj, strict_proto) +def assert_implements_protocol(cls: type, protocol: type) -> None: + """Assert that cls has all annotations required by a Protocol. + + Works with any Protocol (not just Spec subclasses). Checks that every + annotation defined by the protocol is present on cls with a matching type. + + Example: + class MyProto(Protocol): + x: Out[int] + + class Good: + x: Out[int] + + assert_implements_protocol(Good, MyProto) # passes + """ + proto_hints = typing.get_type_hints(protocol, include_extras=True) + cls_hints = typing.get_type_hints(cls, include_extras=True) + + for name, expected_type in proto_hints.items(): + assert name in cls_hints, f"{cls.__name__} missing '{name}' required by {protocol.__name__}" + assert cls_hints[name] == expected_type, ( + f"{cls.__name__}.{name}: expected {expected_type}, got {cls_hints[name]}" + ) + + def get_protocol_method_signatures(proto: type[object]) -> dict[str, inspect.Signature]: """ Return a mapping of method_name -> inspect.Signature diff --git a/dimos/visualization/rerun/bridge.py b/dimos/visualization/rerun/bridge.py index 5263e59a44..af8c4de4cf 100644 --- a/dimos/visualization/rerun/bridge.py +++ b/dimos/visualization/rerun/bridge.py @@ -255,6 +255,7 @@ def start(self) -> None: # Initialize and spawn Rerun viewer rr.init("dimos") + if self.config.viewer_mode == "native": rr.spawn(connect=True, memory_limit=self.config.memory_limit) elif self.config.viewer_mode == "web": @@ -309,35 +310,6 @@ def run_bridge( # any pubsub that supports subscribe_all and topic that supports str(topic) # is acceptable here pubsubs=[LCM(autoconf=True)], - # Custom converters for specific rerun entity paths - # Normally all these would be specified in their respectative modules - # Until this is implemented we have central overrides here - # - # This is unsustainable once we move to multi robot etc - visual_override={ - "world/camera_info": lambda camera_info: camera_info.to_rerun( - image_topic="/world/color_image", - optical_frame="camera_optical", - ), - "world/global_map": lambda grid: grid.to_rerun(voxel_size=0.1), - "world/navigation_costmap": lambda grid: grid.to_rerun( - colormap="Accent", - z_offset=0.015, - opacity=0.2, - background="#484981", - ), - }, - # slapping a go2 shaped box on top of tf/base_link - static={ - "world/tf/base_link": lambda rr: [ - rr.Boxes3D( - half_sizes=[0.35, 0.155, 0.2], - colors=[(0, 255, 127)], - fill_mode="wireframe", - ), - rr.Transform3D(parent_frame="tf#/base_link"), - ] - }, ) bridge.start() @@ -346,7 +318,7 @@ def run_bridge( signal.pause() -def _cli( +def main( viewer_mode: str = typer.Option( "native", help="Viewer mode: native (desktop), web (browser), none (headless)" ), @@ -359,7 +331,7 @@ def _cli( if __name__ == "__main__": - typer.run(_cli) + typer.run(main) # you don't need to include this in your blueprint if you are not creating a # custom rerun configuration for your deployment, you can also run rerun-bridge standalone From 6618eede4cf7f29925fc14d937a62ff77c3eb695 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Thu, 12 Feb 2026 03:31:43 +0800 Subject: [PATCH 02/48] Fix LiDAR module port resolution broken by `from __future__ import annotations` The future annotations import made Out[PointCloud2] and Out[Imu] lazy strings, preventing the Module metaclass from creating real port descriptors. Move type imports out of TYPE_CHECKING block so ports are properly resolved at class definition time. --- dimos/hardware/sensors/lidar/module.py | 15 +++++---------- 1 file changed, 5 insertions(+), 10 deletions(-) diff --git a/dimos/hardware/sensors/lidar/module.py b/dimos/hardware/sensors/lidar/module.py index d637ea347a..9a277f42ae 100644 --- a/dimos/hardware/sensors/lidar/module.py +++ b/dimos/hardware/sensors/lidar/module.py @@ -14,26 +14,21 @@ """LiDAR module wrappers that convert LidarHardware observables into module streams.""" -from __future__ import annotations - +from collections.abc import Callable from dataclasses import dataclass, field import time -from typing import TYPE_CHECKING, Any +from typing import Any import reactivex as rx from reactivex import operators as ops from dimos.core import Module, ModuleConfig, Out, rpc +from dimos.hardware.sensors.lidar.spec import LidarHardware from dimos.msgs.geometry_msgs import Quaternion, Transform, Vector3 +from dimos.msgs.sensor_msgs.Imu import Imu +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.spec import perception -if TYPE_CHECKING: - from collections.abc import Callable - - from dimos.hardware.sensors.lidar.spec import LidarHardware - from dimos.msgs.sensor_msgs.Imu import Imu - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 - def default_lidar_transform() -> Transform: return Transform( From 2746433a3592c82b4679c923bf6a711f7aaab1d2 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Thu, 12 Feb 2026 03:58:31 +0800 Subject: [PATCH 03/48] Move LivoxLidarModule into livox/ directory Livox-specific module (LivoxLidarModule, LivoxLidarModuleConfig) now lives in dimos/hardware/sensors/lidar/livox/module.py alongside the driver and SDK code. Generic LidarModule stays in the parent. --- dimos/hardware/sensors/lidar/livox/module.py | 59 ++++++++++++++ .../lidar/livox/test_spec_compliance.py | 29 +++++++ dimos/hardware/sensors/lidar/module.py | 38 +-------- test_livox_hw.py | 80 +++++++++++++++++++ 4 files changed, 169 insertions(+), 37 deletions(-) create mode 100644 dimos/hardware/sensors/lidar/livox/module.py create mode 100644 dimos/hardware/sensors/lidar/livox/test_spec_compliance.py create mode 100644 test_livox_hw.py diff --git a/dimos/hardware/sensors/lidar/livox/module.py b/dimos/hardware/sensors/lidar/livox/module.py new file mode 100644 index 0000000000..bdde07225f --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/module.py @@ -0,0 +1,59 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Livox-specific LiDAR module with IMU support.""" + +from dataclasses import dataclass + +from dimos.core import Out, rpc +from dimos.hardware.sensors.lidar.module import LidarModule, LidarModuleConfig +from dimos.msgs.sensor_msgs.Imu import Imu +from dimos.spec import perception + + +@dataclass +class LivoxLidarModuleConfig(LidarModuleConfig): + enable_imu: bool = True + + +class LivoxLidarModule(LidarModule, perception.IMU): + """Livox LiDAR module — pointcloud + IMU. + + Extends LidarModule with IMU stream support for sensors like the Mid-360. + """ + + imu: Out[Imu] + + config: LivoxLidarModuleConfig # type: ignore[assignment] + default_config = LivoxLidarModuleConfig # type: ignore[assignment] + + @rpc + def start(self) -> None: + super().start() + + if self.config.enable_imu: + imu_stream = self.hardware.imu_stream() + if imu_stream is not None: + self._disposables.add( + imu_stream.subscribe(lambda imu_msg: self.imu.publish(imu_msg)), + ) + + +livox_lidar_module = LivoxLidarModule.blueprint + +__all__ = [ + "LivoxLidarModule", + "LivoxLidarModuleConfig", + "livox_lidar_module", +] diff --git a/dimos/hardware/sensors/lidar/livox/test_spec_compliance.py b/dimos/hardware/sensors/lidar/livox/test_spec_compliance.py new file mode 100644 index 0000000000..79abb58dbd --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/test_spec_compliance.py @@ -0,0 +1,29 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Spec compliance tests for the Livox LiDAR module.""" + +from __future__ import annotations + +from dimos.hardware.sensors.lidar.livox.module import LivoxLidarModule +from dimos.spec import perception +from dimos.spec.utils import assert_implements_protocol + + +def test_livox_module_implements_lidar_spec() -> None: + assert_implements_protocol(LivoxLidarModule, perception.Lidar) + + +def test_livox_module_implements_imu_spec() -> None: + assert_implements_protocol(LivoxLidarModule, perception.IMU) diff --git a/dimos/hardware/sensors/lidar/module.py b/dimos/hardware/sensors/lidar/module.py index 9a277f42ae..660044174b 100644 --- a/dimos/hardware/sensors/lidar/module.py +++ b/dimos/hardware/sensors/lidar/module.py @@ -25,7 +25,6 @@ from dimos.core import Module, ModuleConfig, Out, rpc from dimos.hardware.sensors.lidar.spec import LidarHardware from dimos.msgs.geometry_msgs import Quaternion, Transform, Vector3 -from dimos.msgs.sensor_msgs.Imu import Imu from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.spec import perception @@ -98,45 +97,10 @@ def stop(self) -> None: super().stop() -# --------------------------------------------------------------------------- -# Livox-specific: LiDAR + IMU -# --------------------------------------------------------------------------- - - -@dataclass -class LivoxLidarModuleConfig(LidarModuleConfig): - enable_imu: bool = True - - -class LivoxLidarModule(LidarModule, perception.IMU): - """Livox LiDAR module — pointcloud + IMU. - - Extends LidarModule with IMU stream support for sensors like the Mid-360. - """ - - imu: Out[Imu] - - config: LivoxLidarModuleConfig # type: ignore[assignment] - default_config = LivoxLidarModuleConfig # type: ignore[assignment] - - @rpc - def start(self) -> None: - super().start() - - if self.config.enable_imu: - imu_stream = self.hardware.imu_stream() - if imu_stream is not None: - self._disposables.add( - imu_stream.subscribe(lambda imu_msg: self.imu.publish(imu_msg)), - ) - - lidar_module = LidarModule.blueprint -livox_lidar_module = LivoxLidarModule.blueprint __all__ = [ "LidarModule", - "LivoxLidarModule", + "LidarModuleConfig", "lidar_module", - "livox_lidar_module", ] diff --git a/test_livox_hw.py b/test_livox_hw.py new file mode 100644 index 0000000000..4384454302 --- /dev/null +++ b/test_livox_hw.py @@ -0,0 +1,80 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Deploy LivoxLidarModule with LCM transport for pointcloud and IMU.""" + +import time + +from dimos.core import In, Module, start +from dimos.core.transport import LCMTransport +from dimos.hardware.sensors.lidar.livox.mid360 import LivoxMid360 +from dimos.hardware.sensors.lidar.livox.module import LivoxLidarModule +from dimos.msgs.sensor_msgs.Imu import Imu +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + +pc_count = 0 +imu_count = 0 + + +class LidarListener(Module): + pointcloud: In[PointCloud2] + imu: In[Imu] + + def start(self): + super().start() + self.pointcloud.subscribe(self._on_pc) + self.imu.subscribe(self._on_imu) + + def _on_pc(self, pc): + global pc_count + pc_count += 1 + n = len(pc.pointcloud.points) if hasattr(pc.pointcloud, "points") else "?" + print(f" [PC #{pc_count}] {n} points, ts={pc.ts:.3f}", flush=True) + + def _on_imu(self, imu_msg): + global imu_count + imu_count += 1 + if imu_count % 200 == 1: + print( + f" [IMU #{imu_count}] acc=({imu_msg.linear_acceleration.x:.2f}, " + f"{imu_msg.linear_acceleration.y:.2f}, {imu_msg.linear_acceleration.z:.2f})", + flush=True, + ) + + +if __name__ == "__main__": + dimos = start(2) + + lidar = dimos.deploy( + LivoxLidarModule, + hardware=lambda: LivoxMid360(host_ip="192.168.1.5", lidar_ips=["192.168.1.155"]), + ) + listener = dimos.deploy(LidarListener) + + lidar.pointcloud.transport = LCMTransport("/lidar/pointcloud", PointCloud2) + lidar.imu.transport = LCMTransport("/lidar/imu", Imu) + listener.pointcloud.transport = LCMTransport("/lidar/pointcloud", PointCloud2) + listener.imu.transport = LCMTransport("/lidar/imu", Imu) + + lidar.start() + listener.start() + + print("LivoxLidarModule + listener running. Ctrl+C to stop.", flush=True) + try: + while True: + time.sleep(1) + except KeyboardInterrupt: + pass + + dimos.stop() From 1bbb3fb1c5273abfca089a08ff35d92371f341b6 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Thu, 12 Feb 2026 15:35:19 +0800 Subject: [PATCH 04/48] livox voxelization --- dimos/hardware/sensors/lidar/livox/mid360.py | 9 ++++++++- dimos/msgs/sensor_msgs/PointCloud2.py | 5 +++++ .../unitree_webrtc/unitree_go2_blueprints.py | 2 +- dimos/visualization/rerun/bridge.py | 5 +++++ test_livox_hw.py | 16 ++++++++++++++-- 5 files changed, 33 insertions(+), 4 deletions(-) diff --git a/dimos/hardware/sensors/lidar/livox/mid360.py b/dimos/hardware/sensors/lidar/livox/mid360.py index 9c06c4af20..dbf8a5d81b 100644 --- a/dimos/hardware/sensors/lidar/livox/mid360.py +++ b/dimos/hardware/sensors/lidar/livox/mid360.py @@ -68,7 +68,7 @@ class LivoxMid360Config(LidarConfig): host_ip: str = "192.168.1.5" lidar_ips: list[str] = field(default_factory=lambda: ["192.168.1.155"]) - frequency: float = 10.0 # Hz, point cloud output rate + frequency: float = 5.0 # Hz, point cloud output rate frame_id: str = "lidar_link" frame_id_prefix: str | None = None enable_imu: bool = True @@ -86,6 +86,9 @@ class LivoxMid360Config(LidarConfig): host_imu_data_port: int = 56401 host_log_data_port: int = 56501 + # Voxel downsampling size in meters (None = no downsampling) + voxel_size: float | None = None + # Socket buffer size (bytes) to avoid packet loss at high data rates recv_buf_size: int = 4 * 1024 * 1024 # 4 MB @@ -334,6 +337,10 @@ def _emit_pointcloud( dtype=o3c.float32, ) + # Optional voxel downsampling + if self.config.voxel_size is not None: + pcd_t = pcd_t.voxel_down_sample(self.config.voxel_size) + pc2 = PointCloud2( pointcloud=pcd_t, frame_id=self._frame_id(self.config.frame_id), diff --git a/dimos/msgs/sensor_msgs/PointCloud2.py b/dimos/msgs/sensor_msgs/PointCloud2.py index 911c66997e..7b087356ec 100644 --- a/dimos/msgs/sensor_msgs/PointCloud2.py +++ b/dimos/msgs/sensor_msgs/PointCloud2.py @@ -654,6 +654,11 @@ def to_rerun( elif mode == "boxes": box_size = size if size is not None else voxel_size half = box_size / 2 + # Snap points to voxel grid centers so boxes tile properly + points = np.floor(points / box_size) * box_size + half + points, unique_idx = np.unique(points, axis=0, return_index=True) + if point_colors is not None and isinstance(point_colors, np.ndarray): + point_colors = point_colors[unique_idx] return rr.Boxes3D( centers=points, half_sizes=[half, half, half], diff --git a/dimos/robot/unitree_webrtc/unitree_go2_blueprints.py b/dimos/robot/unitree_webrtc/unitree_go2_blueprints.py index 56a3710dcf..96f722ad98 100644 --- a/dimos/robot/unitree_webrtc/unitree_go2_blueprints.py +++ b/dimos/robot/unitree_webrtc/unitree_go2_blueprints.py @@ -88,7 +88,7 @@ image_topic="/world/color_image", optical_frame="camera_optical", ), - "world/global_map": lambda grid: grid.to_rerun(voxel_size=0.1), + "world/global_map": lambda grid: grid.to_rerun(voxel_size=0.1, mode="boxes"), "world/navigation_costmap": lambda grid: grid.to_rerun( colormap="Accent", z_offset=0.015, diff --git a/dimos/visualization/rerun/bridge.py b/dimos/visualization/rerun/bridge.py index af8c4de4cf..4e39f0b64c 100644 --- a/dimos/visualization/rerun/bridge.py +++ b/dimos/visualization/rerun/bridge.py @@ -247,6 +247,11 @@ def _on_message(self, msg: Any, topic: Any) -> None: else: rr.log(entity_path, cast("Archetype", rerun_data)) + # # Connect entity to its TF frame so transforms apply correctly + # frame_id = getattr(msg, "frame_id", None) + # if frame_id and not is_rerun_multi(rerun_data): + # rr.log(entity_path, rr.Transform3D(parent_frame="tf#/" + frame_id)) + @rpc def start(self) -> None: import rerun as rr diff --git a/test_livox_hw.py b/test_livox_hw.py index 4384454302..75daacff38 100644 --- a/test_livox_hw.py +++ b/test_livox_hw.py @@ -22,6 +22,7 @@ from dimos.hardware.sensors.lidar.livox.module import LivoxLidarModule from dimos.msgs.sensor_msgs.Imu import Imu from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.visualization.rerun.bridge import RerunBridgeModule pc_count = 0 imu_count = 0 @@ -54,13 +55,23 @@ def _on_imu(self, imu_msg): if __name__ == "__main__": - dimos = start(2) + dimos = start(3) lidar = dimos.deploy( LivoxLidarModule, - hardware=lambda: LivoxMid360(host_ip="192.168.1.5", lidar_ips=["192.168.1.155"]), + hardware=lambda: LivoxMid360( + host_ip="192.168.1.5", + lidar_ips=["192.168.1.155"], + frequency=1.0, # , voxel_size=0.25 + ), ) listener = dimos.deploy(LidarListener) + bridge = dimos.deploy( + RerunBridgeModule, + visual_override={ + # "world/lidar/pointcloud": lambda pc: pc.to_rerun(mode="boxes", voxel_size=0.25), + }, + ) lidar.pointcloud.transport = LCMTransport("/lidar/pointcloud", PointCloud2) lidar.imu.transport = LCMTransport("/lidar/imu", Imu) @@ -69,6 +80,7 @@ def _on_imu(self, imu_msg): lidar.start() listener.start() + bridge.start() print("LivoxLidarModule + listener running. Ctrl+C to stop.", flush=True) try: From 0474e74a464c568c0bd9cbcf394b6bbf7f01b260 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Thu, 12 Feb 2026 19:02:05 +0800 Subject: [PATCH 05/48] native module implementation --- dimos/core/__init__.py | 4 + dimos/core/native_echo.py | 37 ++++++ dimos/core/native_module.py | 202 +++++++++++++++++++++++++++++++ dimos/core/test_native_module.py | 168 +++++++++++++++++++++++++ 4 files changed, 411 insertions(+) create mode 100755 dimos/core/native_echo.py create mode 100644 dimos/core/native_module.py create mode 100644 dimos/core/test_native_module.py diff --git a/dimos/core/__init__.py b/dimos/core/__init__.py index 5a4acc1762..28c63b4683 100644 --- a/dimos/core/__init__.py +++ b/dimos/core/__init__.py @@ -11,6 +11,7 @@ import dimos.core.colors as colors from dimos.core.core import rpc from dimos.core.module import Module, ModuleBase, ModuleConfig, ModuleConfigT +from dimos.core.native_module import LogFormat, NativeModule, NativeModuleConfig from dimos.core.rpc_client import RPCClient from dimos.core.stream import In, Out, RemoteIn, RemoteOut, Transport from dimos.core.transport import ( @@ -39,10 +40,13 @@ "DimosCluster", "In", "LCMTransport", + "LogFormat", "Module", "ModuleBase", "ModuleConfig", "ModuleConfigT", + "NativeModule", + "NativeModuleConfig", "Out", "PubSubTF", "RPCSpec", diff --git a/dimos/core/native_echo.py b/dimos/core/native_echo.py new file mode 100755 index 0000000000..0749ec4703 --- /dev/null +++ b/dimos/core/native_echo.py @@ -0,0 +1,37 @@ +#!/usr/bin/env python3 +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Echo binary for NativeModule tests. + +Dumps CLI args as a JSON log line to stdout, then waits for SIGTERM. +""" + +import json +import os +import signal +import sys + +signal.signal(signal.SIGTERM, lambda *_: sys.exit(0)) + +output_path = os.environ.get("NATIVE_ECHO_OUTPUT") +if output_path: + with open(output_path, "w") as f: + json.dump(sys.argv[1:], f) +else: + json.dump({"event": "echo_args", "args": sys.argv[1:]}, sys.stdout) + sys.stdout.write("\n") + sys.stdout.flush() + +signal.pause() diff --git a/dimos/core/native_module.py b/dimos/core/native_module.py new file mode 100644 index 0000000000..721bb5399e --- /dev/null +++ b/dimos/core/native_module.py @@ -0,0 +1,202 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""NativeModule: blueprint-integrated wrapper for native (C/C++) executables. + +A NativeModule is a thin Python Module subclass that declares In/Out ports +for blueprint wiring but delegates all real work to a managed subprocess. +The native process receives its LCM topic names via CLI args and does +pub/sub directly on the LCM multicast bus. + +Example usage:: + + @dataclass(kw_only=True) + class MyConfig(NativeModuleConfig): + executable: str = "./build/my_module" + some_param: float = 1.0 + + class MyCppModule(NativeModule): + default_config = MyConfig + pointcloud: Out[PointCloud2] + cmd_vel: In[Twist] + + # Works with autoconnect, remappings, etc. + autoconnect( + MyCppModule.blueprint(), + SomeConsumer.blueprint(), + ).build().loop() +""" + +from __future__ import annotations + +from dataclasses import dataclass, field +import enum +import json +import os +import signal +import subprocess +import threading +from typing import IO + +from dimos.core.core import rpc +from dimos.core.module import Module, ModuleConfig +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +class LogFormat(enum.Enum): + TEXT = "text" + JSON = "json" + + +@dataclass(kw_only=True) +class NativeModuleConfig(ModuleConfig): + """Configuration for a native (C/C++) subprocess module.""" + + executable: str + extra_args: list[str] = field(default_factory=list) + extra_env: dict[str, str] = field(default_factory=dict) + cwd: str | None = None + shutdown_timeout: float = 10.0 + log_format: LogFormat = LogFormat.TEXT + + +class NativeModule(Module): + """Module that wraps a native executable as a managed subprocess. + + Subclass this, declare In/Out ports, and set ``default_config`` to a + :class:`NativeModuleConfig` subclass pointing at the executable. + + On ``start()``, the binary is launched with CLI args:: + + -- ... + + The native process should parse these args and pub/sub on the given + LCM topics directly. On ``stop()``, the process receives SIGTERM. + """ + + default_config: type[NativeModuleConfig] = NativeModuleConfig # type: ignore[assignment] + _process: subprocess.Popen[bytes] | None = None + _io_threads: list[threading.Thread] + + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + self._io_threads = [] + + @rpc + def start(self) -> None: + if self._process is not None and self._process.poll() is None: + logger.warning("Native process already running", pid=self._process.pid) + return + + topics = self._collect_topics() + extra = self._build_extra_args() + + cmd = [self.config.executable] + for name, topic_str in topics.items(): + cmd.extend([f"--{name}", topic_str]) + cmd.extend(extra) + cmd.extend(self.config.extra_args) + + env = {**os.environ, **self.config.extra_env} + cwd = self.config.cwd + + logger.info("Starting native process", cmd=" ".join(cmd), cwd=cwd) + self._process = subprocess.Popen( + cmd, + env=env, + cwd=cwd, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + ) + logger.info("Native process started", pid=self._process.pid) + + self._io_threads = [ + self._start_reader(self._process.stdout, "info"), + self._start_reader(self._process.stderr, "warning"), + ] + + @rpc + def stop(self) -> None: + if self._process is not None and self._process.poll() is None: + logger.info("Stopping native process", pid=self._process.pid) + self._process.send_signal(signal.SIGTERM) + try: + self._process.wait(timeout=self.config.shutdown_timeout) + except subprocess.TimeoutExpired: + logger.warning( + "Native process did not exit, sending SIGKILL", pid=self._process.pid + ) + self._process.kill() + self._process.wait(timeout=5) + for t in self._io_threads: + t.join(timeout=2) + self._io_threads = [] + self._process = None + super().stop() + + def _start_reader(self, stream: IO[bytes] | None, level: str) -> threading.Thread: + """Spawn a daemon thread that pipes a subprocess stream through the logger.""" + t = threading.Thread(target=self._read_log_stream, args=(stream, level), daemon=True) + t.start() + return t + + def _read_log_stream(self, stream: IO[bytes] | None, level: str) -> None: + if stream is None: + return + log_fn = getattr(logger, level) + for raw in stream: + line = raw.decode("utf-8", errors="replace").rstrip() + if not line: + continue + if self.config.log_format == LogFormat.JSON: + try: + data = json.loads(line) + event = data.pop("event", line) + log_fn(event, **data) + continue + except (json.JSONDecodeError, TypeError): + # TODO: log a warning about malformed JSON and the line contents + pass + log_fn(line, pid=self._process.pid if self._process else None) + stream.close() + + def _collect_topics(self) -> dict[str, str]: + """Extract LCM topic strings from blueprint-assigned stream transports.""" + topics: dict[str, str] = {} + for name in list(self.inputs) + list(self.outputs): + stream = getattr(self, name, None) + if stream is None: + continue + transport = getattr(stream, "_transport", None) + if transport is None: + continue + topic = getattr(transport, "topic", None) + if topic is not None: + topics[name] = str(topic) + return topics + + def _build_extra_args(self) -> list[str]: + """Override in subclasses to pass additional config as CLI args. + + Called after topic args, before ``config.extra_args``. + """ + return [] + + +__all__ = [ + "NativeModule", + "NativeModuleConfig", +] diff --git a/dimos/core/test_native_module.py b/dimos/core/test_native_module.py new file mode 100644 index 0000000000..8403d0f745 --- /dev/null +++ b/dimos/core/test_native_module.py @@ -0,0 +1,168 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Tests for NativeModule: blueprint wiring, topic collection, CLI arg generation. + +Every test launches the real native_echo.py subprocess via blueprint.build(). +The echo script writes received CLI args to a temp file for assertions. +""" + +from dataclasses import dataclass +import json +import os +from pathlib import Path +import tempfile +import time + +import pytest + +from dimos.core.blueprints import autoconnect +from dimos.core.core import rpc +from dimos.core.module import Module +from dimos.core.native_module import LogFormat, NativeModule, NativeModuleConfig +from dimos.core.stream import In, Out +from dimos.core.transport import LCMTransport +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.sensor_msgs.Imu import Imu +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + +_ECHO = str(Path(__file__).parent / "native_echo.py") + + +@pytest.fixture +def args_file(): + """Temp file where native_echo.py writes the CLI args it received.""" + fd, path = tempfile.mkstemp(suffix=".json", prefix="native_echo_") + os.close(fd) + os.unlink(path) + try: + yield path + finally: + if os.path.exists(path): + os.unlink(path) + + +def read_json_file(path: str) -> dict[str, str]: + """Read and parse --key value pairs from the echo output file.""" + raw: list[str] = json.loads(Path(path).read_text()) + result = {} + i = 0 + while i < len(raw): + if raw[i].startswith("--") and i + 1 < len(raw): + result[raw[i][2:]] = raw[i + 1] + i += 2 + else: + i += 1 + return result + + +# -- Test modules -- + + +@dataclass(kw_only=True) +class StubNativeConfig(NativeModuleConfig): + executable: str = _ECHO + log_format: LogFormat = LogFormat.JSON + some_param: float = 1.5 + + +class StubNativeModule(NativeModule): + default_config = StubNativeConfig + pointcloud: Out[PointCloud2] + imu: Out[Imu] + cmd_vel: In[Twist] + + def _build_extra_args(self) -> list[str]: + cfg: StubNativeConfig = self.config # type: ignore[assignment] + return ["--some_param", str(cfg.some_param)] + + +class StubConsumer(Module): + pointcloud: In[PointCloud2] + imu: In[Imu] + + @rpc + def start(self) -> None: + pass + + +class StubProducer(Module): + cmd_vel: Out[Twist] + + @rpc + def start(self) -> None: + pass + + +def test_manual(dimos_cluster, args_file) -> None: + native_module = dimos_cluster.deploy( + StubNativeModule, + some_param=2.5, + extra_env={"NATIVE_ECHO_OUTPUT": args_file}, + ) + + native_module.pointcloud.transport = LCMTransport("/my/custom/lidar", PointCloud2) + native_module.cmd_vel.transport = LCMTransport("/cmd_vel", Twist) + native_module.start() + time.sleep(1) + native_module.stop() + + assert read_json_file(args_file) == { + "cmd_vel": "/cmd_vel#geometry_msgs.Twist", + "pointcloud": "/my/custom/lidar#sensor_msgs.PointCloud2", + "some_param": "2.5", + } + + +def test_autoconnect(args_file) -> None: + """autoconnect passes correct topic args to the native subprocess.""" + blueprint = autoconnect( + StubNativeModule.blueprint( + some_param=2.5, + extra_env={"NATIVE_ECHO_OUTPUT": args_file}, + ), + StubConsumer.blueprint(), + StubProducer.blueprint(), + ).transports( + { + ("pointcloud", PointCloud2): LCMTransport("/my/custom/lidar", PointCloud2), + }, + ) + + coordinator = blueprint.global_config(viewer_backend="none").build() + try: + # Validate blueprint wiring: all modules deployed + native = coordinator.get_instance(StubNativeModule) + consumer = coordinator.get_instance(StubConsumer) + producer = coordinator.get_instance(StubProducer) + assert native is not None + assert consumer is not None + assert producer is not None + + # Out→In topics match between connected modules + assert native.pointcloud.transport.topic == consumer.pointcloud.transport.topic + assert native.imu.transport.topic == consumer.imu.transport.topic + assert producer.cmd_vel.transport.topic == native.cmd_vel.transport.topic + + # Custom transport was applied + assert native.pointcloud.transport.topic == "/my/custom/lidar" + finally: + coordinator.stop() + + assert read_json_file(args_file) == { + "cmd_vel": "/cmd_vel#geometry_msgs.Twist", + "pointcloud": "/my/custom/lidar#sensor_msgs.PointCloud2", + "imu": "/imu#sensor_msgs.Imu", + "some_param": "2.5", + } From ba87583affa319c85c6cb30209d91991fed10027 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Thu, 12 Feb 2026 19:40:07 +0800 Subject: [PATCH 06/48] livox native module wrap --- .../sensors/lidar/livox/cpp/CMakeLists.txt | 42 ++ .../sensors/lidar/livox/cpp/README.md | 73 +++ .../sensors/lidar/livox/cpp/__init__.py | 0 .../lidar/livox/cpp/dimos_native_module.hpp | 64 +++ .../hardware/sensors/lidar/livox/cpp/main.cpp | 437 ++++++++++++++++++ .../sensors/lidar/livox/cpp/module.py | 136 ++++++ .../sensors/lidar/livox/livox_blueprints.py | 22 + dimos/robot/all_blueprints.py | 4 + 8 files changed, 778 insertions(+) create mode 100644 dimos/hardware/sensors/lidar/livox/cpp/CMakeLists.txt create mode 100644 dimos/hardware/sensors/lidar/livox/cpp/README.md create mode 100644 dimos/hardware/sensors/lidar/livox/cpp/__init__.py create mode 100644 dimos/hardware/sensors/lidar/livox/cpp/dimos_native_module.hpp create mode 100644 dimos/hardware/sensors/lidar/livox/cpp/main.cpp create mode 100644 dimos/hardware/sensors/lidar/livox/cpp/module.py create mode 100644 dimos/hardware/sensors/lidar/livox/livox_blueprints.py diff --git a/dimos/hardware/sensors/lidar/livox/cpp/CMakeLists.txt b/dimos/hardware/sensors/lidar/livox/cpp/CMakeLists.txt new file mode 100644 index 0000000000..b0c80c5834 --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/cpp/CMakeLists.txt @@ -0,0 +1,42 @@ +cmake_minimum_required(VERSION 3.14) +project(livox_mid360_native CXX) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +# Fetch dimos-lcm for C++ message headers +include(FetchContent) +FetchContent_Declare(dimos_lcm + GIT_REPOSITORY https://github.com/dimensionalOS/dimos-lcm.git + GIT_TAG main + GIT_SHALLOW TRUE +) +FetchContent_MakeAvailable(dimos_lcm) + +# Find LCM +find_package(PkgConfig REQUIRED) +pkg_check_modules(LCM REQUIRED lcm) + +# Livox SDK2 (installed to /usr/local) +find_library(LIVOX_SDK livox_lidar_sdk_shared HINTS /usr/local/lib) +if(NOT LIVOX_SDK) + message(FATAL_ERROR "Livox SDK2 not found. Install from https://github.com/Livox-SDK/Livox-SDK2") +endif() + +add_executable(mid360_native main.cpp) + +target_include_directories(mid360_native PRIVATE + ${dimos_lcm_SOURCE_DIR}/generated/cpp_lcm_msgs + ${LCM_INCLUDE_DIRS} + ${CMAKE_CURRENT_SOURCE_DIR} + /usr/local/include # Livox SDK2 headers +) + +target_link_libraries(mid360_native PRIVATE + ${LCM_LIBRARIES} + ${LIVOX_SDK} +) + +target_link_directories(mid360_native PRIVATE + ${LCM_LIBRARY_DIRS} +) diff --git a/dimos/hardware/sensors/lidar/livox/cpp/README.md b/dimos/hardware/sensors/lidar/livox/cpp/README.md new file mode 100644 index 0000000000..a66a10edd5 --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/cpp/README.md @@ -0,0 +1,73 @@ +# Livox Mid-360 Native Module (C++) + +Native C++ driver for the Livox Mid-360 LiDAR. Publishes PointCloud2 and IMU +data directly on LCM, bypassing Python for minimal latency. + +## Prerequisites + +- [LCM](https://lcm-proj.github.io/) (`pacman -S lcm` or build from source) +- [Livox SDK2](https://github.com/Livox-SDK/Livox-SDK2) — build and install to `/usr/local` +- CMake >= 3.14 + +### Installing Livox SDK2 + +```bash +cd ~/src +git clone https://github.com/Livox-SDK/Livox-SDK2.git +cd Livox-SDK2 && mkdir build && cd build +cmake .. && make -j$(nproc) +sudo make install +``` + +## Build + +```bash +cd dimos/hardware/sensors/lidar/livox/cpp +mkdir -p build && cd build +cmake .. +make -j$(nproc) +``` + +The binary lands at `build/mid360_native`. + +CMake automatically fetches [dimos-lcm](https://github.com/dimensionalOS/dimos-lcm) +for the C++ message headers on first configure. + +## Usage + +Normally launched by `Mid360CppModule` via the NativeModule framework — you +don't run it directly. The Python wrapper passes LCM topic strings and config +as CLI args: + +```python +from dimos.hardware.sensors.lidar.livox.cpp.module import Mid360CppModule +from dimos.core.blueprints import autoconnect + +autoconnect( + Mid360CppModule.blueprint(host_ip="192.168.1.5"), + SomeConsumer.blueprint(), +).build().loop() +``` + +### Manual invocation (for debugging) + +```bash +./build/mid360_native \ + --pointcloud '/pointcloud#sensor_msgs.PointCloud2' \ + --imu '/imu#sensor_msgs.Imu' \ + --host_ip 192.168.1.5 \ + --lidar_ip 192.168.1.155 \ + --frequency 10 +``` + +Topic strings must include the `#type` suffix — this is the actual LCM channel +name used by dimos subscribers. + +## File overview + +| File | Description | +|---|---| +| `main.cpp` | Livox SDK2 callbacks, frame accumulation, LCM publishing | +| `dimos_native_module.hpp` | Reusable header for parsing NativeModule CLI args | +| `module.py` | Python NativeModule wrapper (`Mid360CppModule`) | +| `CMakeLists.txt` | Build config, fetches dimos-lcm headers automatically | diff --git a/dimos/hardware/sensors/lidar/livox/cpp/__init__.py b/dimos/hardware/sensors/lidar/livox/cpp/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/dimos/hardware/sensors/lidar/livox/cpp/dimos_native_module.hpp b/dimos/hardware/sensors/lidar/livox/cpp/dimos_native_module.hpp new file mode 100644 index 0000000000..73ee618c34 --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/cpp/dimos_native_module.hpp @@ -0,0 +1,64 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 +// +// Lightweight header-only helper for dimos NativeModule C++ binaries. +// Parses -- CLI args passed by the Python NativeModule wrapper. + +#pragma once + +#include +#include +#include + +namespace dimos { + +class NativeModule { +public: + NativeModule(int argc, char** argv) { + for (int i = 1; i < argc; ++i) { + std::string arg(argv[i]); + if (arg.size() > 2 && arg[0] == '-' && arg[1] == '-' && i + 1 < argc) { + args_[arg.substr(2)] = argv[++i]; + } + } + } + + /// Get the full LCM channel string for a declared port. + /// Format is "#", e.g. "/pointcloud#sensor_msgs.PointCloud2". + /// This is the exact channel name used by Python LCMTransport subscribers. + const std::string& topic(const std::string& port) const { + auto it = args_.find(port); + if (it == args_.end()) { + throw std::runtime_error("NativeModule: no topic for port '" + port + "'"); + } + return it->second; + } + + /// Get a string arg value, or a default if not present. + std::string arg(const std::string& key, const std::string& default_val = "") const { + auto it = args_.find(key); + return it != args_.end() ? it->second : default_val; + } + + /// Get a float arg value, or a default if not present. + float arg_float(const std::string& key, float default_val = 0.0f) const { + auto it = args_.find(key); + return it != args_.end() ? std::stof(it->second) : default_val; + } + + /// Get an int arg value, or a default if not present. + int arg_int(const std::string& key, int default_val = 0) const { + auto it = args_.find(key); + return it != args_.end() ? std::stoi(it->second) : default_val; + } + + /// Check if a port/arg was provided. + bool has(const std::string& key) const { + return args_.count(key) > 0; + } + +private: + std::map args_; +}; + +} // namespace dimos diff --git a/dimos/hardware/sensors/lidar/livox/cpp/main.cpp b/dimos/hardware/sensors/lidar/livox/cpp/main.cpp new file mode 100644 index 0000000000..3c7cf5ce17 --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/cpp/main.cpp @@ -0,0 +1,437 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 +// +// Livox Mid-360 native module for dimos NativeModule framework. +// +// Publishes PointCloud2 and Imu messages on LCM topics received via CLI args. +// Usage: ./mid360_native --pointcloud --imu [--host_ip ] [--lidar_ip ] ... + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "dimos_native_module.hpp" + +#include "geometry_msgs/Quaternion.hpp" +#include "geometry_msgs/Vector3.hpp" +#include "sensor_msgs/Imu.hpp" +#include "sensor_msgs/PointCloud2.hpp" +#include "sensor_msgs/PointField.hpp" +#include "std_msgs/Header.hpp" +#include "std_msgs/Time.hpp" + +// Gravity constant for converting accelerometer data from g to m/s^2 +static constexpr double GRAVITY_MS2 = 9.80665; + +// Livox data_type values (not provided as named constants in SDK2 header) +static constexpr uint8_t DATA_TYPE_IMU = 0x00; +static constexpr uint8_t DATA_TYPE_CARTESIAN_HIGH = 0x01; +static constexpr uint8_t DATA_TYPE_CARTESIAN_LOW = 0x02; + +// --------------------------------------------------------------------------- +// Global state +// --------------------------------------------------------------------------- + +static std::atomic g_running{true}; +static lcm::LCM* g_lcm = nullptr; +static std::string g_pointcloud_topic; +static std::string g_imu_topic; +static std::string g_frame_id = "lidar_link"; +static std::string g_imu_frame_id = "imu_link"; +static float g_frequency = 10.0f; + +// Frame accumulator +static std::mutex g_pc_mutex; +static std::vector g_accumulated_xyz; // interleaved x,y,z +static std::vector g_accumulated_intensity; // per-point intensity +static double g_frame_timestamp = 0.0; +static bool g_frame_has_timestamp = false; + +// --------------------------------------------------------------------------- +// Helpers +// --------------------------------------------------------------------------- + +static std_msgs::Time time_from_seconds(double t) { + std_msgs::Time ts; + ts.sec = static_cast(t); + ts.nsec = static_cast((t - ts.sec) * 1e9); + return ts; +} + +static double get_timestamp_ns(const LivoxLidarEthernetPacket* pkt) { + uint64_t ns = 0; + std::memcpy(&ns, pkt->timestamp, sizeof(uint64_t)); + return static_cast(ns); +} + +static std_msgs::Header make_header(const std::string& frame_id, double ts) { + static std::atomic seq{0}; + std_msgs::Header h; + h.seq = seq.fetch_add(1, std::memory_order_relaxed); + h.stamp = time_from_seconds(ts); + h.frame_id = frame_id; + return h; +} + +// --------------------------------------------------------------------------- +// Build and publish PointCloud2 +// --------------------------------------------------------------------------- + +static void publish_pointcloud(const std::vector& xyz, + const std::vector& intensity, + double timestamp) { + if (!g_lcm || xyz.empty()) return; + + int num_points = static_cast(xyz.size()) / 3; + + sensor_msgs::PointCloud2 pc; + pc.header = make_header(g_frame_id, timestamp); + pc.height = 1; + pc.width = num_points; + pc.is_bigendian = 0; + pc.is_dense = 1; + + // Fields: x, y, z (float32), intensity (float32) + pc.fields_length = 4; + pc.fields.resize(4); + + auto make_field = [](const std::string& name, int32_t offset) { + sensor_msgs::PointField f; + f.name = name; + f.offset = offset; + f.datatype = sensor_msgs::PointField::FLOAT32; + f.count = 1; + return f; + }; + + pc.fields[0] = make_field("x", 0); + pc.fields[1] = make_field("y", 4); + pc.fields[2] = make_field("z", 8); + pc.fields[3] = make_field("intensity", 12); + + pc.point_step = 16; // 4 floats * 4 bytes + pc.row_step = pc.point_step * num_points; + + // Pack point data + pc.data_length = pc.row_step; + pc.data.resize(pc.data_length); + + for (int i = 0; i < num_points; ++i) { + float* dst = reinterpret_cast(pc.data.data() + i * 16); + dst[0] = xyz[i * 3 + 0]; + dst[1] = xyz[i * 3 + 1]; + dst[2] = xyz[i * 3 + 2]; + dst[3] = intensity[i]; + } + + g_lcm->publish(g_pointcloud_topic, &pc); +} + +// --------------------------------------------------------------------------- +// SDK callbacks +// --------------------------------------------------------------------------- + +static void on_point_cloud(const uint32_t /*handle*/, const uint8_t /*dev_type*/, + LivoxLidarEthernetPacket* data, void* /*client_data*/) { + if (!g_running.load() || data == nullptr) return; + + double ts_ns = get_timestamp_ns(data); + double ts = ts_ns / 1e9; + uint16_t dot_num = data->dot_num; + + std::lock_guard lock(g_pc_mutex); + + if (!g_frame_has_timestamp) { + g_frame_timestamp = ts; + g_frame_has_timestamp = true; + } + + if (data->data_type == DATA_TYPE_CARTESIAN_HIGH) { + auto* pts = reinterpret_cast(data->data); + for (uint16_t i = 0; i < dot_num; ++i) { + // Livox high-precision coordinates are in mm, convert to meters + g_accumulated_xyz.push_back(static_cast(pts[i].x) / 1000.0f); + g_accumulated_xyz.push_back(static_cast(pts[i].y) / 1000.0f); + g_accumulated_xyz.push_back(static_cast(pts[i].z) / 1000.0f); + g_accumulated_intensity.push_back(static_cast(pts[i].reflectivity) / 255.0f); + } + } else if (data->data_type == DATA_TYPE_CARTESIAN_LOW) { + auto* pts = reinterpret_cast(data->data); + for (uint16_t i = 0; i < dot_num; ++i) { + // Livox low-precision coordinates are in cm, convert to meters + g_accumulated_xyz.push_back(static_cast(pts[i].x) / 100.0f); + g_accumulated_xyz.push_back(static_cast(pts[i].y) / 100.0f); + g_accumulated_xyz.push_back(static_cast(pts[i].z) / 100.0f); + g_accumulated_intensity.push_back(static_cast(pts[i].reflectivity) / 255.0f); + } + } +} + +static void on_imu_data(const uint32_t /*handle*/, const uint8_t /*dev_type*/, + LivoxLidarEthernetPacket* data, void* /*client_data*/) { + if (!g_running.load() || data == nullptr || !g_lcm) return; + if (g_imu_topic.empty()) return; + + double ts = get_timestamp_ns(data) / 1e9; + auto* imu_pts = reinterpret_cast(data->data); + uint16_t dot_num = data->dot_num; + + for (uint16_t i = 0; i < dot_num; ++i) { + sensor_msgs::Imu msg; + msg.header = make_header(g_imu_frame_id, ts); + + // Orientation unknown — set to identity with high covariance + msg.orientation.x = 0.0; + msg.orientation.y = 0.0; + msg.orientation.z = 0.0; + msg.orientation.w = 1.0; + msg.orientation_covariance[0] = -1.0; // indicates unknown + + msg.angular_velocity.x = static_cast(imu_pts[i].gyro_x); + msg.angular_velocity.y = static_cast(imu_pts[i].gyro_y); + msg.angular_velocity.z = static_cast(imu_pts[i].gyro_z); + + msg.linear_acceleration.x = static_cast(imu_pts[i].acc_x) * GRAVITY_MS2; + msg.linear_acceleration.y = static_cast(imu_pts[i].acc_y) * GRAVITY_MS2; + msg.linear_acceleration.z = static_cast(imu_pts[i].acc_z) * GRAVITY_MS2; + + g_lcm->publish(g_imu_topic, &msg); + } +} + +static void on_info_change(const uint32_t handle, const LivoxLidarInfo* info, + void* /*client_data*/) { + if (info == nullptr) return; + + char sn[17] = {}; + std::memcpy(sn, info->sn, 16); + char ip[17] = {}; + std::memcpy(ip, info->lidar_ip, 16); + + printf("[mid360] Device connected: handle=%u type=%u sn=%s ip=%s\n", + handle, info->dev_type, sn, ip); + + // Set to normal work mode + SetLivoxLidarWorkMode(handle, kLivoxLidarNormal, nullptr, nullptr); + + // Enable IMU + if (!g_imu_topic.empty()) { + EnableLivoxLidarImuData(handle, nullptr, nullptr); + } +} + +// --------------------------------------------------------------------------- +// Signal handling +// --------------------------------------------------------------------------- + +static void signal_handler(int /*sig*/) { + g_running.store(false); +} + +// --------------------------------------------------------------------------- +// SDK config file generation +// --------------------------------------------------------------------------- + +struct SdkPorts { + int cmd_data = 56100; + int push_msg = 56200; + int point_data = 56300; + int imu_data = 56400; + int log_data = 56500; + int host_cmd_data = 56101; + int host_push_msg = 56201; + int host_point_data = 56301; + int host_imu_data = 56401; + int host_log_data = 56501; +}; + +static std::string write_sdk_config(const std::string& host_ip, + const std::string& lidar_ip, + const SdkPorts& ports) { + char tmpl[] = "/tmp/livox_mid360_XXXXXX"; + int fd = mkstemp(tmpl); + if (fd < 0) { + perror("mkstemp"); + return ""; + } + + // fdopen takes ownership of fd — fclose will close it + FILE* fp = fdopen(fd, "w"); + if (!fp) { + perror("fdopen"); + close(fd); + return ""; + } + + fprintf(fp, + "{\n" + " \"MID360\": {\n" + " \"lidar_net_info\": {\n" + " \"cmd_data_port\": %d,\n" + " \"push_msg_port\": %d,\n" + " \"point_data_port\": %d,\n" + " \"imu_data_port\": %d,\n" + " \"log_data_port\": %d\n" + " },\n" + " \"host_net_info\": [\n" + " {\n" + " \"host_ip\": \"%s\",\n" + " \"multicast_ip\": \"224.1.1.5\",\n" + " \"cmd_data_port\": %d,\n" + " \"push_msg_port\": %d,\n" + " \"point_data_port\": %d,\n" + " \"imu_data_port\": %d,\n" + " \"log_data_port\": %d\n" + " }\n" + " ]\n" + " }\n" + "}\n", + ports.cmd_data, ports.push_msg, ports.point_data, + ports.imu_data, ports.log_data, + host_ip.c_str(), + ports.host_cmd_data, ports.host_push_msg, ports.host_point_data, + ports.host_imu_data, ports.host_log_data); + fclose(fp); + + return tmpl; +} + +// --------------------------------------------------------------------------- +// Main +// --------------------------------------------------------------------------- + +int main(int argc, char** argv) { + dimos::NativeModule mod(argc, argv); + + // Required: LCM topics for ports + g_pointcloud_topic = mod.has("pointcloud") ? mod.topic("pointcloud") : ""; + g_imu_topic = mod.has("imu") ? mod.topic("imu") : ""; + + if (g_pointcloud_topic.empty()) { + fprintf(stderr, "Error: --pointcloud is required\n"); + return 1; + } + + // Optional config args + std::string host_ip = mod.arg("host_ip", "192.168.1.5"); + std::string lidar_ip = mod.arg("lidar_ip", "192.168.1.155"); + g_frequency = mod.arg_float("frequency", 10.0f); + g_frame_id = mod.arg("frame_id", "lidar_link"); + g_imu_frame_id = mod.arg("imu_frame_id", "imu_link"); + + // SDK network ports (configurable for multi-sensor setups) + SdkPorts ports; + ports.cmd_data = mod.arg_int("cmd_data_port", 56100); + ports.push_msg = mod.arg_int("push_msg_port", 56200); + ports.point_data = mod.arg_int("point_data_port", 56300); + ports.imu_data = mod.arg_int("imu_data_port", 56400); + ports.log_data = mod.arg_int("log_data_port", 56500); + ports.host_cmd_data = mod.arg_int("host_cmd_data_port", 56101); + ports.host_push_msg = mod.arg_int("host_push_msg_port", 56201); + ports.host_point_data = mod.arg_int("host_point_data_port", 56301); + ports.host_imu_data = mod.arg_int("host_imu_data_port", 56401); + ports.host_log_data = mod.arg_int("host_log_data_port", 56501); + + printf("[mid360] Starting native Livox Mid-360 module\n"); + printf("[mid360] pointcloud topic: %s\n", g_pointcloud_topic.c_str()); + printf("[mid360] imu topic: %s\n", g_imu_topic.empty() ? "(disabled)" : g_imu_topic.c_str()); + printf("[mid360] host_ip: %s lidar_ip: %s frequency: %.1f Hz\n", + host_ip.c_str(), lidar_ip.c_str(), g_frequency); + + // Signal handlers + signal(SIGTERM, signal_handler); + signal(SIGINT, signal_handler); + + // Init LCM + lcm::LCM lcm; + if (!lcm.good()) { + fprintf(stderr, "Error: LCM init failed\n"); + return 1; + } + g_lcm = &lcm; + + // Write SDK config + std::string config_path = write_sdk_config(host_ip, lidar_ip, ports); + if (config_path.empty()) { + fprintf(stderr, "Error: failed to write SDK config\n"); + return 1; + } + + // Init Livox SDK + if (!LivoxLidarSdkInit(config_path.c_str(), host_ip.c_str())) { + fprintf(stderr, "Error: LivoxLidarSdkInit failed\n"); + std::remove(config_path.c_str()); + return 1; + } + + // Register callbacks + SetLivoxLidarPointCloudCallBack(on_point_cloud, nullptr); + if (!g_imu_topic.empty()) { + SetLivoxLidarImuDataCallback(on_imu_data, nullptr); + } + SetLivoxLidarInfoChangeCallback(on_info_change, nullptr); + + // Start SDK + if (!LivoxLidarSdkStart()) { + fprintf(stderr, "Error: LivoxLidarSdkStart failed\n"); + LivoxLidarSdkUninit(); + std::remove(config_path.c_str()); + return 1; + } + + printf("[mid360] SDK started, waiting for device...\n"); + + // Main loop: periodically emit accumulated point clouds + auto frame_interval = std::chrono::microseconds( + static_cast(1e6 / g_frequency)); + auto last_emit = std::chrono::steady_clock::now(); + + while (g_running.load()) { + // Handle LCM (for any subscriptions, though we mostly publish) + lcm.handleTimeout(10); // 10ms timeout + + auto now = std::chrono::steady_clock::now(); + if (now - last_emit >= frame_interval) { + // Swap out the accumulated data + std::vector xyz; + std::vector intensity; + double ts = 0.0; + + { + std::lock_guard lock(g_pc_mutex); + if (!g_accumulated_xyz.empty()) { + xyz.swap(g_accumulated_xyz); + intensity.swap(g_accumulated_intensity); + ts = g_frame_timestamp; + g_frame_has_timestamp = false; + } + } + + if (!xyz.empty()) { + publish_pointcloud(xyz, intensity, ts); + } + + last_emit = now; + } + } + + // Cleanup + printf("[mid360] Shutting down...\n"); + LivoxLidarSdkUninit(); + std::remove(config_path.c_str()); + g_lcm = nullptr; + + printf("[mid360] Done.\n"); + return 0; +} diff --git a/dimos/hardware/sensors/lidar/livox/cpp/module.py b/dimos/hardware/sensors/lidar/livox/cpp/module.py new file mode 100644 index 0000000000..dc73ef6548 --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/cpp/module.py @@ -0,0 +1,136 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Python NativeModule wrapper for the C++ Livox Mid-360 driver. + +Declares the same ports as LivoxLidarModule (pointcloud, imu) but delegates +all real work to the ``mid360_native`` C++ binary, which talks directly to +the Livox SDK2 C API and publishes on LCM. + +Usage:: + + from dimos.hardware.sensors.lidar.livox.cpp.module import Mid360CppModule + from dimos.core.blueprints import autoconnect + + autoconnect( + Mid360CppModule.blueprint(host_ip="192.168.1.5"), + SomeConsumer.blueprint(), + ).build().loop() +""" + +from __future__ import annotations + +from dataclasses import dataclass +from pathlib import Path +from typing import TYPE_CHECKING + +from dimos.core.native_module import NativeModule, NativeModuleConfig +from dimos.spec import perception + +if TYPE_CHECKING: + from dimos.core import Out + from dimos.msgs.sensor_msgs.Imu import Imu + from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + +_DEFAULT_EXECUTABLE = str(Path(__file__).parent / "build" / "mid360_native") + + +@dataclass(kw_only=True) +class Mid360CppConfig(NativeModuleConfig): + """Config for the C++ Mid-360 native module.""" + + executable: str = _DEFAULT_EXECUTABLE + host_ip: str = "192.168.1.5" + lidar_ip: str = "192.168.1.155" + frequency: float = 10.0 + enable_imu: bool = True + frame_id: str = "lidar_link" + imu_frame_id: str = "imu_link" + + # SDK port configuration (match defaults in LivoxMid360Config) + cmd_data_port: int = 56100 + push_msg_port: int = 56200 + point_data_port: int = 56300 + imu_data_port: int = 56400 + log_data_port: int = 56500 + host_cmd_data_port: int = 56101 + host_push_msg_port: int = 56201 + host_point_data_port: int = 56301 + host_imu_data_port: int = 56401 + host_log_data_port: int = 56501 + + +class Mid360CppModule(NativeModule, perception.Lidar, perception.IMU): + """Livox Mid-360 LiDAR module backed by a native C++ binary. + + Ports: + pointcloud (Out[PointCloud2]): Point cloud frames at configured frequency. + imu (Out[Imu]): IMU data at ~200 Hz (if enabled). + """ + + default_config: type[Mid360CppConfig] = Mid360CppConfig # type: ignore[assignment] + pointcloud: Out[PointCloud2] + imu: Out[Imu] + + def _collect_topics(self) -> dict[str, str]: + cfg: Mid360CppConfig = self.config # type: ignore[assignment] + topics = super()._collect_topics() + if not cfg.enable_imu: + topics.pop("imu", None) + return topics + + def _build_extra_args(self) -> list[str]: + """Pass hardware config to the C++ binary as CLI args.""" + cfg: Mid360CppConfig = self.config # type: ignore[assignment] + return [ + "--host_ip", + cfg.host_ip, + "--lidar_ip", + cfg.lidar_ip, + "--frequency", + str(cfg.frequency), + "--frame_id", + cfg.frame_id, + "--imu_frame_id", + cfg.imu_frame_id, + "--cmd_data_port", + str(cfg.cmd_data_port), + "--push_msg_port", + str(cfg.push_msg_port), + "--point_data_port", + str(cfg.point_data_port), + "--imu_data_port", + str(cfg.imu_data_port), + "--log_data_port", + str(cfg.log_data_port), + "--host_cmd_data_port", + str(cfg.host_cmd_data_port), + "--host_push_msg_port", + str(cfg.host_push_msg_port), + "--host_point_data_port", + str(cfg.host_point_data_port), + "--host_imu_data_port", + str(cfg.host_imu_data_port), + "--host_log_data_port", + str(cfg.host_log_data_port), + ] + + +mid360_cpp_module = Mid360CppModule.blueprint + +__all__ = [ + "Mid360CppConfig", + "Mid360CppModule", + "mid360_cpp_module", +] diff --git a/dimos/hardware/sensors/lidar/livox/livox_blueprints.py b/dimos/hardware/sensors/lidar/livox/livox_blueprints.py new file mode 100644 index 0000000000..d93dc62a41 --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/livox_blueprints.py @@ -0,0 +1,22 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from dimos.core.blueprints import autoconnect +from dimos.hardware.sensors.lidar.livox.cpp.module import Mid360CppModule +from dimos.visualization.rerun.bridge import rerun_bridge + +mid360cpp = autoconnect( + Mid360CppModule.blueprint(), + rerun_bridge(), +).global_config(n_dask_workers=2, robot_model="mid360_livox_cpp") diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 70cc455d34..d3a6a58993 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -40,6 +40,7 @@ "demo-osm": "dimos.mapping.osm.demo_osm:demo_osm", "demo-skill": "dimos.agents.skills.demo_skill:demo_skill", "dual-xarm6-planner": "dimos.manipulation.manipulation_blueprints:dual_xarm6_planner", + "mid360cpp": "dimos.hardware.sensors.lidar.livox.livox_blueprints:mid360cpp", "unitree-g1": "dimos.robot.unitree_webrtc.unitree_g1_blueprints:unitree_g1", "unitree-g1-agentic": "dimos.robot.unitree_webrtc.unitree_g1_blueprints:unitree_g1_agentic", "unitree-g1-agentic-sim": "dimos.robot.unitree_webrtc.unitree_g1_blueprints:unitree_g1_agentic_sim", @@ -88,9 +89,12 @@ "human_input": "dimos.agents.cli.human", "joint_trajectory_controller": "dimos.manipulation.control.trajectory_controller.joint_trajectory_controller", "keyboard_teleop": "dimos.robot.unitree_webrtc.keyboard_teleop", + "lidar_module": "dimos.hardware.sensors.lidar.module", + "livox_lidar_module": "dimos.hardware.sensors.lidar.livox.module", "llm_agent": "dimos.agents.agent", "manipulation_module": "dimos.manipulation.manipulation_module", "mapper": "dimos.robot.unitree_webrtc.type.map", + "mid360_cpp_module": "dimos.hardware.sensors.lidar.livox.cpp.module", "navigation_skill": "dimos.agents.skills.navigation", "object_scene_registration_module": "dimos.perception.object_scene_registration", "object_tracking": "dimos.perception.object_tracker", From 8cd4ce98ad2c5f70db6fed1bc1146b467c3be9fa Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Thu, 12 Feb 2026 19:50:02 +0800 Subject: [PATCH 07/48] bugfix --- dimos/hardware/sensors/lidar/livox/cpp/module.py | 7 ------- 1 file changed, 7 deletions(-) diff --git a/dimos/hardware/sensors/lidar/livox/cpp/module.py b/dimos/hardware/sensors/lidar/livox/cpp/module.py index dc73ef6548..8ed346a672 100644 --- a/dimos/hardware/sensors/lidar/livox/cpp/module.py +++ b/dimos/hardware/sensors/lidar/livox/cpp/module.py @@ -83,13 +83,6 @@ class Mid360CppModule(NativeModule, perception.Lidar, perception.IMU): pointcloud: Out[PointCloud2] imu: Out[Imu] - def _collect_topics(self) -> dict[str, str]: - cfg: Mid360CppConfig = self.config # type: ignore[assignment] - topics = super()._collect_topics() - if not cfg.enable_imu: - topics.pop("imu", None) - return topics - def _build_extra_args(self) -> list[str]: """Pass hardware config to the C++ binary as CLI args.""" cfg: Mid360CppConfig = self.config # type: ignore[assignment] From 5c5f100a3f9f87888d4a5a8b30d13e85481498ce Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Thu, 12 Feb 2026 19:56:45 +0800 Subject: [PATCH 08/48] removed py livox implementation entirely --- .../sensors/lidar/livox/cpp/README.md | 4 +- .../sensors/lidar/livox/cpp/__init__.py | 0 .../sensors/lidar/livox/cpp/module.py | 129 ----- .../sensors/lidar/livox/livox_blueprints.py | 2 +- dimos/hardware/sensors/lidar/livox/mid360.py | 500 ---------------- dimos/hardware/sensors/lidar/livox/module.py | 124 +++- dimos/hardware/sensors/lidar/livox/sdk.py | 534 ------------------ .../lidar/livox/test_spec_compliance.py | 6 +- 8 files changed, 103 insertions(+), 1196 deletions(-) delete mode 100644 dimos/hardware/sensors/lidar/livox/cpp/__init__.py delete mode 100644 dimos/hardware/sensors/lidar/livox/cpp/module.py delete mode 100644 dimos/hardware/sensors/lidar/livox/mid360.py delete mode 100644 dimos/hardware/sensors/lidar/livox/sdk.py diff --git a/dimos/hardware/sensors/lidar/livox/cpp/README.md b/dimos/hardware/sensors/lidar/livox/cpp/README.md index a66a10edd5..5e78e48f86 100644 --- a/dimos/hardware/sensors/lidar/livox/cpp/README.md +++ b/dimos/hardware/sensors/lidar/livox/cpp/README.md @@ -40,7 +40,7 @@ don't run it directly. The Python wrapper passes LCM topic strings and config as CLI args: ```python -from dimos.hardware.sensors.lidar.livox.cpp.module import Mid360CppModule +from dimos.hardware.sensors.lidar.livox.module import Mid360CppModule from dimos.core.blueprints import autoconnect autoconnect( @@ -69,5 +69,5 @@ name used by dimos subscribers. |---|---| | `main.cpp` | Livox SDK2 callbacks, frame accumulation, LCM publishing | | `dimos_native_module.hpp` | Reusable header for parsing NativeModule CLI args | -| `module.py` | Python NativeModule wrapper (`Mid360CppModule`) | +| `../module.py` | Python NativeModule wrapper (`Mid360CppModule`) | | `CMakeLists.txt` | Build config, fetches dimos-lcm headers automatically | diff --git a/dimos/hardware/sensors/lidar/livox/cpp/__init__.py b/dimos/hardware/sensors/lidar/livox/cpp/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/dimos/hardware/sensors/lidar/livox/cpp/module.py b/dimos/hardware/sensors/lidar/livox/cpp/module.py deleted file mode 100644 index 8ed346a672..0000000000 --- a/dimos/hardware/sensors/lidar/livox/cpp/module.py +++ /dev/null @@ -1,129 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Python NativeModule wrapper for the C++ Livox Mid-360 driver. - -Declares the same ports as LivoxLidarModule (pointcloud, imu) but delegates -all real work to the ``mid360_native`` C++ binary, which talks directly to -the Livox SDK2 C API and publishes on LCM. - -Usage:: - - from dimos.hardware.sensors.lidar.livox.cpp.module import Mid360CppModule - from dimos.core.blueprints import autoconnect - - autoconnect( - Mid360CppModule.blueprint(host_ip="192.168.1.5"), - SomeConsumer.blueprint(), - ).build().loop() -""" - -from __future__ import annotations - -from dataclasses import dataclass -from pathlib import Path -from typing import TYPE_CHECKING - -from dimos.core.native_module import NativeModule, NativeModuleConfig -from dimos.spec import perception - -if TYPE_CHECKING: - from dimos.core import Out - from dimos.msgs.sensor_msgs.Imu import Imu - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 - -_DEFAULT_EXECUTABLE = str(Path(__file__).parent / "build" / "mid360_native") - - -@dataclass(kw_only=True) -class Mid360CppConfig(NativeModuleConfig): - """Config for the C++ Mid-360 native module.""" - - executable: str = _DEFAULT_EXECUTABLE - host_ip: str = "192.168.1.5" - lidar_ip: str = "192.168.1.155" - frequency: float = 10.0 - enable_imu: bool = True - frame_id: str = "lidar_link" - imu_frame_id: str = "imu_link" - - # SDK port configuration (match defaults in LivoxMid360Config) - cmd_data_port: int = 56100 - push_msg_port: int = 56200 - point_data_port: int = 56300 - imu_data_port: int = 56400 - log_data_port: int = 56500 - host_cmd_data_port: int = 56101 - host_push_msg_port: int = 56201 - host_point_data_port: int = 56301 - host_imu_data_port: int = 56401 - host_log_data_port: int = 56501 - - -class Mid360CppModule(NativeModule, perception.Lidar, perception.IMU): - """Livox Mid-360 LiDAR module backed by a native C++ binary. - - Ports: - pointcloud (Out[PointCloud2]): Point cloud frames at configured frequency. - imu (Out[Imu]): IMU data at ~200 Hz (if enabled). - """ - - default_config: type[Mid360CppConfig] = Mid360CppConfig # type: ignore[assignment] - pointcloud: Out[PointCloud2] - imu: Out[Imu] - - def _build_extra_args(self) -> list[str]: - """Pass hardware config to the C++ binary as CLI args.""" - cfg: Mid360CppConfig = self.config # type: ignore[assignment] - return [ - "--host_ip", - cfg.host_ip, - "--lidar_ip", - cfg.lidar_ip, - "--frequency", - str(cfg.frequency), - "--frame_id", - cfg.frame_id, - "--imu_frame_id", - cfg.imu_frame_id, - "--cmd_data_port", - str(cfg.cmd_data_port), - "--push_msg_port", - str(cfg.push_msg_port), - "--point_data_port", - str(cfg.point_data_port), - "--imu_data_port", - str(cfg.imu_data_port), - "--log_data_port", - str(cfg.log_data_port), - "--host_cmd_data_port", - str(cfg.host_cmd_data_port), - "--host_push_msg_port", - str(cfg.host_push_msg_port), - "--host_point_data_port", - str(cfg.host_point_data_port), - "--host_imu_data_port", - str(cfg.host_imu_data_port), - "--host_log_data_port", - str(cfg.host_log_data_port), - ] - - -mid360_cpp_module = Mid360CppModule.blueprint - -__all__ = [ - "Mid360CppConfig", - "Mid360CppModule", - "mid360_cpp_module", -] diff --git a/dimos/hardware/sensors/lidar/livox/livox_blueprints.py b/dimos/hardware/sensors/lidar/livox/livox_blueprints.py index d93dc62a41..a4cbae37fb 100644 --- a/dimos/hardware/sensors/lidar/livox/livox_blueprints.py +++ b/dimos/hardware/sensors/lidar/livox/livox_blueprints.py @@ -13,7 +13,7 @@ # limitations under the License. from dimos.core.blueprints import autoconnect -from dimos.hardware.sensors.lidar.livox.cpp.module import Mid360CppModule +from dimos.hardware.sensors.lidar.livox.module import Mid360CppModule from dimos.visualization.rerun.bridge import rerun_bridge mid360cpp = autoconnect( diff --git a/dimos/hardware/sensors/lidar/livox/mid360.py b/dimos/hardware/sensors/lidar/livox/mid360.py deleted file mode 100644 index dbf8a5d81b..0000000000 --- a/dimos/hardware/sensors/lidar/livox/mid360.py +++ /dev/null @@ -1,500 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Livox Mid-360 LiDAR hardware driver using Livox SDK2 ctypes bindings.""" - -from __future__ import annotations - -from dataclasses import dataclass, field -from functools import cache -import json -import logging -from queue import Empty, Queue -import tempfile -import threading -import time -from typing import TYPE_CHECKING, Any - -import numpy as np -import open3d as o3d # type: ignore[import-untyped] -import open3d.core as o3c # type: ignore[import-untyped] -from reactivex import create - -from dimos.hardware.sensors.lidar.livox.sdk import ( - AsyncControlCallbackType, - ImuDataCallbackType, - InfoChangeCallbackType, - LivoxLidarDeviceType, - LivoxLidarEthernetPacket, - LivoxLidarInfo, - LivoxLidarPointDataType, - LivoxLidarWorkMode, - LivoxSDK, - PointCloudCallbackType, - get_packet_timestamp_ns, - parse_cartesian_high_points, - parse_cartesian_low_points, - parse_imu_data, -) -from dimos.hardware.sensors.lidar.spec import LidarConfig, LidarHardware -from dimos.msgs.geometry_msgs import Vector3 -from dimos.msgs.sensor_msgs.Imu import Imu -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.utils.reactive import backpressure - -if TYPE_CHECKING: - from reactivex.observable import Observable - -logger = logging.getLogger(__name__) - -# Gravity constant for converting accelerometer data from g to m/s^2 -GRAVITY_MS2 = 9.80665 - - -@dataclass -class LivoxMid360Config(LidarConfig): - """Configuration for the Livox Mid-360 LiDAR.""" - - host_ip: str = "192.168.1.5" - lidar_ips: list[str] = field(default_factory=lambda: ["192.168.1.155"]) - frequency: float = 5.0 # Hz, point cloud output rate - frame_id: str = "lidar_link" - frame_id_prefix: str | None = None - enable_imu: bool = True - sdk_lib_path: str | None = None - - # SDK port configuration - cmd_data_port: int = 56100 - push_msg_port: int = 56200 - point_data_port: int = 56300 - imu_data_port: int = 56400 - log_data_port: int = 56500 - host_cmd_data_port: int = 56101 - host_push_msg_port: int = 56201 - host_point_data_port: int = 56301 - host_imu_data_port: int = 56401 - host_log_data_port: int = 56501 - - # Voxel downsampling size in meters (None = no downsampling) - voxel_size: float | None = None - - # Socket buffer size (bytes) to avoid packet loss at high data rates - recv_buf_size: int = 4 * 1024 * 1024 # 4 MB - - -class LivoxMid360(LidarHardware["LivoxMid360Config"]): - """Livox Mid-360 LiDAR driver using SDK2 ctypes bindings. - - Produces Observable[PointCloud2] at the configured frame rate (~10 Hz), - and optionally Observable[Imu] at ~200 Hz. - - Uses a thread-safe queue to pass data from SDK C-thread callbacks - to Python consumer threads that build messages. - """ - - default_config = LivoxMid360Config - - def __init__(self, *args: object, **kwargs: object) -> None: - super().__init__(*args, **kwargs) - self._sdk: LivoxSDK | None = None - self._config_path: str | None = None - self._stop_event = threading.Event() - - # Point cloud frame accumulator state - self._pc_queue: Queue[tuple[int, int, np.ndarray, np.ndarray]] = Queue(maxsize=4096) - self._pc_consumer_thread: threading.Thread | None = None - self._pc_observer: Any[PointCloud2] | None = None - - # IMU state - self._imu_queue: Queue[tuple[int, np.ndarray]] = Queue(maxsize=4096) - self._imu_consumer_thread: threading.Thread | None = None - self._imu_observer: Any[Imu] | None = None - - # Device tracking - self._connected_handles: dict[int, LivoxLidarDeviceType] = {} - - def _frame_id(self, suffix: str) -> str: - prefix = self.config.frame_id_prefix - if prefix: - return f"{prefix}/{suffix}" - return suffix - - # ------------------------------------------------------------------ - # SDK config generation - # ------------------------------------------------------------------ - - def _write_sdk_config(self) -> str: - """Generate a temporary JSON config file for the SDK.""" - config = { - "MID360": { - "lidar_net_info": { - "cmd_data_port": self.config.cmd_data_port, - "push_msg_port": self.config.push_msg_port, - "point_data_port": self.config.point_data_port, - "imu_data_port": self.config.imu_data_port, - "log_data_port": self.config.log_data_port, - }, - "host_net_info": [ - { - "host_ip": self.config.host_ip, - "multicast_ip": "224.1.1.5", - "cmd_data_port": self.config.host_cmd_data_port, - "push_msg_port": self.config.host_push_msg_port, - "point_data_port": self.config.host_point_data_port, - "imu_data_port": self.config.host_imu_data_port, - "log_data_port": self.config.host_log_data_port, - } - ], - } - } - fd, path = tempfile.mkstemp(suffix=".json", prefix="livox_mid360_") - with open(fd, "w") as f: - json.dump(config, f) - return path - - # ------------------------------------------------------------------ - # SDK callbacks (called from C threads - keep minimal, just enqueue) - # ------------------------------------------------------------------ - - def _on_point_cloud( - self, - handle: int, - dev_type: int, - packet_ptr: LivoxLidarEthernetPacket, # type: ignore[override] - client_data: object, - ) -> None: - """SDK point cloud callback. Copies data and enqueues for processing.""" - if self._stop_event.is_set(): - return - try: - packet = packet_ptr.contents if hasattr(packet_ptr, "contents") else packet_ptr - data_type = packet.data_type - - if data_type == LivoxLidarPointDataType.CARTESIAN_HIGH: - xyz, reflectivities, _tags = parse_cartesian_high_points(packet) - elif data_type == LivoxLidarPointDataType.CARTESIAN_LOW: - xyz, reflectivities, _tags = parse_cartesian_low_points(packet) - else: - return # skip spherical for now - - ts_ns = get_packet_timestamp_ns(packet) - frame_cnt = packet.frame_cnt - - self._pc_queue.put_nowait((frame_cnt, ts_ns, xyz, reflectivities)) - except Exception: - logger.debug("Error in point cloud callback", exc_info=True) - - def _on_imu_data( - self, - handle: int, - dev_type: int, - packet_ptr: LivoxLidarEthernetPacket, # type: ignore[override] - client_data: object, - ) -> None: - """SDK IMU callback. Copies data and enqueues.""" - if self._stop_event.is_set(): - return - try: - packet = packet_ptr.contents if hasattr(packet_ptr, "contents") else packet_ptr - ts_ns = get_packet_timestamp_ns(packet) - imu_points = parse_imu_data(packet) - self._imu_queue.put_nowait((ts_ns, imu_points)) - except Exception: - logger.debug("Error in IMU callback", exc_info=True) - - def _on_info_change( - self, - handle: int, - info_ptr: LivoxLidarInfo, # type: ignore[override] - client_data: object, - ) -> None: - """SDK device info change callback. Tracks connected devices.""" - try: - info = info_ptr.contents if hasattr(info_ptr, "contents") else info_ptr - dev_type = LivoxLidarDeviceType(info.dev_type) - sn = info.sn.decode("utf-8", errors="replace").rstrip("\x00") - ip = info.lidar_ip.decode("utf-8", errors="replace").rstrip("\x00") - self._connected_handles[handle] = dev_type - logger.info( - "Livox device connected: handle=%d type=%s sn=%s ip=%s", - handle, - dev_type.name, - sn, - ip, - ) - - # Set to normal work mode - self._sdk_set_work_mode(handle) - - # Enable/disable IMU based on config - if self.config.enable_imu: - self._sdk_enable_imu(handle) - except Exception: - logger.debug("Error in info change callback", exc_info=True) - - def _sdk_set_work_mode(self, handle: int) -> None: - if self._sdk is None: - return - - def _on_work_mode(status: int, handle: int, response: object, client_data: object) -> None: - if status == 0: - logger.info("Work mode set to NORMAL for handle %d", handle) - else: - logger.warning("Failed to set work mode for handle %d: status=%d", handle, status) - - _cb = AsyncControlCallbackType(_on_work_mode) - self._sdk._callbacks[f"work_mode_cb_{handle}"] = _cb - self._sdk.lib.SetLivoxLidarWorkMode(handle, int(LivoxLidarWorkMode.NORMAL), _cb, None) - - def _sdk_enable_imu(self, handle: int) -> None: - if self._sdk is None: - return - - def _on_imu_enable(status: int, handle: int, response: object, client_data: object) -> None: - if status == 0: - logger.info("IMU enabled for handle %d", handle) - else: - logger.warning("Failed to enable IMU for handle %d: status=%d", handle, status) - - _cb = AsyncControlCallbackType(_on_imu_enable) - self._sdk._callbacks[f"imu_enable_cb_{handle}"] = _cb - self._sdk.lib.EnableLivoxLidarImuData(handle, _cb, None) - - # ------------------------------------------------------------------ - # Consumer threads (Python-side, build messages from queued data) - # ------------------------------------------------------------------ - - def _pointcloud_consumer_loop(self) -> None: - """Drain the point cloud queue, accumulate by time window, emit PointCloud2.""" - frame_points: list[np.ndarray] = [] - frame_reflectivities: list[np.ndarray] = [] - frame_timestamp: float | None = None - frame_interval = 1.0 / self.config.frequency if self.config.frequency > 0 else 0.1 - last_emit = time.monotonic() - - while not self._stop_event.is_set(): - try: - _frame_cnt, ts_ns, xyz, reflectivities = self._pc_queue.get(timeout=0.05) - except Empty: - # Check if we should emit on timeout - if frame_points and (time.monotonic() - last_emit) >= frame_interval: - self._emit_pointcloud(frame_points, frame_reflectivities, frame_timestamp) - frame_points = [] - frame_reflectivities = [] - frame_timestamp = None - last_emit = time.monotonic() - continue - - frame_points.append(xyz) - frame_reflectivities.append(reflectivities) - if frame_timestamp is None: - frame_timestamp = ts_ns / 1e9 - - # Emit when time window is full - if (time.monotonic() - last_emit) >= frame_interval: - self._emit_pointcloud(frame_points, frame_reflectivities, frame_timestamp) - frame_points = [] - frame_reflectivities = [] - frame_timestamp = None - last_emit = time.monotonic() - - # Emit any remaining data - if frame_points: - self._emit_pointcloud(frame_points, frame_reflectivities, frame_timestamp) - - def _emit_pointcloud( - self, - frame_points: list[np.ndarray], - frame_reflectivities: list[np.ndarray], - frame_timestamp: float | None, - ) -> None: - if not self._pc_observer or self._stop_event.is_set(): - return - - all_points = np.concatenate(frame_points, axis=0) - all_reflectivities = np.concatenate(frame_reflectivities, axis=0) - - if len(all_points) == 0: - return - - # Build Open3D tensor point cloud - pcd_t = o3d.t.geometry.PointCloud() - pcd_t.point["positions"] = o3c.Tensor(all_points, dtype=o3c.float32) - # Store reflectivity as intensity (normalized to 0-1 range) - pcd_t.point["intensities"] = o3c.Tensor( - all_reflectivities.astype(np.float32).reshape(-1, 1) / 255.0, - dtype=o3c.float32, - ) - - # Optional voxel downsampling - if self.config.voxel_size is not None: - pcd_t = pcd_t.voxel_down_sample(self.config.voxel_size) - - pc2 = PointCloud2( - pointcloud=pcd_t, - frame_id=self._frame_id(self.config.frame_id), - ts=frame_timestamp if frame_timestamp else time.time(), - ) - - try: - self._pc_observer.on_next(pc2) - except Exception: - logger.debug("Error emitting point cloud", exc_info=True) - - def _imu_consumer_loop(self) -> None: - """Drain the IMU queue and emit Imu messages.""" - while not self._stop_event.is_set(): - try: - ts_ns, imu_points = self._imu_queue.get(timeout=0.5) - except Empty: - continue - - if not self._imu_observer or self._stop_event.is_set(): - continue - - ts = ts_ns / 1e9 - for i in range(len(imu_points)): - pt = imu_points[i] - imu_msg = Imu( - angular_velocity=Vector3( - float(pt["gyro_x"]), - float(pt["gyro_y"]), - float(pt["gyro_z"]), - ), - linear_acceleration=Vector3( - float(pt["acc_x"]) * GRAVITY_MS2, - float(pt["acc_y"]) * GRAVITY_MS2, - float(pt["acc_z"]) * GRAVITY_MS2, - ), - frame_id=self._frame_id("imu_link"), - ts=ts, - ) - try: - self._imu_observer.on_next(imu_msg) - except Exception: - logger.debug("Error emitting IMU data", exc_info=True) - - # ------------------------------------------------------------------ - # Public API: Observable streams - # ------------------------------------------------------------------ - - @cache - def pointcloud_stream(self) -> Observable[PointCloud2]: - """Observable stream of PointCloud2 messages (~10 Hz full-frame scans).""" - - def subscribe(observer, scheduler=None): # type: ignore[no-untyped-def] - self._pc_observer = observer - try: - self._start_sdk() - except Exception as e: - observer.on_error(e) - return - - def dispose() -> None: - self._pc_observer = None - self.stop() - - return dispose - - return backpressure(create(subscribe)) - - @cache - def imu_stream(self) -> Observable[Imu]: - """Observable stream of Imu messages (~200 Hz).""" - - def subscribe(observer, scheduler=None): # type: ignore[no-untyped-def] - self._imu_observer = observer - - def dispose() -> None: - self._imu_observer = None - - return dispose - - return backpressure(create(subscribe)) - - # ------------------------------------------------------------------ - # Lifecycle - # ------------------------------------------------------------------ - - def _start_sdk(self) -> None: - """Initialize and start the Livox SDK.""" - if self._sdk is not None: - return # already running - - self._stop_event.clear() - - # Write temp config - self._config_path = self._write_sdk_config() - - # Load and init SDK - self._sdk = LivoxSDK(self.config.sdk_lib_path) - self._sdk.init(self._config_path, self.config.host_ip) - - # Register callbacks (must keep references to prevent GC) - pc_cb = PointCloudCallbackType(self._on_point_cloud) - imu_cb = ImuDataCallbackType(self._on_imu_data) - info_cb = InfoChangeCallbackType(self._on_info_change) - - self._sdk.set_point_cloud_callback(pc_cb) - if self.config.enable_imu: - self._sdk.set_imu_callback(imu_cb) - self._sdk.set_info_change_callback(info_cb) - - # Start SDK background threads - self._sdk.start() - logger.info( - "Livox SDK started (host_ip=%s, lidar_ips=%s)", - self.config.host_ip, - self.config.lidar_ips, - ) - - # Start consumer threads - self._pc_consumer_thread = threading.Thread( - target=self._pointcloud_consumer_loop, daemon=True, name="livox-pc-consumer" - ) - self._pc_consumer_thread.start() - - if self.config.enable_imu: - self._imu_consumer_thread = threading.Thread( - target=self._imu_consumer_loop, daemon=True, name="livox-imu-consumer" - ) - self._imu_consumer_thread.start() - - def stop(self) -> None: - """Stop the SDK and all consumer threads.""" - self._stop_event.set() - - if self._sdk is not None: - self._sdk.uninit() - self._sdk = None - - for thread in [self._pc_consumer_thread, self._imu_consumer_thread]: - if thread is not None and thread.is_alive(): - thread.join(timeout=3.0) - - self._pc_consumer_thread = None - self._imu_consumer_thread = None - self._connected_handles.clear() - - # Cleanup temp config - if self._config_path: - import os - - try: - os.unlink(self._config_path) - except OSError: - pass - self._config_path = None - - logger.info("Livox Mid-360 stopped") diff --git a/dimos/hardware/sensors/lidar/livox/module.py b/dimos/hardware/sensors/lidar/livox/module.py index bdde07225f..dd4d8c2cc5 100644 --- a/dimos/hardware/sensors/lidar/livox/module.py +++ b/dimos/hardware/sensors/lidar/livox/module.py @@ -1,4 +1,4 @@ -# Copyright 2025-2026 Dimensional Inc. +# Copyright 2026 Dimensional Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -12,48 +12,118 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Livox-specific LiDAR module with IMU support.""" +"""Python NativeModule wrapper for the C++ Livox Mid-360 driver. + +Declares the same ports as LivoxLidarModule (pointcloud, imu) but delegates +all real work to the ``mid360_native`` C++ binary, which talks directly to +the Livox SDK2 C API and publishes on LCM. + +Usage:: + + from dimos.hardware.sensors.lidar.livox.module import Mid360CppModule + from dimos.core.blueprints import autoconnect + + autoconnect( + Mid360CppModule.blueprint(host_ip="192.168.1.5"), + SomeConsumer.blueprint(), + ).build().loop() +""" + +from __future__ import annotations from dataclasses import dataclass +from pathlib import Path +from typing import TYPE_CHECKING -from dimos.core import Out, rpc -from dimos.hardware.sensors.lidar.module import LidarModule, LidarModuleConfig -from dimos.msgs.sensor_msgs.Imu import Imu +from dimos.core.native_module import NativeModule, NativeModuleConfig from dimos.spec import perception +if TYPE_CHECKING: + from dimos.core import Out + from dimos.msgs.sensor_msgs.Imu import Imu + from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + +_DEFAULT_EXECUTABLE = str(Path(__file__).parent / "cpp" / "build" / "mid360_native") -@dataclass -class LivoxLidarModuleConfig(LidarModuleConfig): + +@dataclass(kw_only=True) +class Mid360CppConfig(NativeModuleConfig): + """Config for the C++ Mid-360 native module.""" + + executable: str = _DEFAULT_EXECUTABLE + host_ip: str = "192.168.1.5" + lidar_ip: str = "192.168.1.155" + frequency: float = 10.0 enable_imu: bool = True + frame_id: str = "lidar_link" + imu_frame_id: str = "imu_link" + + # SDK port configuration (match defaults in LivoxMid360Config) + cmd_data_port: int = 56100 + push_msg_port: int = 56200 + point_data_port: int = 56300 + imu_data_port: int = 56400 + log_data_port: int = 56500 + host_cmd_data_port: int = 56101 + host_push_msg_port: int = 56201 + host_point_data_port: int = 56301 + host_imu_data_port: int = 56401 + host_log_data_port: int = 56501 -class LivoxLidarModule(LidarModule, perception.IMU): - """Livox LiDAR module — pointcloud + IMU. +class Mid360CppModule(NativeModule, perception.Lidar, perception.IMU): + """Livox Mid-360 LiDAR module backed by a native C++ binary. - Extends LidarModule with IMU stream support for sensors like the Mid-360. + Ports: + pointcloud (Out[PointCloud2]): Point cloud frames at configured frequency. + imu (Out[Imu]): IMU data at ~200 Hz (if enabled). """ + default_config: type[Mid360CppConfig] = Mid360CppConfig # type: ignore[assignment] + pointcloud: Out[PointCloud2] imu: Out[Imu] - config: LivoxLidarModuleConfig # type: ignore[assignment] - default_config = LivoxLidarModuleConfig # type: ignore[assignment] - - @rpc - def start(self) -> None: - super().start() - - if self.config.enable_imu: - imu_stream = self.hardware.imu_stream() - if imu_stream is not None: - self._disposables.add( - imu_stream.subscribe(lambda imu_msg: self.imu.publish(imu_msg)), - ) + def _build_extra_args(self) -> list[str]: + """Pass hardware config to the C++ binary as CLI args.""" + cfg: Mid360CppConfig = self.config # type: ignore[assignment] + return [ + "--host_ip", + cfg.host_ip, + "--lidar_ip", + cfg.lidar_ip, + "--frequency", + str(cfg.frequency), + "--frame_id", + cfg.frame_id, + "--imu_frame_id", + cfg.imu_frame_id, + "--cmd_data_port", + str(cfg.cmd_data_port), + "--push_msg_port", + str(cfg.push_msg_port), + "--point_data_port", + str(cfg.point_data_port), + "--imu_data_port", + str(cfg.imu_data_port), + "--log_data_port", + str(cfg.log_data_port), + "--host_cmd_data_port", + str(cfg.host_cmd_data_port), + "--host_push_msg_port", + str(cfg.host_push_msg_port), + "--host_point_data_port", + str(cfg.host_point_data_port), + "--host_imu_data_port", + str(cfg.host_imu_data_port), + "--host_log_data_port", + str(cfg.host_log_data_port), + ] -livox_lidar_module = LivoxLidarModule.blueprint +mid360_cpp_module = Mid360CppModule.blueprint __all__ = [ - "LivoxLidarModule", - "LivoxLidarModuleConfig", - "livox_lidar_module", + "Mid360CppConfig", + "Mid360CppModule", + "mid360_cpp_module", ] diff --git a/dimos/hardware/sensors/lidar/livox/sdk.py b/dimos/hardware/sensors/lidar/livox/sdk.py deleted file mode 100644 index eb5fbcbccb..0000000000 --- a/dimos/hardware/sensors/lidar/livox/sdk.py +++ /dev/null @@ -1,534 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""ctypes bindings for Livox SDK2 (liblivox_lidar_sdk_shared.so). - -Provides Python access to the Livox LiDAR SDK2 C API for device -communication, point cloud streaming, and IMU data. - -The SDK .so must be pre-built from https://github.com/Livox-SDK/Livox-SDK2. -Set LIVOX_SDK2_LIB_PATH env var or install to /usr/local/lib. -""" - -from __future__ import annotations - -import ctypes -import ctypes.util -from enum import IntEnum -import logging -import os -from pathlib import Path -from typing import Any - -import numpy as np - -logger = logging.getLogger(__name__) - - -# --------------------------------------------------------------------------- -# Enums -# --------------------------------------------------------------------------- - - -class LivoxLidarDeviceType(IntEnum): - HUB = 0 - MID40 = 1 - TELE = 2 - HORIZON = 3 - MID70 = 6 - AVIA = 7 - MID360 = 9 - INDUSTRIAL_HAP = 10 - HAP = 15 - PA = 16 - - -class LivoxLidarPointDataType(IntEnum): - IMU = 0x00 - CARTESIAN_HIGH = 0x01 - CARTESIAN_LOW = 0x02 - SPHERICAL = 0x03 - - -class LivoxLidarWorkMode(IntEnum): - NORMAL = 0x01 - WAKE_UP = 0x02 - SLEEP = 0x03 - ERROR = 0x04 - POWER_ON_SELF_TEST = 0x05 - MOTOR_STARTING = 0x06 - MOTOR_STOPPING = 0x07 - UPGRADE = 0x08 - - -class LivoxLidarScanPattern(IntEnum): - NON_REPETITIVE = 0x00 - REPETITIVE = 0x01 - REPETITIVE_LOW_FRAME_RATE = 0x02 - - -class LivoxLidarStatus(IntEnum): - SEND_FAILED = -9 - HANDLER_IMPL_NOT_EXIST = -8 - INVALID_HANDLE = -7 - CHANNEL_NOT_EXIST = -6 - NOT_ENOUGH_MEMORY = -5 - TIMEOUT = -4 - NOT_SUPPORTED = -3 - NOT_CONNECTED = -2 - FAILURE = -1 - SUCCESS = 0 - - -# --------------------------------------------------------------------------- -# Packed C structures (all #pragma pack(1) in the SDK header) -# --------------------------------------------------------------------------- - - -class LivoxLidarCartesianHighRawPoint(ctypes.LittleEndianStructure): - """High-resolution cartesian point (14 bytes). Coordinates in mm.""" - - _pack_ = 1 - _fields_ = [ - ("x", ctypes.c_int32), - ("y", ctypes.c_int32), - ("z", ctypes.c_int32), - ("reflectivity", ctypes.c_uint8), - ("tag", ctypes.c_uint8), - ] - - -class LivoxLidarCartesianLowRawPoint(ctypes.LittleEndianStructure): - """Low-resolution cartesian point (8 bytes). Coordinates in cm.""" - - _pack_ = 1 - _fields_ = [ - ("x", ctypes.c_int16), - ("y", ctypes.c_int16), - ("z", ctypes.c_int16), - ("reflectivity", ctypes.c_uint8), - ("tag", ctypes.c_uint8), - ] - - -class LivoxLidarSpherPoint(ctypes.LittleEndianStructure): - """Spherical coordinate point (10 bytes).""" - - _pack_ = 1 - _fields_ = [ - ("depth", ctypes.c_uint32), - ("theta", ctypes.c_uint16), - ("phi", ctypes.c_uint16), - ("reflectivity", ctypes.c_uint8), - ("tag", ctypes.c_uint8), - ] - - -class LivoxLidarImuRawPoint(ctypes.LittleEndianStructure): - """IMU data point (24 bytes). Gyro in rad/s, accel in g.""" - - _pack_ = 1 - _fields_ = [ - ("gyro_x", ctypes.c_float), - ("gyro_y", ctypes.c_float), - ("gyro_z", ctypes.c_float), - ("acc_x", ctypes.c_float), - ("acc_y", ctypes.c_float), - ("acc_z", ctypes.c_float), - ] - - -class LivoxLidarEthernetPacket(ctypes.LittleEndianStructure): - """Point cloud / IMU ethernet packet header (32 bytes + variable data).""" - - _pack_ = 1 - _fields_ = [ - ("version", ctypes.c_uint8), - ("length", ctypes.c_uint16), - ("time_interval", ctypes.c_uint16), # unit: 0.1 us - ("dot_num", ctypes.c_uint16), - ("udp_cnt", ctypes.c_uint16), - ("frame_cnt", ctypes.c_uint8), - ("data_type", ctypes.c_uint8), - ("time_type", ctypes.c_uint8), - ("rsvd", ctypes.c_uint8 * 12), - ("crc32", ctypes.c_uint32), - ("timestamp", ctypes.c_uint8 * 8), - ("data", ctypes.c_uint8 * 1), # variable-length payload - ] - - -class LivoxLidarInfo(ctypes.LittleEndianStructure): - """Device info received on connection.""" - - _pack_ = 1 - _fields_ = [ - ("dev_type", ctypes.c_uint8), - ("sn", ctypes.c_char * 16), - ("lidar_ip", ctypes.c_char * 16), - ] - - -class LivoxLidarAsyncControlResponse(ctypes.LittleEndianStructure): - _pack_ = 1 - _fields_ = [ - ("ret_code", ctypes.c_uint8), - ("error_key", ctypes.c_uint16), - ] - - -# --------------------------------------------------------------------------- -# numpy dtypes for efficient batch parsing of point data -# --------------------------------------------------------------------------- - -CART_HIGH_DTYPE = np.dtype( - [("x", " tuple[np.ndarray, np.ndarray, np.ndarray]: - """Parse high-res cartesian points from a packet. - - Returns: - (xyz_meters, reflectivities, tags) where xyz is (N,3) float32 in meters. - """ - dot_num = packet.dot_num - if dot_num == 0: - empty = np.empty((0, 3), dtype=np.float32) - return empty, np.empty(0, dtype=np.uint8), np.empty(0, dtype=np.uint8) - - data_ptr = ctypes.addressof(packet) + LivoxLidarEthernetPacket.data.offset - buf = (ctypes.c_uint8 * (dot_num * CART_HIGH_DTYPE.itemsize)).from_address(data_ptr) - points = np.frombuffer(buf, dtype=CART_HIGH_DTYPE, count=dot_num) - - xyz = np.column_stack([points["x"], points["y"], points["z"]]).astype(np.float32) / 1000.0 - return xyz, points["reflectivity"].copy(), points["tag"].copy() - - -def parse_cartesian_low_points( - packet: LivoxLidarEthernetPacket, -) -> tuple[np.ndarray, np.ndarray, np.ndarray]: - """Parse low-res cartesian points. Returns xyz in meters.""" - dot_num = packet.dot_num - if dot_num == 0: - empty = np.empty((0, 3), dtype=np.float32) - return empty, np.empty(0, dtype=np.uint8), np.empty(0, dtype=np.uint8) - - data_ptr = ctypes.addressof(packet) + LivoxLidarEthernetPacket.data.offset - buf = (ctypes.c_uint8 * (dot_num * CART_LOW_DTYPE.itemsize)).from_address(data_ptr) - points = np.frombuffer(buf, dtype=CART_LOW_DTYPE, count=dot_num) - - xyz = np.column_stack([points["x"], points["y"], points["z"]]).astype(np.float32) / 100.0 - return xyz, points["reflectivity"].copy(), points["tag"].copy() - - -def parse_imu_data(packet: LivoxLidarEthernetPacket) -> np.ndarray: - """Parse IMU data from a packet. Returns structured array with gyro/accel fields.""" - dot_num = packet.dot_num - if dot_num == 0: - return np.empty(0, dtype=IMU_DTYPE) - - data_ptr = ctypes.addressof(packet) + LivoxLidarEthernetPacket.data.offset - buf = (ctypes.c_uint8 * (dot_num * IMU_DTYPE.itemsize)).from_address(data_ptr) - return np.frombuffer(buf, dtype=IMU_DTYPE, count=dot_num).copy() - - -def get_packet_timestamp_ns(packet: LivoxLidarEthernetPacket) -> int: - """Extract the 64-bit nanosecond timestamp from a packet.""" - return int.from_bytes(bytes(packet.timestamp), byteorder="little") - - -# --------------------------------------------------------------------------- -# SDK library loader -# --------------------------------------------------------------------------- - -_SDK_LIB_NAMES = [ - "livox_lidar_sdk_shared", - "liblivox_lidar_sdk_shared.so", - "liblivox_lidar_sdk_shared.so.2", -] - -_SDK_SEARCH_PATHS = [ - "/usr/local/lib", - "/usr/lib", - "/usr/lib/x86_64-linux-gnu", - "/usr/lib/aarch64-linux-gnu", -] - - -def _find_sdk_library(lib_path: str | None = None) -> str: - """Find the Livox SDK2 shared library. - - Search order: - 1. Explicit lib_path argument - 2. LIVOX_SDK2_LIB_PATH environment variable - 3. ctypes.util.find_library - 4. Well-known system paths - """ - if lib_path and Path(lib_path).exists(): - return lib_path - - env_path = os.environ.get("LIVOX_SDK2_LIB_PATH") - if env_path and Path(env_path).exists(): - return env_path - - for name in _SDK_LIB_NAMES: - found = ctypes.util.find_library(name) - if found: - return found - - for search_dir in _SDK_SEARCH_PATHS: - for name in _SDK_LIB_NAMES: - candidate = Path(search_dir) / name - if candidate.exists(): - return str(candidate) - - raise RuntimeError( - "Livox SDK2 shared library not found. " - "Build from https://github.com/Livox-SDK/Livox-SDK2 and install, " - "or set LIVOX_SDK2_LIB_PATH to the .so file path." - ) - - -def load_sdk(lib_path: str | None = None) -> ctypes.CDLL: - """Load the Livox SDK2 shared library and set up function signatures.""" - path = _find_sdk_library(lib_path) - lib = ctypes.CDLL(path) - logger.info("Loaded Livox SDK2 from %s", path) - - # --- SDK lifecycle --- - lib.LivoxLidarSdkInit.argtypes = [ctypes.c_char_p, ctypes.c_char_p, ctypes.c_void_p] - lib.LivoxLidarSdkInit.restype = ctypes.c_bool - - lib.LivoxLidarSdkStart.argtypes = [] - lib.LivoxLidarSdkStart.restype = ctypes.c_bool - - lib.LivoxLidarSdkUninit.argtypes = [] - lib.LivoxLidarSdkUninit.restype = None - - # --- Callbacks --- - lib.SetLivoxLidarPointCloudCallBack.argtypes = [PointCloudCallbackType, ctypes.c_void_p] - lib.SetLivoxLidarPointCloudCallBack.restype = None - - lib.SetLivoxLidarImuDataCallback.argtypes = [ImuDataCallbackType, ctypes.c_void_p] - lib.SetLivoxLidarImuDataCallback.restype = None - - lib.SetLivoxLidarInfoChangeCallback.argtypes = [InfoChangeCallbackType, ctypes.c_void_p] - lib.SetLivoxLidarInfoChangeCallback.restype = None - - lib.SetLivoxLidarInfoCallback.argtypes = [InfoCallbackType, ctypes.c_void_p] - lib.SetLivoxLidarInfoCallback.restype = None - - # --- Device configuration --- - lib.SetLivoxLidarWorkMode.argtypes = [ - ctypes.c_uint32, - ctypes.c_int32, - AsyncControlCallbackType, - ctypes.c_void_p, - ] - lib.SetLivoxLidarWorkMode.restype = ctypes.c_int32 - - lib.EnableLivoxLidarImuData.argtypes = [ - ctypes.c_uint32, - AsyncControlCallbackType, - ctypes.c_void_p, - ] - lib.EnableLivoxLidarImuData.restype = ctypes.c_int32 - - lib.DisableLivoxLidarImuData.argtypes = [ - ctypes.c_uint32, - AsyncControlCallbackType, - ctypes.c_void_p, - ] - lib.DisableLivoxLidarImuData.restype = ctypes.c_int32 - - lib.SetLivoxLidarPclDataType.argtypes = [ - ctypes.c_uint32, - ctypes.c_int32, - AsyncControlCallbackType, - ctypes.c_void_p, - ] - lib.SetLivoxLidarPclDataType.restype = ctypes.c_int32 - - lib.SetLivoxLidarScanPattern.argtypes = [ - ctypes.c_uint32, - ctypes.c_int32, - AsyncControlCallbackType, - ctypes.c_void_p, - ] - lib.SetLivoxLidarScanPattern.restype = ctypes.c_int32 - - # --- Console logging --- - lib.DisableLivoxSdkConsoleLogger.argtypes = [] - lib.DisableLivoxSdkConsoleLogger.restype = None - - return lib - - -class LivoxSDK: - """Convenience wrapper managing SDK lifecycle and callback registration. - - Prevents garbage collection of ctypes callback references (which would - cause segfaults when the SDK tries to call them from C threads). - """ - - def __init__(self, lib_path: str | None = None) -> None: - self._lib = load_sdk(lib_path) - self._initialized = False - # prevent GC of callback C function pointers - self._callbacks: dict[str, Any] = {} - - @property - def lib(self) -> ctypes.CDLL: - return self._lib - - def init(self, config_path: str, host_ip: str, quiet: bool = True) -> None: - """Initialize the SDK with a JSON config file and host IP.""" - if self._initialized: - raise RuntimeError("SDK already initialized. Call uninit() first.") - if quiet: - self._lib.DisableLivoxSdkConsoleLogger() - ok = self._lib.LivoxLidarSdkInit( - config_path.encode(), - host_ip.encode(), - None, - ) - if not ok: - raise RuntimeError( - f"LivoxLidarSdkInit failed (config={config_path}, host_ip={host_ip})" - ) - self._initialized = True - - def start(self) -> None: - """Start the SDK (begins device communication on background threads).""" - if not self._initialized: - raise RuntimeError("SDK not initialized. Call init() first.") - ok = self._lib.LivoxLidarSdkStart() - if not ok: - raise RuntimeError("LivoxLidarSdkStart failed") - - def uninit(self) -> None: - """Shut down the SDK and release resources.""" - if self._initialized: - self._lib.LivoxLidarSdkUninit() - self._initialized = False - self._callbacks.clear() - - def set_point_cloud_callback(self, callback: Any) -> None: - self._callbacks["pointcloud"] = callback # prevent GC - self._lib.SetLivoxLidarPointCloudCallBack(callback, None) - - def set_imu_callback(self, callback: Any) -> None: - self._callbacks["imu"] = callback - self._lib.SetLivoxLidarImuDataCallback(callback, None) - - def set_info_change_callback(self, callback: Any) -> None: - self._callbacks["info_change"] = callback - self._lib.SetLivoxLidarInfoChangeCallback(callback, None) - - def set_info_callback(self, callback: Any) -> None: - self._callbacks["info"] = callback - self._lib.SetLivoxLidarInfoCallback(callback, None) - - def set_work_mode(self, handle: int, mode: LivoxLidarWorkMode, callback: Any = None) -> int: - cb = callback or AsyncControlCallbackType(0) # type: ignore[arg-type] - if callback: - self._callbacks[f"work_mode_{handle}"] = callback - return self._lib.SetLivoxLidarWorkMode(handle, int(mode), cb, None) # type: ignore[no-any-return] - - def enable_imu_data(self, handle: int, callback: Any = None) -> int: - cb = callback or AsyncControlCallbackType(0) # type: ignore[arg-type] - if callback: - self._callbacks[f"imu_enable_{handle}"] = callback - return self._lib.EnableLivoxLidarImuData(handle, cb, None) # type: ignore[no-any-return] - - def disable_imu_data(self, handle: int, callback: Any = None) -> int: - cb = callback or AsyncControlCallbackType(0) # type: ignore[arg-type] - if callback: - self._callbacks[f"imu_disable_{handle}"] = callback - return self._lib.DisableLivoxLidarImuData(handle, cb, None) # type: ignore[no-any-return] diff --git a/dimos/hardware/sensors/lidar/livox/test_spec_compliance.py b/dimos/hardware/sensors/lidar/livox/test_spec_compliance.py index 79abb58dbd..92bf0e7c6e 100644 --- a/dimos/hardware/sensors/lidar/livox/test_spec_compliance.py +++ b/dimos/hardware/sensors/lidar/livox/test_spec_compliance.py @@ -16,14 +16,14 @@ from __future__ import annotations -from dimos.hardware.sensors.lidar.livox.module import LivoxLidarModule +from dimos.hardware.sensors.lidar.livox.module import Mid360CppModule from dimos.spec import perception from dimos.spec.utils import assert_implements_protocol def test_livox_module_implements_lidar_spec() -> None: - assert_implements_protocol(LivoxLidarModule, perception.Lidar) + assert_implements_protocol(Mid360CppModule, perception.Lidar) def test_livox_module_implements_imu_spec() -> None: - assert_implements_protocol(LivoxLidarModule, perception.IMU) + assert_implements_protocol(Mid360CppModule, perception.IMU) From e24ea8e561be959ac84511667f627b13dd05a72f Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Thu, 12 Feb 2026 20:06:57 +0800 Subject: [PATCH 09/48] correct config defaults --- dimos/hardware/sensors/lidar/livox/module.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/dimos/hardware/sensors/lidar/livox/module.py b/dimos/hardware/sensors/lidar/livox/module.py index dd4d8c2cc5..d6c4eb4744 100644 --- a/dimos/hardware/sensors/lidar/livox/module.py +++ b/dimos/hardware/sensors/lidar/livox/module.py @@ -79,7 +79,9 @@ class Mid360CppModule(NativeModule, perception.Lidar, perception.IMU): imu (Out[Imu]): IMU data at ~200 Hz (if enabled). """ - default_config: type[Mid360CppConfig] = Mid360CppConfig # type: ignore[assignment] + config: Mid360CppConfig + default_config = Mid360CppConfig + pointcloud: Out[PointCloud2] imu: Out[Imu] From 71ee68d369c8d4e4ece2096dbf3fdf4feed6f69a Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Thu, 12 Feb 2026 21:33:08 +0800 Subject: [PATCH 10/48] Add FAST-LIO2 native module with Livox Mid-360 SLAM integration Integrates FAST-LIO-NON-ROS directly with Livox SDK2 as a dimos NativeModule. The C++ binary feeds live LiDAR/IMU data into FAST-LIO's EKF-LOAM SLAM and publishes aggregated world-frame point clouds and odometry over LCM. Includes rate-limited output (pointcloud_freq, odom_freq), Odometry.to_rerun() for visualization, and nix flake deps. --- .../sensors/lidar/fastlio2/__init__.py | 0 .../sensors/lidar/fastlio2/cpp/CMakeLists.txt | 91 +++ .../lidar/fastlio2/cpp/config/mid360.json | 38 ++ .../fastlio2/cpp/dimos_native_module.hpp | 64 ++ .../sensors/lidar/fastlio2/cpp/main.cpp | 578 ++++++++++++++++++ .../lidar/fastlio2/fastlio_blueprints.py | 36 ++ .../hardware/sensors/lidar/fastlio2/module.py | 191 ++++++ .../lidar/fastlio2/test_spec_compliance.py | 29 + .../sensors/lidar/test_spec_compliance.py | 34 ++ dimos/msgs/nav_msgs/Odometry.py | 19 + dimos/robot/all_blueprints.py | 6 +- flake.nix | 5 + 12 files changed, 1089 insertions(+), 2 deletions(-) create mode 100644 dimos/hardware/sensors/lidar/fastlio2/__init__.py create mode 100644 dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt create mode 100644 dimos/hardware/sensors/lidar/fastlio2/cpp/config/mid360.json create mode 100644 dimos/hardware/sensors/lidar/fastlio2/cpp/dimos_native_module.hpp create mode 100644 dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp create mode 100644 dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py create mode 100644 dimos/hardware/sensors/lidar/fastlio2/module.py create mode 100644 dimos/hardware/sensors/lidar/fastlio2/test_spec_compliance.py create mode 100644 dimos/hardware/sensors/lidar/test_spec_compliance.py diff --git a/dimos/hardware/sensors/lidar/fastlio2/__init__.py b/dimos/hardware/sensors/lidar/fastlio2/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt b/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt new file mode 100644 index 0000000000..301c257ddd --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt @@ -0,0 +1,91 @@ +cmake_minimum_required(VERSION 3.14) +project(fastlio2_native CXX) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3") + +# FAST-LIO-NON-ROS source directory (local reference) +set(FASTLIO_DIR "$ENV{HOME}/coding/FAST-LIO-NON-ROS") + +# OpenMP for parallel processing +find_package(OpenMP QUIET) +if(OpenMP_CXX_FOUND) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") +endif() + +# MP defines (same logic as FAST-LIO) +message("CPU architecture: ${CMAKE_SYSTEM_PROCESSOR}") +if(CMAKE_SYSTEM_PROCESSOR MATCHES "(x86)|(X86)|(amd64)|(AMD64)") + include(ProcessorCount) + ProcessorCount(N) + if(N GREATER 4) + add_definitions(-DMP_EN -DMP_PROC_NUM=3) + elseif(N GREATER 3) + add_definitions(-DMP_EN -DMP_PROC_NUM=2) + else() + add_definitions(-DMP_PROC_NUM=1) + endif() +else() + add_definitions(-DMP_PROC_NUM=1) +endif() + +# Fetch dimos-lcm for C++ message headers +include(FetchContent) +FetchContent_Declare(dimos_lcm + GIT_REPOSITORY https://github.com/dimensionalOS/dimos-lcm.git + GIT_TAG main + GIT_SHALLOW TRUE +) +FetchContent_MakeAvailable(dimos_lcm) + +# LCM +find_package(PkgConfig REQUIRED) +pkg_check_modules(LCM REQUIRED lcm) + +# Eigen3 +find_package(Eigen3 REQUIRED) + +# PCL +find_package(PCL 1.8 REQUIRED) + +# nlohmann_json (FAST-LIO config parsing) +find_package(nlohmann_json 3.2.0 REQUIRED) + +# Livox SDK2 (installed to /usr/local) +find_library(LIVOX_SDK livox_lidar_sdk_shared HINTS /usr/local/lib) +if(NOT LIVOX_SDK) + message(FATAL_ERROR "Livox SDK2 not found. Install from https://github.com/Livox-SDK/Livox-SDK2") +endif() + +add_executable(fastlio2_native + main.cpp + ${FASTLIO_DIR}/src/preprocess.cpp + ${FASTLIO_DIR}/include/ikd-Tree/ikd_Tree.cpp +) + +target_include_directories(fastlio2_native PRIVATE + ${FASTLIO_DIR}/include + ${FASTLIO_DIR}/src + ${dimos_lcm_SOURCE_DIR}/generated/cpp_lcm_msgs + ${LCM_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} + ${PCL_INCLUDE_DIRS} + ${CMAKE_CURRENT_SOURCE_DIR} + /usr/local/include # Livox SDK2 headers +) + +target_link_libraries(fastlio2_native PRIVATE + ${LCM_LIBRARIES} + ${LIVOX_SDK} + ${PCL_LIBRARIES} + nlohmann_json::nlohmann_json +) + +if(OpenMP_CXX_FOUND) + target_link_libraries(fastlio2_native PRIVATE OpenMP::OpenMP_CXX) +endif() + +target_link_directories(fastlio2_native PRIVATE + ${LCM_LIBRARY_DIRS} +) diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/config/mid360.json b/dimos/hardware/sensors/lidar/fastlio2/cpp/config/mid360.json new file mode 100644 index 0000000000..ff6cc6dbf6 --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/config/mid360.json @@ -0,0 +1,38 @@ +{ + "common": { + "time_sync_en": false, + "time_offset_lidar_to_imu": 0.0, + "msr_freq": 50.0, + "main_freq": 5000.0 + }, + "preprocess": { + "lidar_type": 1, + "scan_line": 1, + "blind": 1 + }, + "mapping": { + "acc_cov": 0.1, + "gyr_cov": 0.1, + "b_acc_cov": 0.0001, + "b_gyr_cov": 0.0001, + "fov_degree": 360, + "det_range": 100.0, + "extrinsic_est_en": true, + "extrinsic_T": [ + 0.04165, + 0.02326, + -0.0284 + ], + "extrinsic_R": [ + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 1.0 + ] + } +} diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/dimos_native_module.hpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/dimos_native_module.hpp new file mode 100644 index 0000000000..73ee618c34 --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/dimos_native_module.hpp @@ -0,0 +1,64 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 +// +// Lightweight header-only helper for dimos NativeModule C++ binaries. +// Parses -- CLI args passed by the Python NativeModule wrapper. + +#pragma once + +#include +#include +#include + +namespace dimos { + +class NativeModule { +public: + NativeModule(int argc, char** argv) { + for (int i = 1; i < argc; ++i) { + std::string arg(argv[i]); + if (arg.size() > 2 && arg[0] == '-' && arg[1] == '-' && i + 1 < argc) { + args_[arg.substr(2)] = argv[++i]; + } + } + } + + /// Get the full LCM channel string for a declared port. + /// Format is "#", e.g. "/pointcloud#sensor_msgs.PointCloud2". + /// This is the exact channel name used by Python LCMTransport subscribers. + const std::string& topic(const std::string& port) const { + auto it = args_.find(port); + if (it == args_.end()) { + throw std::runtime_error("NativeModule: no topic for port '" + port + "'"); + } + return it->second; + } + + /// Get a string arg value, or a default if not present. + std::string arg(const std::string& key, const std::string& default_val = "") const { + auto it = args_.find(key); + return it != args_.end() ? it->second : default_val; + } + + /// Get a float arg value, or a default if not present. + float arg_float(const std::string& key, float default_val = 0.0f) const { + auto it = args_.find(key); + return it != args_.end() ? std::stof(it->second) : default_val; + } + + /// Get an int arg value, or a default if not present. + int arg_int(const std::string& key, int default_val = 0) const { + auto it = args_.find(key); + return it != args_.end() ? std::stoi(it->second) : default_val; + } + + /// Check if a port/arg was provided. + bool has(const std::string& key) const { + return args_.count(key) > 0; + } + +private: + std::map args_; +}; + +} // namespace dimos diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp new file mode 100644 index 0000000000..d931ba27b4 --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp @@ -0,0 +1,578 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 +// +// FAST-LIO2 + Livox Mid-360 native module for dimos NativeModule framework. +// +// Binds Livox SDK2 directly into FAST-LIO-NON-ROS: SDK callbacks feed +// CustomMsg/Imu to FastLio, which performs EKF-LOAM SLAM. Registered +// (world-frame) point clouds and odometry are published on LCM. +// +// Usage: +// ./fastlio2_native \ +// --lidar '/lidar#sensor_msgs.PointCloud2' \ +// --odometry '/odometry#nav_msgs.Odometry' \ +// --config_path /path/to/mid360.json \ +// --host_ip 192.168.1.5 --lidar_ip 192.168.1.155 + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "dimos_native_module.hpp" + +// dimos LCM message headers +#include "geometry_msgs/Quaternion.hpp" +#include "geometry_msgs/Vector3.hpp" +#include "nav_msgs/Odometry.hpp" +#include "sensor_msgs/Imu.hpp" +#include "sensor_msgs/PointCloud2.hpp" +#include "sensor_msgs/PointField.hpp" +#include "std_msgs/Header.hpp" +#include "std_msgs/Time.hpp" + +// FAST-LIO (header-only core, compiled sources linked via CMake) +#include "fast_lio.hpp" + +// Gravity constant for converting accelerometer data from g to m/s^2 +static constexpr double GRAVITY_MS2 = 9.80665; + +// Livox data_type values +static constexpr uint8_t DATA_TYPE_IMU = 0x00; +static constexpr uint8_t DATA_TYPE_CARTESIAN_HIGH = 0x01; +static constexpr uint8_t DATA_TYPE_CARTESIAN_LOW = 0x02; + +// --------------------------------------------------------------------------- +// Global state +// --------------------------------------------------------------------------- + +static std::atomic g_running{true}; +static lcm::LCM* g_lcm = nullptr; +static FastLio* g_fastlio = nullptr; + +static std::string g_lidar_topic; +static std::string g_odometry_topic; +static std::string g_frame_id = "map"; +static std::string g_child_frame_id = "body"; +static float g_frequency = 10.0f; + +// Frame accumulator (Livox SDK raw → CustomMsg) +static std::mutex g_pc_mutex; +static std::vector g_accumulated_points; +static uint64_t g_frame_start_ns = 0; +static bool g_frame_has_timestamp = false; + +// Track whether FastLio has produced a valid result this cycle +static std::atomic g_has_new_result{false}; + +// --------------------------------------------------------------------------- +// Helpers +// --------------------------------------------------------------------------- + +static std_msgs::Time time_from_seconds(double t) { + std_msgs::Time ts; + ts.sec = static_cast(t); + ts.nsec = static_cast((t - ts.sec) * 1e9); + return ts; +} + +static uint64_t get_timestamp_ns(const LivoxLidarEthernetPacket* pkt) { + uint64_t ns = 0; + std::memcpy(&ns, pkt->timestamp, sizeof(uint64_t)); + return ns; +} + +static std_msgs::Header make_header(const std::string& frame_id, double ts) { + static std::atomic seq{0}; + std_msgs::Header h; + h.seq = seq.fetch_add(1, std::memory_order_relaxed); + h.stamp = time_from_seconds(ts); + h.frame_id = frame_id; + return h; +} + +// --------------------------------------------------------------------------- +// Publish lidar (world-frame point cloud) +// --------------------------------------------------------------------------- + +static void publish_lidar(PointCloudXYZI::Ptr cloud, double timestamp) { + if (!g_lcm || !cloud || cloud->empty()) return; + + int num_points = static_cast(cloud->size()); + + sensor_msgs::PointCloud2 pc; + pc.header = make_header(g_frame_id, timestamp); + pc.height = 1; + pc.width = num_points; + pc.is_bigendian = 0; + pc.is_dense = 1; + + // Fields: x, y, z, intensity (float32 each) + pc.fields_length = 4; + pc.fields.resize(4); + + auto make_field = [](const std::string& name, int32_t offset) { + sensor_msgs::PointField f; + f.name = name; + f.offset = offset; + f.datatype = sensor_msgs::PointField::FLOAT32; + f.count = 1; + return f; + }; + + pc.fields[0] = make_field("x", 0); + pc.fields[1] = make_field("y", 4); + pc.fields[2] = make_field("z", 8); + pc.fields[3] = make_field("intensity", 12); + + pc.point_step = 16; + pc.row_step = pc.point_step * num_points; + + pc.data_length = pc.row_step; + pc.data.resize(pc.data_length); + + for (int i = 0; i < num_points; ++i) { + float* dst = reinterpret_cast(pc.data.data() + i * 16); + dst[0] = cloud->points[i].x; + dst[1] = cloud->points[i].y; + dst[2] = cloud->points[i].z; + dst[3] = cloud->points[i].intensity; + } + + g_lcm->publish(g_lidar_topic, &pc); +} + +// --------------------------------------------------------------------------- +// Publish odometry +// --------------------------------------------------------------------------- + +static void publish_odometry(const custom_messages::Odometry& odom, double timestamp) { + if (!g_lcm) return; + + nav_msgs::Odometry msg; + msg.header = make_header(g_frame_id, timestamp); + msg.child_frame_id = g_child_frame_id; + + // Pose + msg.pose.pose.position.x = odom.pose.pose.position.x; + msg.pose.pose.position.y = odom.pose.pose.position.y; + msg.pose.pose.position.z = odom.pose.pose.position.z; + msg.pose.pose.orientation.x = odom.pose.pose.orientation.x; + msg.pose.pose.orientation.y = odom.pose.pose.orientation.y; + msg.pose.pose.orientation.z = odom.pose.pose.orientation.z; + msg.pose.pose.orientation.w = odom.pose.pose.orientation.w; + + // Covariance (fixed-size double[36]) + for (int i = 0; i < 36; ++i) { + msg.pose.covariance[i] = odom.pose.covariance[i]; + } + + // Twist (zero — FAST-LIO doesn't output velocity directly) + msg.twist.twist.linear.x = 0; + msg.twist.twist.linear.y = 0; + msg.twist.twist.linear.z = 0; + msg.twist.twist.angular.x = 0; + msg.twist.twist.angular.y = 0; + msg.twist.twist.angular.z = 0; + std::memset(msg.twist.covariance, 0, sizeof(msg.twist.covariance)); + + g_lcm->publish(g_odometry_topic, &msg); +} + +// --------------------------------------------------------------------------- +// Livox SDK callbacks +// --------------------------------------------------------------------------- + +static void on_point_cloud(const uint32_t /*handle*/, const uint8_t /*dev_type*/, + LivoxLidarEthernetPacket* data, void* /*client_data*/) { + if (!g_running.load() || data == nullptr) return; + + uint64_t ts_ns = get_timestamp_ns(data); + uint16_t dot_num = data->dot_num; + + std::lock_guard lock(g_pc_mutex); + + if (!g_frame_has_timestamp) { + g_frame_start_ns = ts_ns; + g_frame_has_timestamp = true; + } + + if (data->data_type == DATA_TYPE_CARTESIAN_HIGH) { + auto* pts = reinterpret_cast(data->data); + for (uint16_t i = 0; i < dot_num; ++i) { + custom_messages::CustomPoint cp; + cp.x = static_cast(pts[i].x) / 1000.0; // mm → m + cp.y = static_cast(pts[i].y) / 1000.0; + cp.z = static_cast(pts[i].z) / 1000.0; + cp.reflectivity = pts[i].reflectivity; + cp.tag = pts[i].tag; + cp.line = 0; // Mid-360: non-repetitive, single "line" + cp.offset_time = static_cast(ts_ns - g_frame_start_ns); + g_accumulated_points.push_back(cp); + } + } else if (data->data_type == DATA_TYPE_CARTESIAN_LOW) { + auto* pts = reinterpret_cast(data->data); + for (uint16_t i = 0; i < dot_num; ++i) { + custom_messages::CustomPoint cp; + cp.x = static_cast(pts[i].x) / 100.0; // cm → m + cp.y = static_cast(pts[i].y) / 100.0; + cp.z = static_cast(pts[i].z) / 100.0; + cp.reflectivity = pts[i].reflectivity; + cp.tag = pts[i].tag; + cp.line = 0; + cp.offset_time = static_cast(ts_ns - g_frame_start_ns); + g_accumulated_points.push_back(cp); + } + } +} + +static void on_imu_data(const uint32_t /*handle*/, const uint8_t /*dev_type*/, + LivoxLidarEthernetPacket* data, void* /*client_data*/) { + if (!g_running.load() || data == nullptr || !g_fastlio) return; + + double ts = static_cast(get_timestamp_ns(data)) / 1e9; + auto* imu_pts = reinterpret_cast(data->data); + uint16_t dot_num = data->dot_num; + + for (uint16_t i = 0; i < dot_num; ++i) { + auto imu_msg = boost::make_shared(); + imu_msg->header.stamp = custom_messages::Time().fromSec(ts); + imu_msg->header.seq = 0; + imu_msg->header.frame_id = "livox_frame"; + + imu_msg->orientation.x = 0.0; + imu_msg->orientation.y = 0.0; + imu_msg->orientation.z = 0.0; + imu_msg->orientation.w = 1.0; + for (int j = 0; j < 9; ++j) + imu_msg->orientation_covariance[j] = 0.0; + + imu_msg->angular_velocity.x = static_cast(imu_pts[i].gyro_x); + imu_msg->angular_velocity.y = static_cast(imu_pts[i].gyro_y); + imu_msg->angular_velocity.z = static_cast(imu_pts[i].gyro_z); + for (int j = 0; j < 9; ++j) + imu_msg->angular_velocity_covariance[j] = 0.0; + + imu_msg->linear_acceleration.x = static_cast(imu_pts[i].acc_x) * GRAVITY_MS2; + imu_msg->linear_acceleration.y = static_cast(imu_pts[i].acc_y) * GRAVITY_MS2; + imu_msg->linear_acceleration.z = static_cast(imu_pts[i].acc_z) * GRAVITY_MS2; + for (int j = 0; j < 9; ++j) + imu_msg->linear_acceleration_covariance[j] = 0.0; + + g_fastlio->feed_imu(imu_msg); + } +} + +static void on_info_change(const uint32_t handle, const LivoxLidarInfo* info, + void* /*client_data*/) { + if (info == nullptr) return; + + char sn[17] = {}; + std::memcpy(sn, info->sn, 16); + char ip[17] = {}; + std::memcpy(ip, info->lidar_ip, 16); + + printf("[fastlio2] Device connected: handle=%u type=%u sn=%s ip=%s\n", + handle, info->dev_type, sn, ip); + + SetLivoxLidarWorkMode(handle, kLivoxLidarNormal, nullptr, nullptr); + EnableLivoxLidarImuData(handle, nullptr, nullptr); +} + +// --------------------------------------------------------------------------- +// Signal handling +// --------------------------------------------------------------------------- + +static void signal_handler(int /*sig*/) { + g_running.store(false); +} + +// --------------------------------------------------------------------------- +// Livox SDK config file generation (same pattern as mid360_native) +// --------------------------------------------------------------------------- + +struct SdkPorts { + int cmd_data = 56100; + int push_msg = 56200; + int point_data = 56300; + int imu_data = 56400; + int log_data = 56500; + int host_cmd_data = 56101; + int host_push_msg = 56201; + int host_point_data = 56301; + int host_imu_data = 56401; + int host_log_data = 56501; +}; + +static std::string write_sdk_config(const std::string& host_ip, + const std::string& lidar_ip, + const SdkPorts& ports) { + char tmpl[] = "/tmp/livox_fastlio2_XXXXXX"; + int fd = mkstemp(tmpl); + if (fd < 0) { + perror("mkstemp"); + return ""; + } + + FILE* fp = fdopen(fd, "w"); + if (!fp) { + perror("fdopen"); + close(fd); + return ""; + } + + fprintf(fp, + "{\n" + " \"MID360\": {\n" + " \"lidar_net_info\": {\n" + " \"cmd_data_port\": %d,\n" + " \"push_msg_port\": %d,\n" + " \"point_data_port\": %d,\n" + " \"imu_data_port\": %d,\n" + " \"log_data_port\": %d\n" + " },\n" + " \"host_net_info\": [\n" + " {\n" + " \"host_ip\": \"%s\",\n" + " \"multicast_ip\": \"224.1.1.5\",\n" + " \"cmd_data_port\": %d,\n" + " \"push_msg_port\": %d,\n" + " \"point_data_port\": %d,\n" + " \"imu_data_port\": %d,\n" + " \"log_data_port\": %d\n" + " }\n" + " ]\n" + " }\n" + "}\n", + ports.cmd_data, ports.push_msg, ports.point_data, + ports.imu_data, ports.log_data, + host_ip.c_str(), + ports.host_cmd_data, ports.host_push_msg, ports.host_point_data, + ports.host_imu_data, ports.host_log_data); + fclose(fp); + + return tmpl; +} + +// --------------------------------------------------------------------------- +// Main +// --------------------------------------------------------------------------- + +int main(int argc, char** argv) { + dimos::NativeModule mod(argc, argv); + + // Required: LCM topics for output ports + g_lidar_topic = mod.has("lidar") ? mod.topic("lidar") : ""; + g_odometry_topic = mod.has("odometry") ? mod.topic("odometry") : ""; + + if (g_lidar_topic.empty() && g_odometry_topic.empty()) { + fprintf(stderr, "Error: at least one of --lidar or --odometry is required\n"); + return 1; + } + + // FAST-LIO config path + std::string config_path = mod.arg("config_path", ""); + if (config_path.empty()) { + fprintf(stderr, "Error: --config_path is required\n"); + return 1; + } + + // Livox hardware config + std::string host_ip = mod.arg("host_ip", "192.168.1.5"); + std::string lidar_ip = mod.arg("lidar_ip", "192.168.1.155"); + g_frequency = mod.arg_float("frequency", 10.0f); + g_frame_id = mod.arg("frame_id", "map"); + g_child_frame_id = mod.arg("child_frame_id", "body"); + float pointcloud_freq = mod.arg_float("pointcloud_freq", 5.0f); + float odom_freq = mod.arg_float("odom_freq", 50.0f); + + // SDK network ports + SdkPorts ports; + ports.cmd_data = mod.arg_int("cmd_data_port", 56100); + ports.push_msg = mod.arg_int("push_msg_port", 56200); + ports.point_data = mod.arg_int("point_data_port", 56300); + ports.imu_data = mod.arg_int("imu_data_port", 56400); + ports.log_data = mod.arg_int("log_data_port", 56500); + ports.host_cmd_data = mod.arg_int("host_cmd_data_port", 56101); + ports.host_push_msg = mod.arg_int("host_push_msg_port", 56201); + ports.host_point_data = mod.arg_int("host_point_data_port", 56301); + ports.host_imu_data = mod.arg_int("host_imu_data_port", 56401); + ports.host_log_data = mod.arg_int("host_log_data_port", 56501); + + printf("[fastlio2] Starting FAST-LIO2 + Livox Mid-360 native module\n"); + printf("[fastlio2] lidar topic: %s\n", + g_lidar_topic.empty() ? "(disabled)" : g_lidar_topic.c_str()); + printf("[fastlio2] odometry topic: %s\n", + g_odometry_topic.empty() ? "(disabled)" : g_odometry_topic.c_str()); + printf("[fastlio2] config: %s\n", config_path.c_str()); + printf("[fastlio2] host_ip: %s lidar_ip: %s frequency: %.1f Hz\n", + host_ip.c_str(), lidar_ip.c_str(), g_frequency); + printf("[fastlio2] pointcloud_freq: %.1f Hz odom_freq: %.1f Hz\n", + pointcloud_freq, odom_freq); + + // Signal handlers + signal(SIGTERM, signal_handler); + signal(SIGINT, signal_handler); + + // Init LCM + lcm::LCM lcm; + if (!lcm.good()) { + fprintf(stderr, "Error: LCM init failed\n"); + return 1; + } + g_lcm = &lcm; + + // Init FAST-LIO with config + printf("[fastlio2] Initializing FAST-LIO...\n"); + FastLio fast_lio(config_path); + g_fastlio = &fast_lio; + printf("[fastlio2] FAST-LIO initialized.\n"); + + // Write Livox SDK config + std::string sdk_config_path = write_sdk_config(host_ip, lidar_ip, ports); + if (sdk_config_path.empty()) { + fprintf(stderr, "Error: failed to write SDK config\n"); + return 1; + } + + // Init Livox SDK + if (!LivoxLidarSdkInit(sdk_config_path.c_str(), host_ip.c_str())) { + fprintf(stderr, "Error: LivoxLidarSdkInit failed\n"); + std::remove(sdk_config_path.c_str()); + return 1; + } + + // Register SDK callbacks + SetLivoxLidarPointCloudCallBack(on_point_cloud, nullptr); + SetLivoxLidarImuDataCallback(on_imu_data, nullptr); + SetLivoxLidarInfoChangeCallback(on_info_change, nullptr); + + // Start SDK + if (!LivoxLidarSdkStart()) { + fprintf(stderr, "Error: LivoxLidarSdkStart failed\n"); + LivoxLidarSdkUninit(); + std::remove(sdk_config_path.c_str()); + return 1; + } + + printf("[fastlio2] SDK started, waiting for device...\n"); + + // Main loop + auto frame_interval = std::chrono::microseconds( + static_cast(1e6 / g_frequency)); + auto last_emit = std::chrono::steady_clock::now(); + const double process_period_ms = 1000.0 / 5000.0; // FAST-LIO processes at ~5kHz + + // Rate limiters for output publishing + auto pc_interval = std::chrono::microseconds( + static_cast(1e6 / pointcloud_freq)); + auto odom_interval = std::chrono::microseconds( + static_cast(1e6 / odom_freq)); + auto last_pc_publish = std::chrono::steady_clock::now(); + auto last_odom_publish = std::chrono::steady_clock::now(); + + // Accumulate world-frame scans between pointcloud publishes + PointCloudXYZI::Ptr accumulated_scan(new PointCloudXYZI()); + + while (g_running.load()) { + auto loop_start = std::chrono::high_resolution_clock::now(); + + // At frame rate: build CustomMsg from accumulated points and feed to FAST-LIO + auto now = std::chrono::steady_clock::now(); + if (now - last_emit >= frame_interval) { + std::vector points; + uint64_t frame_start = 0; + + { + std::lock_guard lock(g_pc_mutex); + if (!g_accumulated_points.empty()) { + points.swap(g_accumulated_points); + frame_start = g_frame_start_ns; + g_frame_has_timestamp = false; + } + } + + if (!points.empty()) { + // Build CustomMsg + auto lidar_msg = boost::make_shared(); + lidar_msg->header.seq = 0; + lidar_msg->header.stamp = custom_messages::Time().fromSec( + static_cast(frame_start) / 1e9); + lidar_msg->header.frame_id = "livox_frame"; + lidar_msg->timebase = frame_start; + lidar_msg->lidar_id = 0; + for (int i = 0; i < 3; i++) + lidar_msg->rsvd[i] = 0; + lidar_msg->point_num = static_cast(points.size()); + lidar_msg->points = std::move(points); + + fast_lio.feed_lidar(lidar_msg); + } + + last_emit = now; + } + + // Run FAST-LIO processing step (high frequency) + fast_lio.process(); + + // Check for new results and accumulate/publish (rate-limited) + auto pose = fast_lio.get_pose(); + if (!pose.empty() && (pose[0] != 0.0 || pose[1] != 0.0 || pose[2] != 0.0)) { + double ts = std::chrono::duration( + std::chrono::system_clock::now().time_since_epoch()).count(); + + // Accumulate registered scans between publishes + if (!g_lidar_topic.empty()) { + auto world_cloud = fast_lio.get_world_cloud(); + if (world_cloud && !world_cloud->empty()) { + *accumulated_scan += *world_cloud; + } + + // Publish aggregated cloud at pointcloud_freq + if (now - last_pc_publish >= pc_interval && !accumulated_scan->empty()) { + publish_lidar(accumulated_scan, ts); + accumulated_scan->clear(); + last_pc_publish = now; + } + } + + // Publish odometry (rate-limited to odom_freq) + if (!g_odometry_topic.empty() && (now - last_odom_publish >= odom_interval)) { + publish_odometry(fast_lio.get_odometry(), ts); + last_odom_publish = now; + } + } + + // Handle LCM messages + lcm.handleTimeout(0); + + // Rate control (~5kHz processing) + auto loop_end = std::chrono::high_resolution_clock::now(); + auto elapsed_ms = std::chrono::duration(loop_end - loop_start).count(); + if (elapsed_ms < process_period_ms) { + std::this_thread::sleep_for(std::chrono::microseconds( + static_cast((process_period_ms - elapsed_ms) * 1000))); + } + } + + // Cleanup + printf("[fastlio2] Shutting down...\n"); + g_fastlio = nullptr; + LivoxLidarSdkUninit(); + std::remove(sdk_config_path.c_str()); + g_lcm = nullptr; + + printf("[fastlio2] Done.\n"); + return 0; +} diff --git a/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py b/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py new file mode 100644 index 0000000000..2dded02a32 --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py @@ -0,0 +1,36 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from dimos.core.blueprints import autoconnect +from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2Module +from dimos.mapping.voxels import VoxelGridMapper +from dimos.visualization.rerun.bridge import rerun_bridge + +mid360_fastlio = autoconnect( + FastLio2Module.blueprint(), + rerun_bridge(), +).global_config(n_dask_workers=2, robot_model="mid360_fastlio2") + + +rerun_config = {} + +mid360_fastlio_mapper = autoconnect( + FastLio2Module.blueprint(), + VoxelGridMapper.blueprint(publish_interval=0.5, voxel_size=0.25, carve_columns=False), + rerun_bridge( + visual_override={ + "world/global_map": lambda grid: grid.to_rerun(voxel_size=0.25, mode="boxes"), + } + ), +).global_config(n_dask_workers=2, robot_model="mid360_fastlio2_voxels") diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py new file mode 100644 index 0000000000..5d97ee2cdd --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -0,0 +1,191 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Python NativeModule wrapper for the FAST-LIO2 + Livox Mid-360 binary. + +Binds Livox SDK2 directly into FAST-LIO-NON-ROS for real-time LiDAR SLAM. +Outputs registered (world-frame) point clouds and odometry with covariance. + +Usage:: + + from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2Module + from dimos.core.blueprints import autoconnect + + autoconnect( + FastLio2Module.blueprint(host_ip="192.168.1.5"), + SomeConsumer.blueprint(), + ).build().loop() +""" + +from __future__ import annotations + +from dataclasses import dataclass, field +import json +from pathlib import Path +import tempfile +from typing import TYPE_CHECKING + +from dimos.core.native_module import NativeModule, NativeModuleConfig + +if TYPE_CHECKING: + from dimos.core import Out + from dimos.msgs.nav_msgs.Odometry import Odometry + from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + +_DEFAULT_EXECUTABLE = str(Path(__file__).parent / "cpp" / "build" / "fastlio2_native") +_DEFAULT_CONFIG = str(Path(__file__).parent / "cpp" / "config" / "mid360.json") + + +@dataclass(kw_only=True) +class FastLio2Config(NativeModuleConfig): + """Config for the FAST-LIO2 + Livox Mid-360 native module.""" + + executable: str = _DEFAULT_EXECUTABLE + + # Livox SDK hardware config + host_ip: str = "192.168.1.5" + lidar_ip: str = "192.168.1.155" + frequency: float = 10.0 + + # Frame IDs for output messages + frame_id: str = "map" + child_frame_id: str = "body" + + # Output publish rates (Hz) + pointcloud_freq: float = 10.0 + odom_freq: float = 30.0 + + # FAST-LIO config (written to JSON, passed as --config_path) + scan_line: int = 1 + blind: float = 1.0 + fov_degree: int = 360 + det_range: float = 100.0 + acc_cov: float = 0.1 + gyr_cov: float = 0.1 + b_acc_cov: float = 0.0001 + b_gyr_cov: float = 0.0001 + extrinsic_est_en: bool = True + extrinsic_t: list[float] = field(default_factory=lambda: [0.04165, 0.02326, -0.0284]) + extrinsic_r: list[float] = field( + default_factory=lambda: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] + ) + + # SDK port configuration (for multi-sensor setups) + cmd_data_port: int = 56100 + push_msg_port: int = 56200 + point_data_port: int = 56300 + imu_data_port: int = 56400 + log_data_port: int = 56500 + host_cmd_data_port: int = 56101 + host_push_msg_port: int = 56201 + host_point_data_port: int = 56301 + host_imu_data_port: int = 56401 + host_log_data_port: int = 56501 + + +class FastLio2Module(NativeModule): + """FAST-LIO2 SLAM module with integrated Livox Mid-360 driver. + + Ports: + lidar (Out[PointCloud2]): World-frame registered point cloud. + odometry (Out[Odometry]): Pose with covariance at LiDAR scan rate. + """ + + default_config: type[FastLio2Config] = FastLio2Config # type: ignore[assignment] + lidar: Out[PointCloud2] + odometry: Out[Odometry] + + def _build_extra_args(self) -> list[str]: + """Pass hardware and SLAM config to the C++ binary as CLI args.""" + cfg: FastLio2Config = self.config # type: ignore[assignment] + config_path = self._write_fastlio_config(cfg) + return [ + "--config_path", + config_path, + "--host_ip", + cfg.host_ip, + "--lidar_ip", + cfg.lidar_ip, + "--frequency", + str(cfg.frequency), + "--frame_id", + cfg.frame_id, + "--child_frame_id", + cfg.child_frame_id, + "--pointcloud_freq", + str(cfg.pointcloud_freq), + "--odom_freq", + str(cfg.odom_freq), + "--cmd_data_port", + str(cfg.cmd_data_port), + "--push_msg_port", + str(cfg.push_msg_port), + "--point_data_port", + str(cfg.point_data_port), + "--imu_data_port", + str(cfg.imu_data_port), + "--log_data_port", + str(cfg.log_data_port), + "--host_cmd_data_port", + str(cfg.host_cmd_data_port), + "--host_push_msg_port", + str(cfg.host_push_msg_port), + "--host_point_data_port", + str(cfg.host_point_data_port), + "--host_imu_data_port", + str(cfg.host_imu_data_port), + "--host_log_data_port", + str(cfg.host_log_data_port), + ] + + @staticmethod + def _write_fastlio_config(cfg: FastLio2Config) -> str: + """Write FAST-LIO JSON config to a temp file, return path.""" + config = { + "common": { + "time_sync_en": False, + "time_offset_lidar_to_imu": 0.0, + "msr_freq": 50.0, + "main_freq": 5000.0, + }, + "preprocess": { + "lidar_type": 1, + "scan_line": cfg.scan_line, + "blind": cfg.blind, + }, + "mapping": { + "acc_cov": cfg.acc_cov, + "gyr_cov": cfg.gyr_cov, + "b_acc_cov": cfg.b_acc_cov, + "b_gyr_cov": cfg.b_gyr_cov, + "fov_degree": cfg.fov_degree, + "det_range": cfg.det_range, + "extrinsic_est_en": cfg.extrinsic_est_en, + "extrinsic_T": list(cfg.extrinsic_t), + "extrinsic_R": list(cfg.extrinsic_r), + }, + } + f = tempfile.NamedTemporaryFile(mode="w", suffix=".json", prefix="fastlio2_", delete=False) + json.dump(config, f, indent=2) + f.close() + return f.name + + +fastlio2_module = FastLio2Module.blueprint + +__all__ = [ + "FastLio2Config", + "FastLio2Module", + "fastlio2_module", +] diff --git a/dimos/hardware/sensors/lidar/fastlio2/test_spec_compliance.py b/dimos/hardware/sensors/lidar/fastlio2/test_spec_compliance.py new file mode 100644 index 0000000000..8707d7c0af --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/test_spec_compliance.py @@ -0,0 +1,29 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Spec compliance tests for the FAST-LIO2 module.""" + +from __future__ import annotations + +from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2Module + + +def test_fastlio2_has_lidar_port() -> None: + hints = FastLio2Module.__annotations__ + assert "lidar" in hints + + +def test_fastlio2_has_odometry_port() -> None: + hints = FastLio2Module.__annotations__ + assert "odometry" in hints diff --git a/dimos/hardware/sensors/lidar/test_spec_compliance.py b/dimos/hardware/sensors/lidar/test_spec_compliance.py new file mode 100644 index 0000000000..14d7a2e61e --- /dev/null +++ b/dimos/hardware/sensors/lidar/test_spec_compliance.py @@ -0,0 +1,34 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Spec compliance tests for LidarModule.""" + +from __future__ import annotations + +import typing + +from dimos.hardware.sensors.lidar.module import LidarModule +from dimos.spec import perception +from dimos.spec.utils import assert_implements_protocol + + +def test_lidar_module_implements_lidar_spec() -> None: + assert_implements_protocol(LidarModule, perception.Lidar) + + +def test_lidar_spec_does_not_require_imu() -> None: + """Not all LiDARs have IMU — Lidar spec should only require pointcloud.""" + hints = typing.get_type_hints(perception.Lidar, include_extras=True) + assert "pointcloud" in hints + assert "imu" not in hints diff --git a/dimos/msgs/nav_msgs/Odometry.py b/dimos/msgs/nav_msgs/Odometry.py index e6d0844f46..14d26455f8 100644 --- a/dimos/msgs/nav_msgs/Odometry.py +++ b/dimos/msgs/nav_msgs/Odometry.py @@ -17,6 +17,9 @@ import time from typing import TYPE_CHECKING, TypeAlias +if TYPE_CHECKING: + from rerun._baseclasses import Archetype + from dimos_lcm.nav_msgs import Odometry as LCMOdometry import numpy as np from plum import dispatch @@ -252,6 +255,22 @@ def __eq__(self, other) -> bool: # type: ignore[no-untyped-def] and self.twist == other.twist ) + def to_rerun(self) -> Archetype: + """Convert to rerun Transform3D for visualizing the pose.""" + import rerun as rr + + return rr.Transform3D( + translation=[self.x, self.y, self.z], + rotation=rr.Quaternion( + xyzw=[ + self.orientation.x, + self.orientation.y, + self.orientation.z, + self.orientation.w, + ] + ), + ) + def lcm_encode(self) -> bytes: """Encode to LCM binary format.""" lcm_msg = LCMOdometry() diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index d3a6a58993..7f3b2f07d3 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -40,6 +40,8 @@ "demo-osm": "dimos.mapping.osm.demo_osm:demo_osm", "demo-skill": "dimos.agents.skills.demo_skill:demo_skill", "dual-xarm6-planner": "dimos.manipulation.manipulation_blueprints:dual_xarm6_planner", + "mid360-fastlio": "dimos.hardware.sensors.lidar.fastlio2.fastlio_blueprints:mid360_fastlio", + "mid360-fastlio-mapper": "dimos.hardware.sensors.lidar.fastlio2.fastlio_blueprints:mid360_fastlio_mapper", "mid360cpp": "dimos.hardware.sensors.lidar.livox.livox_blueprints:mid360cpp", "unitree-g1": "dimos.robot.unitree_webrtc.unitree_g1_blueprints:unitree_g1", "unitree-g1-agentic": "dimos.robot.unitree_webrtc.unitree_g1_blueprints:unitree_g1_agentic", @@ -78,6 +80,7 @@ "depth_module": "dimos.robot.unitree_webrtc.depth_module", "detection3d_module": "dimos.perception.detection.module3D", "detection_db_module": "dimos.perception.detection.moduleDB", + "fastlio2_module": "dimos.hardware.sensors.lidar.fastlio2.module", "foxglove_bridge": "dimos.robot.foxglove_bridge", "g1_connection": "dimos.robot.unitree.connection.g1", "g1_sim_connection": "dimos.robot.unitree.connection.g1sim", @@ -90,11 +93,10 @@ "joint_trajectory_controller": "dimos.manipulation.control.trajectory_controller.joint_trajectory_controller", "keyboard_teleop": "dimos.robot.unitree_webrtc.keyboard_teleop", "lidar_module": "dimos.hardware.sensors.lidar.module", - "livox_lidar_module": "dimos.hardware.sensors.lidar.livox.module", "llm_agent": "dimos.agents.agent", "manipulation_module": "dimos.manipulation.manipulation_module", "mapper": "dimos.robot.unitree_webrtc.type.map", - "mid360_cpp_module": "dimos.hardware.sensors.lidar.livox.cpp.module", + "mid360_cpp_module": "dimos.hardware.sensors.lidar.livox.module", "navigation_skill": "dimos.agents.skills.navigation", "object_scene_registration_module": "dimos.perception.object_scene_registration", "object_tracking": "dimos.perception.object_tracker", diff --git a/flake.nix b/flake.nix index 50738effc0..ec3e919a90 100644 --- a/flake.nix +++ b/flake.nix @@ -129,6 +129,11 @@ { vals.pkg=pkgs.libjpeg_turbo; flags.ldLibraryGroup=true; } { vals.pkg=pkgs.libpng; flags={}; } + ### FAST-LIO2 native module deps + { vals.pkg=pkgs.pcl; flags.ldLibraryGroup=true; } + { vals.pkg=pkgs.nlohmann_json; flags={}; } + { vals.pkg=pkgs.llvmPackages.openmp; flags.ldLibraryGroup=true; } + ### Docs generators { vals.pkg=pkgs.pikchr; flags={}; } { vals.pkg=pkgs.graphviz; flags={}; } From a84cc090c3867fc380e6dca02ccb9df78497f712 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Thu, 12 Feb 2026 21:52:53 +0800 Subject: [PATCH 11/48] Fetch FAST-LIO-NON-ROS from GitHub instead of hardcoded local path --- .../sensors/lidar/fastlio2/cpp/CMakeLists.txt | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt b/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt index 301c257ddd..9ddf08bade 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt @@ -5,9 +5,6 @@ set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3") -# FAST-LIO-NON-ROS source directory (local reference) -set(FASTLIO_DIR "$ENV{HOME}/coding/FAST-LIO-NON-ROS") - # OpenMP for parallel processing find_package(OpenMP QUIET) if(OpenMP_CXX_FOUND) @@ -30,8 +27,22 @@ else() add_definitions(-DMP_PROC_NUM=1) endif() -# Fetch dimos-lcm for C++ message headers +# Fetch dependencies include(FetchContent) + +# FAST-LIO-NON-ROS +FetchContent_Declare(fastlio + GIT_REPOSITORY https://github.com/leshy/FAST-LIO-NON-ROS.git + GIT_TAG dimos-integration + GIT_SHALLOW TRUE +) +FetchContent_GetProperties(fastlio) +if(NOT fastlio_POPULATED) + FetchContent_Populate(fastlio) +endif() +set(FASTLIO_DIR ${fastlio_SOURCE_DIR}) + +# dimos-lcm C++ message headers FetchContent_Declare(dimos_lcm GIT_REPOSITORY https://github.com/dimensionalOS/dimos-lcm.git GIT_TAG main From cb8fcdc0ba5619ae0008082321821d638b987bf7 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Thu, 12 Feb 2026 23:02:22 +0800 Subject: [PATCH 12/48] ruff bugfix --- .../sensors/lidar/fastlio2/fastlio_blueprints.py | 2 +- dimos/hardware/sensors/lidar/fastlio2/module.py | 9 +++------ dimos/hardware/sensors/lidar/livox/livox_blueprints.py | 4 ++-- dimos/hardware/sensors/lidar/livox/module.py | 9 +++------ dimos/robot/all_blueprints.py | 4 ++-- 5 files changed, 11 insertions(+), 17 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py b/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py index 2dded02a32..7337a41ef4 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py +++ b/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py @@ -25,7 +25,7 @@ rerun_config = {} -mid360_fastlio_mapper = autoconnect( +mid360_fastlio_voxels = autoconnect( FastLio2Module.blueprint(), VoxelGridMapper.blueprint(publish_interval=0.5, voxel_size=0.25, carve_columns=False), rerun_bridge( diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py index 5d97ee2cdd..936ecf8e5c 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -34,14 +34,11 @@ import json from pathlib import Path import tempfile -from typing import TYPE_CHECKING +from dimos.core import Out # noqa: TC001 from dimos.core.native_module import NativeModule, NativeModuleConfig - -if TYPE_CHECKING: - from dimos.core import Out - from dimos.msgs.nav_msgs.Odometry import Odometry - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.nav_msgs.Odometry import Odometry # noqa: TC001 +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 # noqa: TC001 _DEFAULT_EXECUTABLE = str(Path(__file__).parent / "cpp" / "build" / "fastlio2_native") _DEFAULT_CONFIG = str(Path(__file__).parent / "cpp" / "config" / "mid360.json") diff --git a/dimos/hardware/sensors/lidar/livox/livox_blueprints.py b/dimos/hardware/sensors/lidar/livox/livox_blueprints.py index a4cbae37fb..29f404b23f 100644 --- a/dimos/hardware/sensors/lidar/livox/livox_blueprints.py +++ b/dimos/hardware/sensors/lidar/livox/livox_blueprints.py @@ -16,7 +16,7 @@ from dimos.hardware.sensors.lidar.livox.module import Mid360CppModule from dimos.visualization.rerun.bridge import rerun_bridge -mid360cpp = autoconnect( +mid360 = autoconnect( Mid360CppModule.blueprint(), rerun_bridge(), -).global_config(n_dask_workers=2, robot_model="mid360_livox_cpp") +).global_config(n_dask_workers=2, robot_model="mid360") diff --git a/dimos/hardware/sensors/lidar/livox/module.py b/dimos/hardware/sensors/lidar/livox/module.py index d6c4eb4744..3959810ef4 100644 --- a/dimos/hardware/sensors/lidar/livox/module.py +++ b/dimos/hardware/sensors/lidar/livox/module.py @@ -33,16 +33,13 @@ from dataclasses import dataclass from pathlib import Path -from typing import TYPE_CHECKING +from dimos.core import Out # noqa: TC001 from dimos.core.native_module import NativeModule, NativeModuleConfig +from dimos.msgs.sensor_msgs.Imu import Imu # noqa: TC001 +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 # noqa: TC001 from dimos.spec import perception -if TYPE_CHECKING: - from dimos.core import Out - from dimos.msgs.sensor_msgs.Imu import Imu - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 - _DEFAULT_EXECUTABLE = str(Path(__file__).parent / "cpp" / "build" / "mid360_native") diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 7f3b2f07d3..eabf588388 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -40,9 +40,9 @@ "demo-osm": "dimos.mapping.osm.demo_osm:demo_osm", "demo-skill": "dimos.agents.skills.demo_skill:demo_skill", "dual-xarm6-planner": "dimos.manipulation.manipulation_blueprints:dual_xarm6_planner", + "mid360": "dimos.hardware.sensors.lidar.livox.livox_blueprints:mid360", "mid360-fastlio": "dimos.hardware.sensors.lidar.fastlio2.fastlio_blueprints:mid360_fastlio", - "mid360-fastlio-mapper": "dimos.hardware.sensors.lidar.fastlio2.fastlio_blueprints:mid360_fastlio_mapper", - "mid360cpp": "dimos.hardware.sensors.lidar.livox.livox_blueprints:mid360cpp", + "mid360-fastlio-voxels": "dimos.hardware.sensors.lidar.fastlio2.fastlio_blueprints:mid360_fastlio_voxels", "unitree-g1": "dimos.robot.unitree_webrtc.unitree_g1_blueprints:unitree_g1", "unitree-g1-agentic": "dimos.robot.unitree_webrtc.unitree_g1_blueprints:unitree_g1_agentic", "unitree-g1-agentic-sim": "dimos.robot.unitree_webrtc.unitree_g1_blueprints:unitree_g1_agentic_sim", From d345e8276dad036d714a2a4476df12728cc96087 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Thu, 12 Feb 2026 23:19:20 +0800 Subject: [PATCH 13/48] nicer fastlio config --- .../sensors/lidar/fastlio2/fastlio_blueprints.py | 11 ++++++----- dimos/hardware/sensors/lidar/fastlio2/module.py | 6 +++--- dimos/visualization/rerun/bridge.py | 4 ++++ 3 files changed, 13 insertions(+), 8 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py b/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py index 7337a41ef4..f1a25c0d0b 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py +++ b/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py @@ -13,12 +13,12 @@ # limitations under the License. from dimos.core.blueprints import autoconnect -from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2Module +from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 from dimos.mapping.voxels import VoxelGridMapper from dimos.visualization.rerun.bridge import rerun_bridge mid360_fastlio = autoconnect( - FastLio2Module.blueprint(), + FastLio2.blueprint(), rerun_bridge(), ).global_config(n_dask_workers=2, robot_model="mid360_fastlio2") @@ -27,10 +27,11 @@ mid360_fastlio_voxels = autoconnect( FastLio2Module.blueprint(), - VoxelGridMapper.blueprint(publish_interval=0.5, voxel_size=0.25, carve_columns=False), + VoxelGridMapper.blueprint(publish_interval=0.5, voxel_size=0.1, carve_columns=False), rerun_bridge( visual_override={ - "world/global_map": lambda grid: grid.to_rerun(voxel_size=0.25, mode="boxes"), + "world/global_map": lambda grid: grid.to_rerun(voxel_size=0.1, mode="boxes"), + "world/lidar": None, } ), -).global_config(n_dask_workers=2, robot_model="mid360_fastlio2_voxels") +).global_config(n_dask_workers=3, robot_model="mid360_fastlio2_voxels") diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py index 936ecf8e5c..fa2a5c4e11 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -91,7 +91,7 @@ class FastLio2Config(NativeModuleConfig): host_log_data_port: int = 56501 -class FastLio2Module(NativeModule): +class FastLio2(NativeModule): """FAST-LIO2 SLAM module with integrated Livox Mid-360 driver. Ports: @@ -179,10 +179,10 @@ def _write_fastlio_config(cfg: FastLio2Config) -> str: return f.name -fastlio2_module = FastLio2Module.blueprint +fastlio2_module = FastLio2.blueprint __all__ = [ + "FastLio2", "FastLio2Config", - "FastLio2Module", "fastlio2_module", ] diff --git a/dimos/visualization/rerun/bridge.py b/dimos/visualization/rerun/bridge.py index 4e39f0b64c..c236b5c543 100644 --- a/dimos/visualization/rerun/bridge.py +++ b/dimos/visualization/rerun/bridge.py @@ -202,6 +202,10 @@ def _visual_override_for_entity_path( if pattern_matches(pattern, entity_path) ] + # None means "suppress this topic entirely" + if any(fn is None for fn in matches): + return lambda msg: None + # final step (ensures we return Archetype or None) def final_convert(msg: Any) -> RerunData | None: if isinstance(msg, Archetype): From bcda438aa52eefd870dddf88f6d559887c99df73 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Thu, 12 Feb 2026 23:42:42 +0800 Subject: [PATCH 14/48] correct mid360 config --- .../sensors/lidar/fastlio2/cpp/CMakeLists.txt | 23 ++++++++++--------- .../lidar/fastlio2/fastlio_blueprints.py | 2 +- .../hardware/sensors/lidar/fastlio2/module.py | 6 ++--- 3 files changed, 16 insertions(+), 15 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt b/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt index 9ddf08bade..b10847b9a6 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt @@ -30,17 +30,18 @@ endif() # Fetch dependencies include(FetchContent) -# FAST-LIO-NON-ROS -FetchContent_Declare(fastlio - GIT_REPOSITORY https://github.com/leshy/FAST-LIO-NON-ROS.git - GIT_TAG dimos-integration - GIT_SHALLOW TRUE -) -FetchContent_GetProperties(fastlio) -if(NOT fastlio_POPULATED) - FetchContent_Populate(fastlio) -endif() -set(FASTLIO_DIR ${fastlio_SOURCE_DIR}) +# FAST-LIO-NON-ROS (local checkout for development) +set(FASTLIO_DIR $ENV{HOME}/coding/FAST-LIO-NON-ROS) +# FetchContent_Declare(fastlio +# GIT_REPOSITORY https://github.com/leshy/FAST-LIO-NON-ROS.git +# GIT_TAG dimos-integration +# GIT_SHALLOW TRUE +# ) +# FetchContent_GetProperties(fastlio) +# if(NOT fastlio_POPULATED) +# FetchContent_Populate(fastlio) +# endif() +# set(FASTLIO_DIR ${fastlio_SOURCE_DIR}) # dimos-lcm C++ message headers FetchContent_Declare(dimos_lcm diff --git a/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py b/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py index f1a25c0d0b..f22af3b4b0 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py +++ b/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py @@ -26,7 +26,7 @@ rerun_config = {} mid360_fastlio_voxels = autoconnect( - FastLio2Module.blueprint(), + FastLio2.blueprint(), VoxelGridMapper.blueprint(publish_interval=0.5, voxel_size=0.1, carve_columns=False), rerun_bridge( visual_override={ diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py index fa2a5c4e11..95b8c6ef29 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -64,8 +64,8 @@ class FastLio2Config(NativeModuleConfig): odom_freq: float = 30.0 # FAST-LIO config (written to JSON, passed as --config_path) - scan_line: int = 1 - blind: float = 1.0 + scan_line: int = 4 + blind: float = 0.5 fov_degree: int = 360 det_range: float = 100.0 acc_cov: float = 0.1 @@ -73,7 +73,7 @@ class FastLio2Config(NativeModuleConfig): b_acc_cov: float = 0.0001 b_gyr_cov: float = 0.0001 extrinsic_est_en: bool = True - extrinsic_t: list[float] = field(default_factory=lambda: [0.04165, 0.02326, -0.0284]) + extrinsic_t: list[float] = field(default_factory=lambda: [-0.011, -0.02329, 0.04412]) extrinsic_r: list[float] = field( default_factory=lambda: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] ) From 28c80ac679a5f4893da4e37b716878b2b6edac82 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Fri, 13 Feb 2026 00:01:17 +0800 Subject: [PATCH 15/48] fastlio filtering, tuning --- .../lidar/fastlio2/cpp/cloud_filter.hpp | 51 +++++++++++++++++++ .../sensors/lidar/fastlio2/cpp/main.cpp | 10 +++- .../lidar/fastlio2/fastlio_blueprints.py | 14 +++-- .../hardware/sensors/lidar/fastlio2/module.py | 11 ++++ 4 files changed, 81 insertions(+), 5 deletions(-) create mode 100644 dimos/hardware/sensors/lidar/fastlio2/cpp/cloud_filter.hpp diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/cloud_filter.hpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/cloud_filter.hpp new file mode 100644 index 0000000000..352ba9bef5 --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/cloud_filter.hpp @@ -0,0 +1,51 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 +// +// Point cloud filtering utilities: voxel grid downsampling and +// statistical outlier removal using PCL. + +#ifndef CLOUD_FILTER_HPP_ +#define CLOUD_FILTER_HPP_ + +#include +#include +#include +#include + +struct CloudFilterConfig { + float voxel_size = 0.1f; + int sor_mean_k = 50; + float sor_stddev = 1.0f; +}; + +/// Apply voxel grid downsample + statistical outlier removal in-place. +/// Returns the filtered cloud (new allocation). +template +typename pcl::PointCloud::Ptr filter_cloud( + const typename pcl::PointCloud::Ptr& input, + const CloudFilterConfig& cfg) { + + if (!input || input->empty()) return input; + + // Voxel grid downsample + typename pcl::PointCloud::Ptr voxelized(new pcl::PointCloud()); + pcl::VoxelGrid vg; + vg.setInputCloud(input); + vg.setLeafSize(cfg.voxel_size, cfg.voxel_size, cfg.voxel_size); + vg.filter(*voxelized); + + // Statistical outlier removal + if (cfg.sor_mean_k > 0 && voxelized->size() > static_cast(cfg.sor_mean_k)) { + typename pcl::PointCloud::Ptr cleaned(new pcl::PointCloud()); + pcl::StatisticalOutlierRemoval sor; + sor.setInputCloud(voxelized); + sor.setMeanK(cfg.sor_mean_k); + sor.setStddevMulThresh(cfg.sor_stddev); + sor.filter(*cleaned); + return cleaned; + } + + return voxelized; +} + +#endif diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp index d931ba27b4..01ce74bc86 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp @@ -30,6 +30,7 @@ #include #include +#include "cloud_filter.hpp" #include "dimos_native_module.hpp" // dimos LCM message headers @@ -396,6 +397,10 @@ int main(int argc, char** argv) { g_child_frame_id = mod.arg("child_frame_id", "body"); float pointcloud_freq = mod.arg_float("pointcloud_freq", 5.0f); float odom_freq = mod.arg_float("odom_freq", 50.0f); + CloudFilterConfig filter_cfg; + filter_cfg.voxel_size = mod.arg_float("voxel_size", 0.1f); + filter_cfg.sor_mean_k = mod.arg_int("sor_mean_k", 50); + filter_cfg.sor_stddev = mod.arg_float("sor_stddev", 1.0f); // SDK network ports SdkPorts ports; @@ -420,6 +425,8 @@ int main(int argc, char** argv) { host_ip.c_str(), lidar_ip.c_str(), g_frequency); printf("[fastlio2] pointcloud_freq: %.1f Hz odom_freq: %.1f Hz\n", pointcloud_freq, odom_freq); + printf("[fastlio2] voxel_size: %.3f sor_mean_k: %d sor_stddev: %.1f\n", + filter_cfg.voxel_size, filter_cfg.sor_mean_k, filter_cfg.sor_stddev); // Signal handlers signal(SIGTERM, signal_handler); @@ -541,7 +548,8 @@ int main(int argc, char** argv) { // Publish aggregated cloud at pointcloud_freq if (now - last_pc_publish >= pc_interval && !accumulated_scan->empty()) { - publish_lidar(accumulated_scan, ts); + auto filtered = filter_cloud(accumulated_scan, filter_cfg); + publish_lidar(filtered, ts); accumulated_scan->clear(); last_pc_publish = now; } diff --git a/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py b/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py index f22af3b4b0..0ed3cddd05 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py +++ b/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py @@ -17,9 +17,15 @@ from dimos.mapping.voxels import VoxelGridMapper from dimos.visualization.rerun.bridge import rerun_bridge +voxel_size = 0.05 + mid360_fastlio = autoconnect( - FastLio2.blueprint(), - rerun_bridge(), + FastLio2.blueprint(voxel_size=voxel_size), + rerun_bridge( + visual_override={ + "world/lidar": lambda grid: grid.to_rerun(voxel_size=voxel_size, mode="boxes"), + } + ), ).global_config(n_dask_workers=2, robot_model="mid360_fastlio2") @@ -27,10 +33,10 @@ mid360_fastlio_voxels = autoconnect( FastLio2.blueprint(), - VoxelGridMapper.blueprint(publish_interval=0.5, voxel_size=0.1, carve_columns=False), + VoxelGridMapper.blueprint(publish_interval=1.0, voxel_size=voxel_size, carve_columns=False), rerun_bridge( visual_override={ - "world/global_map": lambda grid: grid.to_rerun(voxel_size=0.1, mode="boxes"), + "world/global_map": lambda grid: grid.to_rerun(voxel_size=voxel_size, mode="boxes"), "world/lidar": None, } ), diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py index 95b8c6ef29..1440a03ccc 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -63,6 +63,11 @@ class FastLio2Config(NativeModuleConfig): pointcloud_freq: float = 10.0 odom_freq: float = 30.0 + # Point cloud filtering + voxel_size: float = 0.1 + sor_mean_k: int = 50 + sor_stddev: float = 1.0 + # FAST-LIO config (written to JSON, passed as --config_path) scan_line: int = 4 blind: float = 0.5 @@ -124,6 +129,12 @@ def _build_extra_args(self) -> list[str]: str(cfg.pointcloud_freq), "--odom_freq", str(cfg.odom_freq), + "--voxel_size", + str(cfg.voxel_size), + "--sor_mean_k", + str(cfg.sor_mean_k), + "--sor_stddev", + str(cfg.sor_stddev), "--cmd_data_port", str(cfg.cmd_data_port), "--push_msg_port", From e1f06ada4c9f3a9a4d67495ae6243b67fcc9b2c8 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Fri, 13 Feb 2026 00:25:05 +0800 Subject: [PATCH 16/48] cpp mapper --- dimos/core/native_module.py | 40 ++++++++++- .../sensors/lidar/fastlio2/cpp/main.cpp | 60 ++++++++++++---- .../lidar/fastlio2/fastlio_blueprints.py | 12 +++- .../hardware/sensors/lidar/fastlio2/module.py | 69 +++++++------------ dimos/hardware/sensors/lidar/livox/module.py | 34 +-------- dimos/robot/all_blueprints.py | 1 + 6 files changed, 121 insertions(+), 95 deletions(-) diff --git a/dimos/core/native_module.py b/dimos/core/native_module.py index 721bb5399e..833f95a7a2 100644 --- a/dimos/core/native_module.py +++ b/dimos/core/native_module.py @@ -40,7 +40,7 @@ class MyCppModule(NativeModule): from __future__ import annotations -from dataclasses import dataclass, field +from dataclasses import dataclass, field, fields import enum import json import os @@ -72,6 +72,44 @@ class NativeModuleConfig(ModuleConfig): shutdown_timeout: float = 10.0 log_format: LogFormat = LogFormat.TEXT + # Field names from base classes that should not be converted to CLI args + _BASE_FIELDS: frozenset[str] = field(default=frozenset(), init=False, repr=False, compare=False) + + def __post_init__(self): + # Collect all field names from NativeModuleConfig and its parents + object.__setattr__( + self, + "_BASE_FIELDS", + frozenset(f.name for f in fields(NativeModuleConfig)), + ) + + # Override in subclasses to exclude fields from CLI arg generation + cli_exclude: frozenset[str] = frozenset() + + def to_cli_args(self) -> list[str]: + """Auto-convert subclass config fields to CLI args. + + Iterates fields defined on the concrete subclass (not NativeModuleConfig + or its parents) and converts them to ``["--name", str(value)]`` pairs. + Skips fields whose values are ``None`` and fields in ``cli_exclude``. + """ + args: list[str] = [] + for f in fields(self): + if f.name in self._BASE_FIELDS or f.name.startswith("_"): + continue + if f.name in self.cli_exclude: + continue + val = getattr(self, f.name) + if val is None: + continue + if isinstance(val, bool): + args.extend([f"--{f.name}", str(val).lower()]) + elif isinstance(val, list): + args.extend([f"--{f.name}", ",".join(str(v) for v in val)]) + else: + args.extend([f"--{f.name}", str(val)]) + return args + class NativeModule(Module): """Module that wraps a native executable as a managed subprocess. diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp index 01ce74bc86..07a45ea5ca 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp @@ -32,6 +32,7 @@ #include "cloud_filter.hpp" #include "dimos_native_module.hpp" +#include "voxel_map.hpp" // dimos LCM message headers #include "geometry_msgs/Quaternion.hpp" @@ -64,6 +65,7 @@ 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; @@ -107,8 +109,10 @@ static std_msgs::Header make_header(const std::string& frame_id, double ts) { // Publish lidar (world-frame point cloud) // --------------------------------------------------------------------------- -static void publish_lidar(PointCloudXYZI::Ptr cloud, double timestamp) { - if (!g_lcm || !cloud || cloud->empty()) return; +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()); @@ -151,7 +155,7 @@ static void publish_lidar(PointCloudXYZI::Ptr cloud, double timestamp) { dst[3] = cloud->points[i].intensity; } - g_lcm->publish(g_lidar_topic, &pc); + g_lcm->publish(chan, &pc); } // --------------------------------------------------------------------------- @@ -376,6 +380,7 @@ int main(int argc, char** 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"); @@ -401,6 +406,9 @@ int main(int argc, char** argv) { filter_cfg.voxel_size = mod.arg_float("voxel_size", 0.1f); filter_cfg.sor_mean_k = mod.arg_int("sor_mean_k", 50); filter_cfg.sor_stddev = mod.arg_float("sor_stddev", 1.0f); + float map_voxel_size = mod.arg_float("map_voxel_size", 0.1f); + float map_max_range = mod.arg_float("map_max_range", 100.0f); + float map_freq = mod.arg_float("map_freq", 0.0f); // SDK network ports SdkPorts ports; @@ -420,6 +428,8 @@ int main(int argc, char** argv) { 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); @@ -427,6 +437,9 @@ int main(int argc, char** argv) { 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); @@ -489,8 +502,15 @@ int main(int argc, char** argv) { auto last_pc_publish = std::chrono::steady_clock::now(); auto last_odom_publish = std::chrono::steady_clock::now(); - // Accumulate world-frame scans between pointcloud publishes - PointCloudXYZI::Ptr accumulated_scan(new PointCloudXYZI()); + // 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(); @@ -539,20 +559,30 @@ int main(int argc, char** argv) { double ts = std::chrono::duration( std::chrono::system_clock::now().time_since_epoch()).count(); - // Accumulate registered scans between publishes - if (!g_lidar_topic.empty()) { - auto world_cloud = fast_lio.get_world_cloud(); - if (world_cloud && !world_cloud->empty()) { - *accumulated_scan += *world_cloud; - } + auto world_cloud = fast_lio.get_world_cloud(); + if (world_cloud && !world_cloud->empty()) { + auto filtered = filter_cloud(world_cloud, filter_cfg); - // Publish aggregated cloud at pointcloud_freq - if (now - last_pc_publish >= pc_interval && !accumulated_scan->empty()) { - auto filtered = filter_cloud(accumulated_scan, filter_cfg); + // Per-scan publish at pointcloud_freq + if (!g_lidar_topic.empty() && now - last_pc_publish >= pc_interval) { publish_lidar(filtered, ts); - accumulated_scan->clear(); 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) diff --git a/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py b/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py index 0ed3cddd05..4928878f62 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py +++ b/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py @@ -20,7 +20,7 @@ voxel_size = 0.05 mid360_fastlio = autoconnect( - FastLio2.blueprint(voxel_size=voxel_size), + 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"), @@ -28,6 +28,16 @@ ), ).global_config(n_dask_workers=2, robot_model="mid360_fastlio2") +mid360_fastlio_voxels_native = autoconnect( + FastLio2.blueprint(voxel_size=voxel_size, map_voxel_size=voxel_size, map_freq=3.0), + rerun_bridge( + visual_override={ + "world/lidar": None, + "world/global_map": lambda grid: grid.to_rerun(voxel_size=voxel_size, mode="boxes"), + } + ), +).global_config(n_dask_workers=2, robot_model="mid360_fastlio2") + rerun_config = {} diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py index 1440a03ccc..d2fb8fd5ae 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -68,7 +68,27 @@ class FastLio2Config(NativeModuleConfig): sor_mean_k: int = 50 sor_stddev: float = 1.0 - # FAST-LIO config (written to JSON, passed as --config_path) + # Global voxel map (disabled when map_freq <= 0) + map_freq: float = 0.0 + map_voxel_size: float = 0.1 + map_max_range: float = 100.0 + + # FAST-LIO config (written to JSON, passed as --config_path — excluded from CLI) + cli_exclude: frozenset[str] = frozenset( + { + "scan_line", + "blind", + "fov_degree", + "det_range", + "acc_cov", + "gyr_cov", + "b_acc_cov", + "b_gyr_cov", + "extrinsic_est_en", + "extrinsic_t", + "extrinsic_r", + } + ) scan_line: int = 4 blind: float = 0.5 fov_degree: int = 360 @@ -102,60 +122,19 @@ class FastLio2(NativeModule): Ports: lidar (Out[PointCloud2]): World-frame registered point cloud. odometry (Out[Odometry]): Pose with covariance at LiDAR scan rate. + global_map (Out[PointCloud2]): Global voxel map (optional, enable via map_freq > 0). """ default_config: type[FastLio2Config] = FastLio2Config # type: ignore[assignment] lidar: Out[PointCloud2] odometry: Out[Odometry] + global_map: Out[PointCloud2] def _build_extra_args(self) -> list[str]: """Pass hardware and SLAM config to the C++ binary as CLI args.""" cfg: FastLio2Config = self.config # type: ignore[assignment] config_path = self._write_fastlio_config(cfg) - return [ - "--config_path", - config_path, - "--host_ip", - cfg.host_ip, - "--lidar_ip", - cfg.lidar_ip, - "--frequency", - str(cfg.frequency), - "--frame_id", - cfg.frame_id, - "--child_frame_id", - cfg.child_frame_id, - "--pointcloud_freq", - str(cfg.pointcloud_freq), - "--odom_freq", - str(cfg.odom_freq), - "--voxel_size", - str(cfg.voxel_size), - "--sor_mean_k", - str(cfg.sor_mean_k), - "--sor_stddev", - str(cfg.sor_stddev), - "--cmd_data_port", - str(cfg.cmd_data_port), - "--push_msg_port", - str(cfg.push_msg_port), - "--point_data_port", - str(cfg.point_data_port), - "--imu_data_port", - str(cfg.imu_data_port), - "--log_data_port", - str(cfg.log_data_port), - "--host_cmd_data_port", - str(cfg.host_cmd_data_port), - "--host_push_msg_port", - str(cfg.host_push_msg_port), - "--host_point_data_port", - str(cfg.host_point_data_port), - "--host_imu_data_port", - str(cfg.host_imu_data_port), - "--host_log_data_port", - str(cfg.host_log_data_port), - ] + return ["--config_path", config_path, *cfg.to_cli_args()] @staticmethod def _write_fastlio_config(cfg: FastLio2Config) -> str: diff --git a/dimos/hardware/sensors/lidar/livox/module.py b/dimos/hardware/sensors/lidar/livox/module.py index 3959810ef4..d5fa365b81 100644 --- a/dimos/hardware/sensors/lidar/livox/module.py +++ b/dimos/hardware/sensors/lidar/livox/module.py @@ -84,39 +84,7 @@ class Mid360CppModule(NativeModule, perception.Lidar, perception.IMU): def _build_extra_args(self) -> list[str]: """Pass hardware config to the C++ binary as CLI args.""" - cfg: Mid360CppConfig = self.config # type: ignore[assignment] - return [ - "--host_ip", - cfg.host_ip, - "--lidar_ip", - cfg.lidar_ip, - "--frequency", - str(cfg.frequency), - "--frame_id", - cfg.frame_id, - "--imu_frame_id", - cfg.imu_frame_id, - "--cmd_data_port", - str(cfg.cmd_data_port), - "--push_msg_port", - str(cfg.push_msg_port), - "--point_data_port", - str(cfg.point_data_port), - "--imu_data_port", - str(cfg.imu_data_port), - "--log_data_port", - str(cfg.log_data_port), - "--host_cmd_data_port", - str(cfg.host_cmd_data_port), - "--host_push_msg_port", - str(cfg.host_push_msg_port), - "--host_point_data_port", - str(cfg.host_point_data_port), - "--host_imu_data_port", - str(cfg.host_imu_data_port), - "--host_log_data_port", - str(cfg.host_log_data_port), - ] + return self.config.to_cli_args() mid360_cpp_module = Mid360CppModule.blueprint diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index eabf588388..75e08d8dd4 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -43,6 +43,7 @@ "mid360": "dimos.hardware.sensors.lidar.livox.livox_blueprints:mid360", "mid360-fastlio": "dimos.hardware.sensors.lidar.fastlio2.fastlio_blueprints:mid360_fastlio", "mid360-fastlio-voxels": "dimos.hardware.sensors.lidar.fastlio2.fastlio_blueprints:mid360_fastlio_voxels", + "mid360-fastlio-voxels-native": "dimos.hardware.sensors.lidar.fastlio2.fastlio_blueprints:mid360_fastlio_voxels_native", "unitree-g1": "dimos.robot.unitree_webrtc.unitree_g1_blueprints:unitree_g1", "unitree-g1-agentic": "dimos.robot.unitree_webrtc.unitree_g1_blueprints:unitree_g1_agentic", "unitree-g1-agentic-sim": "dimos.robot.unitree_webrtc.unitree_g1_blueprints:unitree_g1_agentic_sim", From cf6725df0162c307fe3a892d792efa6439abafbe Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Fri, 13 Feb 2026 10:38:57 +0800 Subject: [PATCH 17/48] renamed mid360 module --- .../lidar/fastlio2/fastlio_blueprints.py | 3 --- .../sensors/lidar/livox/cpp/README.md | 8 ++++---- .../sensors/lidar/livox/livox_blueprints.py | 4 ++-- dimos/hardware/sensors/lidar/livox/module.py | 20 +++++++++---------- .../lidar/livox/test_spec_compliance.py | 6 +++--- dimos/robot/all_blueprints.py | 2 +- 6 files changed, 20 insertions(+), 23 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py b/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py index 4928878f62..ced2b74374 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py +++ b/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py @@ -38,9 +38,6 @@ ), ).global_config(n_dask_workers=2, robot_model="mid360_fastlio2") - -rerun_config = {} - mid360_fastlio_voxels = autoconnect( FastLio2.blueprint(), VoxelGridMapper.blueprint(publish_interval=1.0, voxel_size=voxel_size, carve_columns=False), diff --git a/dimos/hardware/sensors/lidar/livox/cpp/README.md b/dimos/hardware/sensors/lidar/livox/cpp/README.md index 5e78e48f86..c4f8293008 100644 --- a/dimos/hardware/sensors/lidar/livox/cpp/README.md +++ b/dimos/hardware/sensors/lidar/livox/cpp/README.md @@ -35,16 +35,16 @@ for the C++ message headers on first configure. ## Usage -Normally launched by `Mid360CppModule` via the NativeModule framework — you +Normally launched by `Mid360Module` via the NativeModule framework — you don't run it directly. The Python wrapper passes LCM topic strings and config as CLI args: ```python -from dimos.hardware.sensors.lidar.livox.module import Mid360CppModule +from dimos.hardware.sensors.lidar.livox.module import Mid360Module from dimos.core.blueprints import autoconnect autoconnect( - Mid360CppModule.blueprint(host_ip="192.168.1.5"), + Mid360Module.blueprint(host_ip="192.168.1.5"), SomeConsumer.blueprint(), ).build().loop() ``` @@ -69,5 +69,5 @@ name used by dimos subscribers. |---|---| | `main.cpp` | Livox SDK2 callbacks, frame accumulation, LCM publishing | | `dimos_native_module.hpp` | Reusable header for parsing NativeModule CLI args | -| `../module.py` | Python NativeModule wrapper (`Mid360CppModule`) | +| `../module.py` | Python NativeModule wrapper (`Mid360Module`) | | `CMakeLists.txt` | Build config, fetches dimos-lcm headers automatically | diff --git a/dimos/hardware/sensors/lidar/livox/livox_blueprints.py b/dimos/hardware/sensors/lidar/livox/livox_blueprints.py index 29f404b23f..b95d9fccd5 100644 --- a/dimos/hardware/sensors/lidar/livox/livox_blueprints.py +++ b/dimos/hardware/sensors/lidar/livox/livox_blueprints.py @@ -13,10 +13,10 @@ # limitations under the License. from dimos.core.blueprints import autoconnect -from dimos.hardware.sensors.lidar.livox.module import Mid360CppModule +from dimos.hardware.sensors.lidar.livox.module import Mid360Module from dimos.visualization.rerun.bridge import rerun_bridge mid360 = autoconnect( - Mid360CppModule.blueprint(), + Mid360Module.blueprint(), rerun_bridge(), ).global_config(n_dask_workers=2, robot_model="mid360") diff --git a/dimos/hardware/sensors/lidar/livox/module.py b/dimos/hardware/sensors/lidar/livox/module.py index d5fa365b81..cbf266eb36 100644 --- a/dimos/hardware/sensors/lidar/livox/module.py +++ b/dimos/hardware/sensors/lidar/livox/module.py @@ -20,11 +20,11 @@ Usage:: - from dimos.hardware.sensors.lidar.livox.module import Mid360CppModule + from dimos.hardware.sensors.lidar.livox.module import Mid360Module from dimos.core.blueprints import autoconnect autoconnect( - Mid360CppModule.blueprint(host_ip="192.168.1.5"), + Mid360Module.blueprint(host_ip="192.168.1.5"), SomeConsumer.blueprint(), ).build().loop() """ @@ -44,7 +44,7 @@ @dataclass(kw_only=True) -class Mid360CppConfig(NativeModuleConfig): +class Mid360Config(NativeModuleConfig): """Config for the C++ Mid-360 native module.""" executable: str = _DEFAULT_EXECUTABLE @@ -68,7 +68,7 @@ class Mid360CppConfig(NativeModuleConfig): host_log_data_port: int = 56501 -class Mid360CppModule(NativeModule, perception.Lidar, perception.IMU): +class Mid360Module(NativeModule, perception.Lidar, perception.IMU): """Livox Mid-360 LiDAR module backed by a native C++ binary. Ports: @@ -76,8 +76,8 @@ class Mid360CppModule(NativeModule, perception.Lidar, perception.IMU): imu (Out[Imu]): IMU data at ~200 Hz (if enabled). """ - config: Mid360CppConfig - default_config = Mid360CppConfig + config: Mid360Config + default_config = Mid360Config pointcloud: Out[PointCloud2] imu: Out[Imu] @@ -87,10 +87,10 @@ def _build_extra_args(self) -> list[str]: return self.config.to_cli_args() -mid360_cpp_module = Mid360CppModule.blueprint +mid360_module = Mid360Module.blueprint __all__ = [ - "Mid360CppConfig", - "Mid360CppModule", - "mid360_cpp_module", + "Mid360Config", + "Mid360Module", + "mid360_module", ] diff --git a/dimos/hardware/sensors/lidar/livox/test_spec_compliance.py b/dimos/hardware/sensors/lidar/livox/test_spec_compliance.py index 92bf0e7c6e..5f78c0922a 100644 --- a/dimos/hardware/sensors/lidar/livox/test_spec_compliance.py +++ b/dimos/hardware/sensors/lidar/livox/test_spec_compliance.py @@ -16,14 +16,14 @@ from __future__ import annotations -from dimos.hardware.sensors.lidar.livox.module import Mid360CppModule +from dimos.hardware.sensors.lidar.livox.module import Mid360Module from dimos.spec import perception from dimos.spec.utils import assert_implements_protocol def test_livox_module_implements_lidar_spec() -> None: - assert_implements_protocol(Mid360CppModule, perception.Lidar) + assert_implements_protocol(Mid360Module, perception.Lidar) def test_livox_module_implements_imu_spec() -> None: - assert_implements_protocol(Mid360CppModule, perception.IMU) + assert_implements_protocol(Mid360Module, perception.IMU) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 75e08d8dd4..e27d9ea3a7 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -97,7 +97,7 @@ "llm_agent": "dimos.agents.agent", "manipulation_module": "dimos.manipulation.manipulation_module", "mapper": "dimos.robot.unitree_webrtc.type.map", - "mid360_cpp_module": "dimos.hardware.sensors.lidar.livox.module", + "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", From a07d8396fe0224e81278d4ca84dc710a532750af Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Fri, 13 Feb 2026 17:52:52 +0800 Subject: [PATCH 18/48] nix build for livox SDK --- dimos/core/native_echo.py | 10 ++ dimos/core/native_module.py | 54 ++++++----- dimos/core/test_native_module.py | 25 +++-- .../sensors/lidar/fastlio2/cpp/CMakeLists.txt | 21 ++-- .../sensors/lidar/fastlio2/cpp/main.cpp | 10 +- .../hardware/sensors/lidar/fastlio2/module.py | 97 +++++-------------- .../sensors/lidar/livox/cpp/CMakeLists.txt | 11 ++- dimos/hardware/sensors/lidar/livox/flake.lock | 61 ++++++++++++ dimos/hardware/sensors/lidar/livox/flake.nix | 53 ++++++++++ dimos/hardware/sensors/lidar/livox/module.py | 4 - flake.lock | 20 ++++ flake.nix | 11 ++- 12 files changed, 250 insertions(+), 127 deletions(-) create mode 100644 dimos/hardware/sensors/lidar/livox/flake.lock create mode 100644 dimos/hardware/sensors/lidar/livox/flake.nix diff --git a/dimos/core/native_echo.py b/dimos/core/native_echo.py index 0749ec4703..982d269e3c 100755 --- a/dimos/core/native_echo.py +++ b/dimos/core/native_echo.py @@ -16,12 +16,17 @@ """Echo binary for NativeModule tests. Dumps CLI args as a JSON log line to stdout, then waits for SIGTERM. + +Env vars: + NATIVE_ECHO_OUTPUT: path to write CLI args as JSON + NATIVE_ECHO_DIE_AFTER: seconds to wait before exiting with code 42 """ import json import os import signal import sys +import time signal.signal(signal.SIGTERM, lambda *_: sys.exit(0)) @@ -34,4 +39,9 @@ sys.stdout.write("\n") sys.stdout.flush() +die_after = os.environ.get("NATIVE_ECHO_DIE_AFTER") +if die_after: + time.sleep(float(die_after)) + sys.exit(42) + signal.pause() diff --git a/dimos/core/native_module.py b/dimos/core/native_module.py index 833f95a7a2..5bb3791756 100644 --- a/dimos/core/native_module.py +++ b/dimos/core/native_module.py @@ -44,6 +44,7 @@ class MyCppModule(NativeModule): import enum import json import os +from pathlib import Path import signal import subprocess import threading @@ -72,17 +73,6 @@ class NativeModuleConfig(ModuleConfig): shutdown_timeout: float = 10.0 log_format: LogFormat = LogFormat.TEXT - # Field names from base classes that should not be converted to CLI args - _BASE_FIELDS: frozenset[str] = field(default=frozenset(), init=False, repr=False, compare=False) - - def __post_init__(self): - # Collect all field names from NativeModuleConfig and its parents - object.__setattr__( - self, - "_BASE_FIELDS", - frozenset(f.name for f in fields(NativeModuleConfig)), - ) - # Override in subclasses to exclude fields from CLI arg generation cli_exclude: frozenset[str] = frozenset() @@ -93,9 +83,10 @@ def to_cli_args(self) -> list[str]: 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 self._BASE_FIELDS or f.name.startswith("_"): + if f.name in ignore_fields: continue if f.name in self.cli_exclude: continue @@ -128,6 +119,8 @@ class NativeModule(Module): default_config: type[NativeModuleConfig] = NativeModuleConfig # type: ignore[assignment] _process: subprocess.Popen[bytes] | None = None _io_threads: list[threading.Thread] + _watchdog: threading.Thread | None = None + _stopping: bool = False def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) @@ -140,16 +133,15 @@ def start(self) -> None: return topics = self._collect_topics() - extra = self._build_extra_args() cmd = [self.config.executable] for name, topic_str in topics.items(): cmd.extend([f"--{name}", topic_str]) - cmd.extend(extra) + cmd.extend(self.config.to_cli_args()) cmd.extend(self.config.extra_args) env = {**os.environ, **self.config.extra_env} - cwd = self.config.cwd + 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( @@ -161,13 +153,17 @@ def start(self) -> None: ) logger.info("Native process started", pid=self._process.pid) + self._stopping = False self._io_threads = [ self._start_reader(self._process.stdout, "info"), self._start_reader(self._process.stderr, "warning"), ] + 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) @@ -181,6 +177,9 @@ def stop(self) -> None: self._process.wait(timeout=5) for t in self._io_threads: t.join(timeout=2) + if self._watchdog is not None and self._watchdog is not threading.current_thread(): + self._watchdog.join(timeout=2) + self._watchdog = None self._io_threads = [] self._process = None super().stop() @@ -191,6 +190,20 @@ def _start_reader(self, stream: IO[bytes] | None, level: str) -> threading.Threa t.start() return t + def _watch_process(self) -> None: + """Block until the native process exits; trigger stop() if it crashed.""" + if self._process is None: + return + rc = self._process.wait() + if self._stopping: + return + logger.error( + "Native process died unexpectedly", + pid=self._process.pid, + returncode=rc, + ) + self.stop() + def _read_log_stream(self, stream: IO[bytes] | None, level: str) -> None: if stream is None: return @@ -206,8 +219,7 @@ def _read_log_stream(self, stream: IO[bytes] | None, level: str) -> None: log_fn(event, **data) continue except (json.JSONDecodeError, TypeError): - # TODO: log a warning about malformed JSON and the line contents - pass + logger.warning("malformed JSON from native module", raw=line) log_fn(line, pid=self._process.pid if self._process else None) stream.close() @@ -226,15 +238,9 @@ def _collect_topics(self) -> dict[str, str]: topics[name] = str(topic) return topics - def _build_extra_args(self) -> list[str]: - """Override in subclasses to pass additional config as CLI args. - - Called after topic args, before ``config.extra_args``. - """ - return [] - __all__ = [ + "LogFormat", "NativeModule", "NativeModuleConfig", ] diff --git a/dimos/core/test_native_module.py b/dimos/core/test_native_module.py index 8403d0f745..6dcb7f6769 100644 --- a/dimos/core/test_native_module.py +++ b/dimos/core/test_native_module.py @@ -67,9 +67,6 @@ def read_json_file(path: str) -> dict[str, str]: return result -# -- Test modules -- - - @dataclass(kw_only=True) class StubNativeConfig(NativeModuleConfig): executable: str = _ECHO @@ -83,10 +80,6 @@ class StubNativeModule(NativeModule): imu: Out[Imu] cmd_vel: In[Twist] - def _build_extra_args(self) -> list[str]: - cfg: StubNativeConfig = self.config # type: ignore[assignment] - return ["--some_param", str(cfg.some_param)] - class StubConsumer(Module): pointcloud: In[PointCloud2] @@ -105,6 +98,24 @@ def start(self) -> None: pass +def test_process_crash_triggers_stop() -> None: + """When the native process dies unexpectedly, the watchdog calls stop().""" + mod = StubNativeModule(extra_env={"NATIVE_ECHO_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, args_file) -> None: native_module = dimos_cluster.deploy( StubNativeModule, diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt b/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt index b10847b9a6..5e17d0b9ba 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt @@ -58,17 +58,20 @@ pkg_check_modules(LCM REQUIRED lcm) # Eigen3 find_package(Eigen3 REQUIRED) -# PCL -find_package(PCL 1.8 REQUIRED) +# PCL (only components we need — avoid full PCL which drags in VTK via io) +find_package(PCL 1.8 REQUIRED COMPONENTS common filters) -# nlohmann_json (FAST-LIO config parsing) -find_package(nlohmann_json 3.2.0 REQUIRED) +# yaml-cpp (FAST-LIO config parsing — standard YAML format) +find_package(yaml-cpp REQUIRED) -# Livox SDK2 (installed to /usr/local) -find_library(LIVOX_SDK livox_lidar_sdk_shared HINTS /usr/local/lib) +# 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. Install from https://github.com/Livox-SDK/Livox-SDK2") + 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 @@ -84,14 +87,14 @@ target_include_directories(fastlio2_native PRIVATE ${EIGEN3_INCLUDE_DIR} ${PCL_INCLUDE_DIRS} ${CMAKE_CURRENT_SOURCE_DIR} - /usr/local/include # Livox SDK2 headers + ${LIVOX_SDK_INCLUDE_DIR} ) target_link_libraries(fastlio2_native PRIVATE ${LCM_LIBRARIES} ${LIVOX_SDK} ${PCL_LIBRARIES} - nlohmann_json::nlohmann_json + yaml-cpp::yaml-cpp ) if(OpenMP_CXX_FOUND) diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp index 07a45ea5ca..e8be572364 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp @@ -11,7 +11,7 @@ // ./fastlio2_native \ // --lidar '/lidar#sensor_msgs.PointCloud2' \ // --odometry '/odometry#nav_msgs.Odometry' \ -// --config_path /path/to/mid360.json \ +// --config_path /path/to/mid360.yaml \ // --host_ip 192.168.1.5 --lidar_ip 192.168.1.155 #include @@ -394,6 +394,10 @@ int main(int argc, char** argv) { 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"); @@ -455,7 +459,7 @@ int main(int argc, char** argv) { // Init FAST-LIO with config printf("[fastlio2] Initializing FAST-LIO...\n"); - FastLio fast_lio(config_path); + FastLio fast_lio(config_path, msr_freq, main_freq); g_fastlio = &fast_lio; printf("[fastlio2] FAST-LIO initialized.\n"); @@ -492,7 +496,7 @@ int main(int argc, char** argv) { auto frame_interval = std::chrono::microseconds( static_cast(1e6 / g_frequency)); auto last_emit = std::chrono::steady_clock::now(); - const double process_period_ms = 1000.0 / 5000.0; // FAST-LIO processes at ~5kHz + const double process_period_ms = 1000.0 / main_freq; // Rate limiters for output publishing auto pc_interval = std::chrono::microseconds( diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py index d2fb8fd5ae..84e9424b09 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -19,21 +19,19 @@ Usage:: - from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2Module + from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 from dimos.core.blueprints import autoconnect autoconnect( - FastLio2Module.blueprint(host_ip="192.168.1.5"), + FastLio2.blueprint(host_ip="192.168.1.5"), SomeConsumer.blueprint(), ).build().loop() """ from __future__ import annotations -from dataclasses import dataclass, field -import json +from dataclasses import dataclass from pathlib import Path -import tempfile from dimos.core import Out # noqa: TC001 from dimos.core.native_module import NativeModule, NativeModuleConfig @@ -41,7 +39,7 @@ from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 # noqa: TC001 _DEFAULT_EXECUTABLE = str(Path(__file__).parent / "cpp" / "build" / "fastlio2_native") -_DEFAULT_CONFIG = str(Path(__file__).parent / "cpp" / "config" / "mid360.json") +_CONFIG_DIR = Path(__file__).parent / "config" @dataclass(kw_only=True) @@ -59,6 +57,10 @@ class FastLio2Config(NativeModuleConfig): 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 @@ -73,35 +75,9 @@ class FastLio2Config(NativeModuleConfig): map_voxel_size: float = 0.1 map_max_range: float = 100.0 - # FAST-LIO config (written to JSON, passed as --config_path — excluded from CLI) - cli_exclude: frozenset[str] = frozenset( - { - "scan_line", - "blind", - "fov_degree", - "det_range", - "acc_cov", - "gyr_cov", - "b_acc_cov", - "b_gyr_cov", - "extrinsic_est_en", - "extrinsic_t", - "extrinsic_r", - } - ) - scan_line: int = 4 - blind: float = 0.5 - fov_degree: int = 360 - det_range: float = 100.0 - acc_cov: float = 0.1 - gyr_cov: float = 0.1 - b_acc_cov: float = 0.0001 - b_gyr_cov: float = 0.0001 - extrinsic_est_en: bool = True - extrinsic_t: list[float] = field(default_factory=lambda: [-0.011, -0.02329, 0.04412]) - extrinsic_r: list[float] = field( - default_factory=lambda: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] - ) + # 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 (for multi-sensor setups) cmd_data_port: int = 56100 @@ -115,6 +91,19 @@ class FastLio2Config(NativeModuleConfig): host_imu_data_port: int = 56401 host_log_data_port: int = 56501 + # 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): """FAST-LIO2 SLAM module with integrated Livox Mid-360 driver. @@ -130,44 +119,6 @@ class FastLio2(NativeModule): odometry: Out[Odometry] global_map: Out[PointCloud2] - def _build_extra_args(self) -> list[str]: - """Pass hardware and SLAM config to the C++ binary as CLI args.""" - cfg: FastLio2Config = self.config # type: ignore[assignment] - config_path = self._write_fastlio_config(cfg) - return ["--config_path", config_path, *cfg.to_cli_args()] - - @staticmethod - def _write_fastlio_config(cfg: FastLio2Config) -> str: - """Write FAST-LIO JSON config to a temp file, return path.""" - config = { - "common": { - "time_sync_en": False, - "time_offset_lidar_to_imu": 0.0, - "msr_freq": 50.0, - "main_freq": 5000.0, - }, - "preprocess": { - "lidar_type": 1, - "scan_line": cfg.scan_line, - "blind": cfg.blind, - }, - "mapping": { - "acc_cov": cfg.acc_cov, - "gyr_cov": cfg.gyr_cov, - "b_acc_cov": cfg.b_acc_cov, - "b_gyr_cov": cfg.b_gyr_cov, - "fov_degree": cfg.fov_degree, - "det_range": cfg.det_range, - "extrinsic_est_en": cfg.extrinsic_est_en, - "extrinsic_T": list(cfg.extrinsic_t), - "extrinsic_R": list(cfg.extrinsic_r), - }, - } - f = tempfile.NamedTemporaryFile(mode="w", suffix=".json", prefix="fastlio2_", delete=False) - json.dump(config, f, indent=2) - f.close() - return f.name - fastlio2_module = FastLio2.blueprint diff --git a/dimos/hardware/sensors/lidar/livox/cpp/CMakeLists.txt b/dimos/hardware/sensors/lidar/livox/cpp/CMakeLists.txt index b0c80c5834..8de380140a 100644 --- a/dimos/hardware/sensors/lidar/livox/cpp/CMakeLists.txt +++ b/dimos/hardware/sensors/lidar/livox/cpp/CMakeLists.txt @@ -17,11 +17,14 @@ FetchContent_MakeAvailable(dimos_lcm) find_package(PkgConfig REQUIRED) pkg_check_modules(LCM REQUIRED lcm) -# Livox SDK2 (installed to /usr/local) -find_library(LIVOX_SDK livox_lidar_sdk_shared HINTS /usr/local/lib) +# 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. Install from https://github.com/Livox-SDK/Livox-SDK2") + 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) @@ -29,7 +32,7 @@ target_include_directories(mid360_native PRIVATE ${dimos_lcm_SOURCE_DIR}/generated/cpp_lcm_msgs ${LCM_INCLUDE_DIRS} ${CMAKE_CURRENT_SOURCE_DIR} - /usr/local/include # Livox SDK2 headers + ${LIVOX_SDK_INCLUDE_DIR} ) target_link_libraries(mid360_native PRIVATE diff --git a/dimos/hardware/sensors/lidar/livox/flake.lock b/dimos/hardware/sensors/lidar/livox/flake.lock new file mode 100644 index 0000000000..3009a12016 --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/flake.lock @@ -0,0 +1,61 @@ +{ + "nodes": { + "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": { + "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/flake.nix b/dimos/hardware/sensors/lidar/livox/flake.nix new file mode 100644 index 0000000000..f503284d50 --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/flake.nix @@ -0,0 +1,53 @@ +{ + description = "Livox SDK2 — driver library for Livox LiDAR sensors"; + + inputs = { + nixpkgs.url = "github:NixOS/nixpkgs/nixos-unstable"; + flake-utils.url = "github:numtide/flake-utils"; + }; + + outputs = { self, nixpkgs, flake-utils, ... }: + 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 = [ + "-DCMAKE_INSTALL_PREFIX=${placeholder "out"}" + "-DBUILD_SHARED_LIBS=ON" + "-DCMAKE_POLICY_VERSION_MINIMUM=3.5" + ]; + + preConfigure = '' + # Skip samples, just build the SDK + substituteInPlace CMakeLists.txt \ + --replace-fail "add_subdirectory(samples)" "" + + # Fix missing includes for newer GCC + sed -i '1i #include ' sdk_core/comm/define.h + sed -i '1i #include ' sdk_core/logger_handler/file_manager.h + ''; + }; + in { + packages = { + default = livox-sdk2; + inherit livox-sdk2; + }; + + devShells.default = pkgs.mkShell { + buildInputs = [ livox-sdk2 ]; + }; + }); +} diff --git a/dimos/hardware/sensors/lidar/livox/module.py b/dimos/hardware/sensors/lidar/livox/module.py index cbf266eb36..a01bc893b8 100644 --- a/dimos/hardware/sensors/lidar/livox/module.py +++ b/dimos/hardware/sensors/lidar/livox/module.py @@ -82,10 +82,6 @@ class Mid360Module(NativeModule, perception.Lidar, perception.IMU): pointcloud: Out[PointCloud2] imu: Out[Imu] - def _build_extra_args(self) -> list[str]: - """Pass hardware config to the C++ binary as CLI args.""" - return self.config.to_cli_args() - mid360_module = Mid360Module.blueprint diff --git a/flake.lock b/flake.lock index 402f251030..3b6b104633 100644 --- a/flake.lock +++ b/flake.lock @@ -106,6 +106,25 @@ "type": "github" } }, + "livox-sdk": { + "inputs": { + "flake-utils": [ + "flake-utils" + ], + "nixpkgs": [ + "nixpkgs" + ] + }, + "locked": { + "path": "./dimos/hardware/sensors/lidar/livox", + "type": "path" + }, + "original": { + "path": "./dimos/hardware/sensors/lidar/livox", + "type": "path" + }, + "parent": [] + }, "nixpkgs": { "locked": { "lastModified": 1748929857, @@ -127,6 +146,7 @@ "diagon": "diagon", "flake-utils": "flake-utils", "lib": "lib", + "livox-sdk": "livox-sdk", "nixpkgs": "nixpkgs", "xome": "xome" } diff --git a/flake.nix b/flake.nix index ec3e919a90..7a5c5caf88 100644 --- a/flake.nix +++ b/flake.nix @@ -10,12 +10,16 @@ xome.inputs.nixpkgs.follows = "nixpkgs"; xome.inputs.flake-utils.follows = "flake-utils"; diagon.url = "github:petertrotman/nixpkgs/Diagon"; + livox-sdk.url = "path:./dimos/hardware/sensors/lidar/livox"; + livox-sdk.inputs.nixpkgs.follows = "nixpkgs"; + livox-sdk.inputs.flake-utils.follows = "flake-utils"; }; - outputs = { self, nixpkgs, flake-utils, lib, xome, diagon, ... }: + outputs = { self, nixpkgs, flake-utils, lib, xome, diagon, livox-sdk, ... }: flake-utils.lib.eachDefaultSystem (system: let pkgs = import nixpkgs { inherit system; }; + livox-sdk2 = livox-sdk.packages.${system}.livox-sdk2; # ------------------------------------------------------------ # 1. Shared package list (tool-chain + project deps) @@ -129,9 +133,10 @@ { vals.pkg=pkgs.libjpeg_turbo; flags.ldLibraryGroup=true; } { vals.pkg=pkgs.libpng; flags={}; } - ### FAST-LIO2 native module deps + ### LiDAR native module deps + { vals.pkg=livox-sdk2; flags.ldLibraryGroup=true; } { vals.pkg=pkgs.pcl; flags.ldLibraryGroup=true; } - { vals.pkg=pkgs.nlohmann_json; flags={}; } + { vals.pkg=pkgs.yaml-cpp; flags.ldLibraryGroup=true; } { vals.pkg=pkgs.llvmPackages.openmp; flags.ldLibraryGroup=true; } ### Docs generators From 87a3b750d6dac8d7bae543ee0f6b82d56b99efdd Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Fri, 13 Feb 2026 17:57:10 +0800 Subject: [PATCH 19/48] included lidar configs, voxel_map implementation --- .../sensors/lidar/fastlio2/config/avia.yaml | 35 +++ .../lidar/fastlio2/config/horizon.yaml | 35 +++ .../sensors/lidar/fastlio2/config/marsim.yaml | 35 +++ .../sensors/lidar/fastlio2/config/mid360.yaml | 35 +++ .../lidar/fastlio2/config/ouster64.yaml | 36 +++ .../lidar/fastlio2/config/velodyne.yaml | 37 +++ .../sensors/lidar/fastlio2/cpp/voxel_map.hpp | 297 ++++++++++++++++++ 7 files changed, 510 insertions(+) create mode 100644 dimos/hardware/sensors/lidar/fastlio2/config/avia.yaml create mode 100644 dimos/hardware/sensors/lidar/fastlio2/config/horizon.yaml create mode 100644 dimos/hardware/sensors/lidar/fastlio2/config/marsim.yaml create mode 100644 dimos/hardware/sensors/lidar/fastlio2/config/mid360.yaml create mode 100644 dimos/hardware/sensors/lidar/fastlio2/config/ouster64.yaml create mode 100644 dimos/hardware/sensors/lidar/fastlio2/config/velodyne.yaml create mode 100644 dimos/hardware/sensors/lidar/fastlio2/cpp/voxel_map.hpp 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/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 From 4c848943fe914c90af69926f0b56c7db7f6b14cd Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Fri, 13 Feb 2026 18:16:54 +0800 Subject: [PATCH 20/48] nix flakes for fastlio2_native and mid360_native builds Add standalone nix flakes to build native C++ modules with all dependencies (Livox SDK2, LCM, PCL, etc.) from nix, avoiding glibc version conflicts. CMakeLists.txt now accepts FASTLIO_DIR as an external parameter for hermetic nix builds. --- .../sensors/lidar/fastlio2/cpp/CMakeLists.txt | 16 +-- .../sensors/lidar/fastlio2/cpp/flake.lock | 135 ++++++++++++++++++ .../sensors/lidar/fastlio2/cpp/flake.nix | 61 ++++++++ dimos/hardware/sensors/lidar/livox/flake.lock | 18 +++ dimos/hardware/sensors/lidar/livox/flake.nix | 36 +++-- 5 files changed, 246 insertions(+), 20 deletions(-) create mode 100644 dimos/hardware/sensors/lidar/fastlio2/cpp/flake.lock create mode 100644 dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt b/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt index 5e17d0b9ba..04d3c2b616 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt @@ -30,18 +30,10 @@ endif() # Fetch dependencies include(FetchContent) -# FAST-LIO-NON-ROS (local checkout for development) -set(FASTLIO_DIR $ENV{HOME}/coding/FAST-LIO-NON-ROS) -# FetchContent_Declare(fastlio -# GIT_REPOSITORY https://github.com/leshy/FAST-LIO-NON-ROS.git -# GIT_TAG dimos-integration -# GIT_SHALLOW TRUE -# ) -# FetchContent_GetProperties(fastlio) -# if(NOT fastlio_POPULATED) -# FetchContent_Populate(fastlio) -# endif() -# set(FASTLIO_DIR ${fastlio_SOURCE_DIR}) +# FAST-LIO-NON-ROS (passed via -DFASTLIO_DIR= or defaults to local checkout) +if(NOT FASTLIO_DIR) + set(FASTLIO_DIR $ENV{HOME}/coding/FAST-LIO-NON-ROS) +endif() # dimos-lcm C++ message headers FetchContent_Declare(dimos_lcm 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..245b7dede6 --- /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", + "type": "path" + }, + "original": { + "path": "../../livox", + "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..5e45a5afcb --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix @@ -0,0 +1,61 @@ +{ + 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"; + 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; + + 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}" + ]; + + installPhase = '' + mkdir -p $out/bin + cp fastlio2_native $out/bin/ + ''; + }; + in { + packages = { + default = fastlio2_native; + inherit fastlio2_native; + }; + }); +} diff --git a/dimos/hardware/sensors/lidar/livox/flake.lock b/dimos/hardware/sensors/lidar/livox/flake.lock index 3009a12016..58e8252be8 100644 --- a/dimos/hardware/sensors/lidar/livox/flake.lock +++ b/dimos/hardware/sensors/lidar/livox/flake.lock @@ -1,5 +1,22 @@ { "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" @@ -36,6 +53,7 @@ }, "root": { "inputs": { + "dimos-lcm": "dimos-lcm", "flake-utils": "flake-utils", "nixpkgs": "nixpkgs" } diff --git a/dimos/hardware/sensors/lidar/livox/flake.nix b/dimos/hardware/sensors/lidar/livox/flake.nix index f503284d50..0ab03c1ef3 100644 --- a/dimos/hardware/sensors/lidar/livox/flake.nix +++ b/dimos/hardware/sensors/lidar/livox/flake.nix @@ -1,12 +1,16 @@ { - description = "Livox SDK2 — driver library for Livox LiDAR sensors"; + 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, ... }: + outputs = { self, nixpkgs, flake-utils, dimos-lcm, ... }: flake-utils.lib.eachDefaultSystem (system: let pkgs = import nixpkgs { inherit system; }; @@ -25,25 +29,41 @@ nativeBuildInputs = [ pkgs.cmake ]; cmakeFlags = [ - "-DCMAKE_INSTALL_PREFIX=${placeholder "out"}" "-DBUILD_SHARED_LIBS=ON" "-DCMAKE_POLICY_VERSION_MINIMUM=3.5" ]; preConfigure = '' - # Skip samples, just build the SDK substituteInPlace CMakeLists.txt \ --replace-fail "add_subdirectory(samples)" "" - - # Fix missing includes for newer GCC sed -i '1i #include ' sdk_core/comm/define.h sed -i '1i #include ' sdk_core/logger_handler/file_manager.h ''; }; + + mid360_native = pkgs.stdenv.mkDerivation { + pname = "mid360_native"; + version = "0.1.0"; + + src = ./cpp; + + 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}" + ]; + + installPhase = '' + mkdir -p $out/bin + cp mid360_native $out/bin/ + ''; + }; in { packages = { - default = livox-sdk2; - inherit livox-sdk2; + default = mid360_native; + inherit livox-sdk2 mid360_native; }; devShells.default = pkgs.mkShell { From 4ff322d8f9e30c6068e79da13d78ded636e21d78 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Fri, 13 Feb 2026 18:46:09 +0800 Subject: [PATCH 21/48] clean up native module build: consolidate install paths, simplify main flake Move livox flake.nix into cpp/ for consistency with fastlio2. Add cmake install() rules with default prefix=result so both nix and cmake produce binaries at cpp/result/bin/. Remove livox-sdk and lidar deps from main flake since native modules build via their own standalone flakes. --- .gitignore | 1 + .../sensors/lidar/fastlio2/cpp/CMakeLists.txt | 6 ++++++ .../sensors/lidar/fastlio2/cpp/flake.lock | 4 ++-- .../sensors/lidar/fastlio2/cpp/flake.nix | 7 +------ .../hardware/sensors/lidar/fastlio2/module.py | 2 +- .../sensors/lidar/livox/cpp/CMakeLists.txt | 6 ++++++ .../sensors/lidar/livox/{ => cpp}/flake.lock | 0 .../sensors/lidar/livox/{ => cpp}/flake.nix | 7 +------ dimos/hardware/sensors/lidar/livox/module.py | 2 +- flake.lock | 20 ------------------- flake.nix | 12 +---------- 11 files changed, 20 insertions(+), 47 deletions(-) rename dimos/hardware/sensors/lidar/livox/{ => cpp}/flake.lock (100%) rename dimos/hardware/sensors/lidar/livox/{ => cpp}/flake.nix (93%) diff --git a/.gitignore b/.gitignore index 686327e990..3da8726cd4 100644 --- a/.gitignore +++ b/.gitignore @@ -65,5 +65,6 @@ yolo11n.pt *mobileclip* /results +result /assets/teleop_certs/ diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt b/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt index 04d3c2b616..1fe302898a 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt @@ -5,6 +5,10 @@ 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) @@ -96,3 +100,5 @@ 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/flake.lock b/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.lock index 245b7dede6..2636f00ada 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.lock +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.lock @@ -80,11 +80,11 @@ ] }, "locked": { - "path": "../../livox", + "path": "../../livox/cpp", "type": "path" }, "original": { - "path": "../../livox", + "path": "../../livox/cpp", "type": "path" }, "parent": [] diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix b/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix index 5e45a5afcb..286218f8b8 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix @@ -4,7 +4,7 @@ inputs = { nixpkgs.url = "github:NixOS/nixpkgs/nixos-unstable"; flake-utils.url = "github:numtide/flake-utils"; - livox-sdk.url = "path:../../livox"; + livox-sdk.url = "path:../../livox/cpp"; livox-sdk.inputs.nixpkgs.follows = "nixpkgs"; livox-sdk.inputs.flake-utils.follows = "flake-utils"; dimos-lcm = { @@ -46,11 +46,6 @@ "-DFETCHCONTENT_SOURCE_DIR_DIMOS_LCM=${dimos-lcm}" "-DFASTLIO_DIR=${fast-lio}" ]; - - installPhase = '' - mkdir -p $out/bin - cp fastlio2_native $out/bin/ - ''; }; in { packages = { diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py index 84e9424b09..ea2546f40e 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -38,7 +38,7 @@ from dimos.msgs.nav_msgs.Odometry import Odometry # noqa: TC001 from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 # noqa: TC001 -_DEFAULT_EXECUTABLE = str(Path(__file__).parent / "cpp" / "build" / "fastlio2_native") +_DEFAULT_EXECUTABLE = str(Path(__file__).parent / "cpp" / "result" / "bin" / "fastlio2_native") _CONFIG_DIR = Path(__file__).parent / "config" diff --git a/dimos/hardware/sensors/lidar/livox/cpp/CMakeLists.txt b/dimos/hardware/sensors/lidar/livox/cpp/CMakeLists.txt index 8de380140a..59e1385e07 100644 --- a/dimos/hardware/sensors/lidar/livox/cpp/CMakeLists.txt +++ b/dimos/hardware/sensors/lidar/livox/cpp/CMakeLists.txt @@ -4,6 +4,10 @@ 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 @@ -43,3 +47,5 @@ target_link_libraries(mid360_native PRIVATE target_link_directories(mid360_native PRIVATE ${LCM_LIBRARY_DIRS} ) + +install(TARGETS mid360_native DESTINATION bin) diff --git a/dimos/hardware/sensors/lidar/livox/flake.lock b/dimos/hardware/sensors/lidar/livox/cpp/flake.lock similarity index 100% rename from dimos/hardware/sensors/lidar/livox/flake.lock rename to dimos/hardware/sensors/lidar/livox/cpp/flake.lock diff --git a/dimos/hardware/sensors/lidar/livox/flake.nix b/dimos/hardware/sensors/lidar/livox/cpp/flake.nix similarity index 93% rename from dimos/hardware/sensors/lidar/livox/flake.nix rename to dimos/hardware/sensors/lidar/livox/cpp/flake.nix index 0ab03c1ef3..9c27a02617 100644 --- a/dimos/hardware/sensors/lidar/livox/flake.nix +++ b/dimos/hardware/sensors/lidar/livox/cpp/flake.nix @@ -45,7 +45,7 @@ pname = "mid360_native"; version = "0.1.0"; - src = ./cpp; + src = ./.; nativeBuildInputs = [ pkgs.cmake pkgs.pkg-config ]; buildInputs = [ livox-sdk2 pkgs.lcm pkgs.glib ]; @@ -54,11 +54,6 @@ "-DCMAKE_POLICY_VERSION_MINIMUM=3.5" "-DFETCHCONTENT_SOURCE_DIR_DIMOS_LCM=${dimos-lcm}" ]; - - installPhase = '' - mkdir -p $out/bin - cp mid360_native $out/bin/ - ''; }; in { packages = { diff --git a/dimos/hardware/sensors/lidar/livox/module.py b/dimos/hardware/sensors/lidar/livox/module.py index a01bc893b8..d6f35e77b5 100644 --- a/dimos/hardware/sensors/lidar/livox/module.py +++ b/dimos/hardware/sensors/lidar/livox/module.py @@ -40,7 +40,7 @@ from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 # noqa: TC001 from dimos.spec import perception -_DEFAULT_EXECUTABLE = str(Path(__file__).parent / "cpp" / "build" / "mid360_native") +_DEFAULT_EXECUTABLE = str(Path(__file__).parent / "cpp" / "result" / "bin" / "mid360_native") @dataclass(kw_only=True) diff --git a/flake.lock b/flake.lock index 3b6b104633..402f251030 100644 --- a/flake.lock +++ b/flake.lock @@ -106,25 +106,6 @@ "type": "github" } }, - "livox-sdk": { - "inputs": { - "flake-utils": [ - "flake-utils" - ], - "nixpkgs": [ - "nixpkgs" - ] - }, - "locked": { - "path": "./dimos/hardware/sensors/lidar/livox", - "type": "path" - }, - "original": { - "path": "./dimos/hardware/sensors/lidar/livox", - "type": "path" - }, - "parent": [] - }, "nixpkgs": { "locked": { "lastModified": 1748929857, @@ -146,7 +127,6 @@ "diagon": "diagon", "flake-utils": "flake-utils", "lib": "lib", - "livox-sdk": "livox-sdk", "nixpkgs": "nixpkgs", "xome": "xome" } diff --git a/flake.nix b/flake.nix index 7a5c5caf88..50738effc0 100644 --- a/flake.nix +++ b/flake.nix @@ -10,16 +10,12 @@ xome.inputs.nixpkgs.follows = "nixpkgs"; xome.inputs.flake-utils.follows = "flake-utils"; diagon.url = "github:petertrotman/nixpkgs/Diagon"; - livox-sdk.url = "path:./dimos/hardware/sensors/lidar/livox"; - livox-sdk.inputs.nixpkgs.follows = "nixpkgs"; - livox-sdk.inputs.flake-utils.follows = "flake-utils"; }; - outputs = { self, nixpkgs, flake-utils, lib, xome, diagon, livox-sdk, ... }: + outputs = { self, nixpkgs, flake-utils, lib, xome, diagon, ... }: flake-utils.lib.eachDefaultSystem (system: let pkgs = import nixpkgs { inherit system; }; - livox-sdk2 = livox-sdk.packages.${system}.livox-sdk2; # ------------------------------------------------------------ # 1. Shared package list (tool-chain + project deps) @@ -133,12 +129,6 @@ { vals.pkg=pkgs.libjpeg_turbo; flags.ldLibraryGroup=true; } { vals.pkg=pkgs.libpng; flags={}; } - ### LiDAR native module deps - { vals.pkg=livox-sdk2; flags.ldLibraryGroup=true; } - { vals.pkg=pkgs.pcl; flags.ldLibraryGroup=true; } - { vals.pkg=pkgs.yaml-cpp; flags.ldLibraryGroup=true; } - { vals.pkg=pkgs.llvmPackages.openmp; flags.ldLibraryGroup=true; } - ### Docs generators { vals.pkg=pkgs.pikchr; flags={}; } { vals.pkg=pkgs.graphviz; flags={}; } From f370d066791557231eeb9e94186a6dcd7ca85ddb Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Fri, 13 Feb 2026 19:03:21 +0800 Subject: [PATCH 22/48] readme files for both modules --- .../sensors/lidar/fastlio2/cpp/README.md | 109 ++++++++++++++++++ .../sensors/lidar/livox/cpp/README.md | 81 +++++++++---- 2 files changed, 170 insertions(+), 20 deletions(-) create mode 100644 dimos/hardware/sensors/lidar/fastlio2/cpp/README.md 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..c93ccfcce8 --- /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 defaults to `~/coding/FAST-LIO-NON-ROS`. + +## 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/livox/cpp/README.md b/dimos/hardware/sensors/lidar/livox/cpp/README.md index c4f8293008..9affadf03f 100644 --- a/dimos/hardware/sensors/lidar/livox/cpp/README.md +++ b/dimos/hardware/sensors/lidar/livox/cpp/README.md @@ -3,13 +3,31 @@ Native C++ driver for the Livox Mid-360 LiDAR. Publishes PointCloud2 and IMU data directly on LCM, bypassing Python for minimal latency. -## Prerequisites +## Build -- [LCM](https://lcm-proj.github.io/) (`pacman -S lcm` or build from source) -- [Livox SDK2](https://github.com/Livox-SDK/Livox-SDK2) — build and install to `/usr/local` +### 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 +Installing Livox SDK2 manually: ```bash cd ~/src @@ -19,25 +37,35 @@ cmake .. && make -j$(nproc) sudo make install ``` -## Build +Then build: ```bash cd dimos/hardware/sensors/lidar/livox/cpp -mkdir -p build && cd build -cmake .. -make -j$(nproc) +cmake -B build +cmake --build build -j$(nproc) +cmake --install build ``` -The binary lands at `build/mid360_native`. +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 `Mid360Module` via the NativeModule framework — you -don't run it directly. The Python wrapper passes LCM topic strings and config -as CLI args: +Normally launched by `Mid360Module` via the NativeModule framework: ```python from dimos.hardware.sensors.lidar.livox.module import Mid360Module @@ -52,7 +80,7 @@ autoconnect( ### Manual invocation (for debugging) ```bash -./build/mid360_native \ +./result/bin/mid360_native \ --pointcloud '/pointcloud#sensor_msgs.PointCloud2' \ --imu '/imu#sensor_msgs.Imu' \ --host_ip 192.168.1.5 \ @@ -60,14 +88,27 @@ autoconnect( --frequency 10 ``` -Topic strings must include the `#type` suffix — this is the actual LCM channel +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 | -| `../module.py` | Python NativeModule wrapper (`Mid360Module`) | -| `CMakeLists.txt` | Build config, fetches dimos-lcm headers automatically | +| 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 (`Mid360Module`) | From 2c75f06e09f0ab1161f83768cc2d6419aa9428c6 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Fri, 13 Feb 2026 19:16:04 +0800 Subject: [PATCH 23/48] update fastlio blueprints: adjust voxel size and reorder configs --- .../lidar/fastlio2/fastlio_blueprints.py | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py b/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py index ced2b74374..e4ece931df 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py +++ b/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py @@ -17,7 +17,7 @@ from dimos.mapping.voxels import VoxelGridMapper from dimos.visualization.rerun.bridge import rerun_bridge -voxel_size = 0.05 +voxel_size = 0.25 mid360_fastlio = autoconnect( FastLio2.blueprint(voxel_size=voxel_size, map_voxel_size=voxel_size, map_freq=-1), @@ -28,23 +28,23 @@ ), ).global_config(n_dask_workers=2, robot_model="mid360_fastlio2") -mid360_fastlio_voxels_native = autoconnect( - FastLio2.blueprint(voxel_size=voxel_size, map_voxel_size=voxel_size, map_freq=3.0), +mid360_fastlio_voxels = autoconnect( + FastLio2.blueprint(), + VoxelGridMapper.blueprint(publish_interval=1.0, voxel_size=voxel_size, carve_columns=False), rerun_bridge( visual_override={ - "world/lidar": None, "world/global_map": lambda grid: grid.to_rerun(voxel_size=voxel_size, mode="boxes"), + "world/lidar": None, } ), -).global_config(n_dask_workers=2, robot_model="mid360_fastlio2") +).global_config(n_dask_workers=3, robot_model="mid360_fastlio2_voxels") -mid360_fastlio_voxels = autoconnect( - FastLio2.blueprint(), - VoxelGridMapper.blueprint(publish_interval=1.0, voxel_size=voxel_size, carve_columns=False), +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/global_map": lambda grid: grid.to_rerun(voxel_size=voxel_size, mode="boxes"), "world/lidar": None, + "world/global_map": lambda grid: grid.to_rerun(voxel_size=voxel_size, mode="boxes"), } ), -).global_config(n_dask_workers=3, robot_model="mid360_fastlio2_voxels") +).global_config(n_dask_workers=2, robot_model="mid360_fastlio2") From 8f4ae4759d129edb36c055cf39d0adbf3672f444 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Fri, 13 Feb 2026 19:17:15 +0800 Subject: [PATCH 24/48] all blueprints update --- dimos/robot/all_blueprints.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 291058c8c9..cf6a644da2 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -40,6 +40,10 @@ "demo-osm": "dimos.mapping.osm.demo_osm:demo_osm", "demo-skill": "dimos.agents.skills.demo_skill:demo_skill", "dual-xarm6-planner": "dimos.manipulation.manipulation_blueprints:dual_xarm6_planner", + "mid360": "dimos.hardware.sensors.lidar.livox.livox_blueprints:mid360", + "mid360-fastlio": "dimos.hardware.sensors.lidar.fastlio2.fastlio_blueprints:mid360_fastlio", + "mid360-fastlio-voxels": "dimos.hardware.sensors.lidar.fastlio2.fastlio_blueprints:mid360_fastlio_voxels", + "mid360-fastlio-voxels-native": "dimos.hardware.sensors.lidar.fastlio2.fastlio_blueprints:mid360_fastlio_voxels_native", "uintree-g1-primitive-no-nav": "dimos.robot.unitree.g1.blueprints.primitive.uintree_g1_primitive_no_nav:uintree_g1_primitive_no_nav", "unitree-g1": "dimos.robot.unitree.g1.blueprints.perceptive.unitree_g1:unitree_g1", "unitree-g1-agentic": "dimos.robot.unitree.g1.blueprints.agentic.unitree_g1_agentic:unitree_g1_agentic", @@ -80,6 +84,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", @@ -91,9 +96,11 @@ "human_input": "dimos.agents.cli.human", "joint_trajectory_controller": "dimos.manipulation.control.trajectory_controller.joint_trajectory_controller", "keyboard_teleop": "dimos.robot.unitree.keyboard_teleop", + "lidar_module": "dimos.hardware.sensors.lidar.module", "llm_agent": "dimos.agents.agent", "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", From e3c84def13c4cb6d49633e2c16490c05af39508e Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Fri, 13 Feb 2026 19:28:49 +0800 Subject: [PATCH 25/48] fix mypy errors and rename test_spec_compliance to test_spec - Add generic type param to NativeModule (Module[NativeModuleConfig]) - Add Any import and type annotations to NativeModule.__init__ - Fix ModuleConfig import in lidar module - Fix FastLio2Module -> FastLio2 import in test - Rename test_spec_compliance.py to test_spec.py across lidar modules - Adjust fastlio2 voxel_size from 0.25 to 0.15 --- dimos/core/native_module.py | 6 +++--- dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py | 2 +- .../fastlio2/{test_spec_compliance.py => test_spec.py} | 6 +++--- .../lidar/livox/{test_spec_compliance.py => test_spec.py} | 0 dimos/hardware/sensors/lidar/module.py | 3 ++- .../sensors/lidar/{test_spec_compliance.py => test_spec.py} | 0 6 files changed, 9 insertions(+), 8 deletions(-) rename dimos/hardware/sensors/lidar/fastlio2/{test_spec_compliance.py => test_spec.py} (90%) rename dimos/hardware/sensors/lidar/livox/{test_spec_compliance.py => test_spec.py} (100%) rename dimos/hardware/sensors/lidar/{test_spec_compliance.py => test_spec.py} (100%) diff --git a/dimos/core/native_module.py b/dimos/core/native_module.py index 5bb3791756..b46ebf38f7 100644 --- a/dimos/core/native_module.py +++ b/dimos/core/native_module.py @@ -48,7 +48,7 @@ class MyCppModule(NativeModule): import signal import subprocess import threading -from typing import IO +from typing import IO, Any from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig @@ -102,7 +102,7 @@ def to_cli_args(self) -> list[str]: return args -class NativeModule(Module): +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 @@ -122,7 +122,7 @@ class NativeModule(Module): _watchdog: threading.Thread | None = None _stopping: bool = False - def __init__(self, *args, **kwargs): + def __init__(self, *args: Any, **kwargs: Any) -> None: super().__init__(*args, **kwargs) self._io_threads = [] diff --git a/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py b/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py index e4ece931df..6ea286500d 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py +++ b/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py @@ -17,7 +17,7 @@ from dimos.mapping.voxels import VoxelGridMapper from dimos.visualization.rerun.bridge import rerun_bridge -voxel_size = 0.25 +voxel_size = 0.15 mid360_fastlio = autoconnect( FastLio2.blueprint(voxel_size=voxel_size, map_voxel_size=voxel_size, map_freq=-1), diff --git a/dimos/hardware/sensors/lidar/fastlio2/test_spec_compliance.py b/dimos/hardware/sensors/lidar/fastlio2/test_spec.py similarity index 90% rename from dimos/hardware/sensors/lidar/fastlio2/test_spec_compliance.py rename to dimos/hardware/sensors/lidar/fastlio2/test_spec.py index 8707d7c0af..fdf39fe070 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/test_spec_compliance.py +++ b/dimos/hardware/sensors/lidar/fastlio2/test_spec.py @@ -16,14 +16,14 @@ from __future__ import annotations -from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2Module +from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 def test_fastlio2_has_lidar_port() -> None: - hints = FastLio2Module.__annotations__ + hints = FastLio2.__annotations__ assert "lidar" in hints def test_fastlio2_has_odometry_port() -> None: - hints = FastLio2Module.__annotations__ + hints = FastLio2.__annotations__ assert "odometry" in hints diff --git a/dimos/hardware/sensors/lidar/livox/test_spec_compliance.py b/dimos/hardware/sensors/lidar/livox/test_spec.py similarity index 100% rename from dimos/hardware/sensors/lidar/livox/test_spec_compliance.py rename to dimos/hardware/sensors/lidar/livox/test_spec.py diff --git a/dimos/hardware/sensors/lidar/module.py b/dimos/hardware/sensors/lidar/module.py index 660044174b..8876d74368 100644 --- a/dimos/hardware/sensors/lidar/module.py +++ b/dimos/hardware/sensors/lidar/module.py @@ -22,7 +22,8 @@ import reactivex as rx from reactivex import operators as ops -from dimos.core import Module, ModuleConfig, Out, rpc +from dimos.core import Module, Out, rpc +from dimos.core.module import ModuleConfig from dimos.hardware.sensors.lidar.spec import LidarHardware from dimos.msgs.geometry_msgs import Quaternion, Transform, Vector3 from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 diff --git a/dimos/hardware/sensors/lidar/test_spec_compliance.py b/dimos/hardware/sensors/lidar/test_spec.py similarity index 100% rename from dimos/hardware/sensors/lidar/test_spec_compliance.py rename to dimos/hardware/sensors/lidar/test_spec.py From 58938059f9fdbe7eebbffe1dcb08734d0eee3c01 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Fri, 13 Feb 2026 19:35:04 +0800 Subject: [PATCH 26/48] fix test_autoconnect: compare Topic.topic string instead of Topic object --- dimos/core/test_native_module.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dimos/core/test_native_module.py b/dimos/core/test_native_module.py index 6dcb7f6769..f4a34346c8 100644 --- a/dimos/core/test_native_module.py +++ b/dimos/core/test_native_module.py @@ -167,7 +167,7 @@ def test_autoconnect(args_file) -> None: assert producer.cmd_vel.transport.topic == native.cmd_vel.transport.topic # Custom transport was applied - assert native.pointcloud.transport.topic == "/my/custom/lidar" + assert native.pointcloud.transport.topic.topic == "/my/custom/lidar" finally: coordinator.stop() From 9592b25deda8e60a91a6f763385681781eb367f8 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Fri, 13 Feb 2026 19:57:21 +0800 Subject: [PATCH 27/48] removed useless file --- test_livox_hw.py | 92 ------------------------------------------------ 1 file changed, 92 deletions(-) delete mode 100644 test_livox_hw.py diff --git a/test_livox_hw.py b/test_livox_hw.py deleted file mode 100644 index 75daacff38..0000000000 --- a/test_livox_hw.py +++ /dev/null @@ -1,92 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Deploy LivoxLidarModule with LCM transport for pointcloud and IMU.""" - -import time - -from dimos.core import In, Module, start -from dimos.core.transport import LCMTransport -from dimos.hardware.sensors.lidar.livox.mid360 import LivoxMid360 -from dimos.hardware.sensors.lidar.livox.module import LivoxLidarModule -from dimos.msgs.sensor_msgs.Imu import Imu -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.visualization.rerun.bridge import RerunBridgeModule - -pc_count = 0 -imu_count = 0 - - -class LidarListener(Module): - pointcloud: In[PointCloud2] - imu: In[Imu] - - def start(self): - super().start() - self.pointcloud.subscribe(self._on_pc) - self.imu.subscribe(self._on_imu) - - def _on_pc(self, pc): - global pc_count - pc_count += 1 - n = len(pc.pointcloud.points) if hasattr(pc.pointcloud, "points") else "?" - print(f" [PC #{pc_count}] {n} points, ts={pc.ts:.3f}", flush=True) - - def _on_imu(self, imu_msg): - global imu_count - imu_count += 1 - if imu_count % 200 == 1: - print( - f" [IMU #{imu_count}] acc=({imu_msg.linear_acceleration.x:.2f}, " - f"{imu_msg.linear_acceleration.y:.2f}, {imu_msg.linear_acceleration.z:.2f})", - flush=True, - ) - - -if __name__ == "__main__": - dimos = start(3) - - lidar = dimos.deploy( - LivoxLidarModule, - hardware=lambda: LivoxMid360( - host_ip="192.168.1.5", - lidar_ips=["192.168.1.155"], - frequency=1.0, # , voxel_size=0.25 - ), - ) - listener = dimos.deploy(LidarListener) - bridge = dimos.deploy( - RerunBridgeModule, - visual_override={ - # "world/lidar/pointcloud": lambda pc: pc.to_rerun(mode="boxes", voxel_size=0.25), - }, - ) - - lidar.pointcloud.transport = LCMTransport("/lidar/pointcloud", PointCloud2) - lidar.imu.transport = LCMTransport("/lidar/imu", Imu) - listener.pointcloud.transport = LCMTransport("/lidar/pointcloud", PointCloud2) - listener.imu.transport = LCMTransport("/lidar/imu", Imu) - - lidar.start() - listener.start() - bridge.start() - - print("LivoxLidarModule + listener running. Ctrl+C to stop.", flush=True) - try: - while True: - time.sleep(1) - except KeyboardInterrupt: - pass - - dimos.stop() From 96619c0e0268d0b3c61b8f40b62cb9306796577d Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Fri, 13 Feb 2026 20:07:40 +0800 Subject: [PATCH 28/48] rerun bridge cli run fix --- dimos/visualization/rerun/bridge.py | 13 ++++++------- pyproject.toml | 4 ++-- 2 files changed, 8 insertions(+), 9 deletions(-) diff --git a/dimos/visualization/rerun/bridge.py b/dimos/visualization/rerun/bridge.py index c236b5c543..1dc104f1b4 100644 --- a/dimos/visualization/rerun/bridge.py +++ b/dimos/visualization/rerun/bridge.py @@ -251,11 +251,6 @@ def _on_message(self, msg: Any, topic: Any) -> None: else: rr.log(entity_path, cast("Archetype", rerun_data)) - # # Connect entity to its TF frame so transforms apply correctly - # frame_id = getattr(msg, "frame_id", None) - # if frame_id and not is_rerun_multi(rerun_data): - # rr.log(entity_path, rr.Transform3D(parent_frame="tf#/" + frame_id)) - @rpc def start(self) -> None: import rerun as rr @@ -327,7 +322,11 @@ def run_bridge( signal.pause() -def main( +app = typer.Typer() + + +@app.command() +def cli( viewer_mode: str = typer.Option( "native", help="Viewer mode: native (desktop), web (browser), none (headless)" ), @@ -340,7 +339,7 @@ def main( if __name__ == "__main__": - typer.run(main) + 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/pyproject.toml b/pyproject.toml index 1837e111e1..e9842b3349 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -74,9 +74,9 @@ lcmspy = "dimos.utils.cli.lcmspy.run_lcmspy:main" foxglove-bridge = "dimos.utils.cli.foxglove_bridge.run_foxglove_bridge:main" skillspy = "dimos.utils.cli.skillspy.skillspy:main" agentspy = "dimos.utils.cli.agentspy.agentspy:main" -humancli = "dimos.utils.cli.human.humanclianim:main" +humancli = "dimos.utils.cli.human.humanclianim:run" 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] From 36bb19a943ff04ae3b4077a4b18e63b0e6aef9be Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Fri, 13 Feb 2026 21:16:42 +0800 Subject: [PATCH 29/48] replace /tmp config files with memfd_create, extract shared livox_sdk_config.hpp Livox SDK2 requires a JSON config file for init. Previously both mid360 and fastlio2 binaries wrote temp files to /tmp via mkstemp with potential fd leaks. Now uses memfd_create + /proc/self/fd/N for a purely in-memory config that auto-cleans on fd close. Extracted duplicated SdkPorts, write_sdk_config, and init_livox_sdk into lidar/common/livox_sdk_config.hpp shared by both modules. --- .../sensors/lidar/common/livox_sdk_config.hpp | 116 ++++++++++++++++++ .../sensors/lidar/fastlio2/cpp/CMakeLists.txt | 6 + .../sensors/lidar/fastlio2/cpp/flake.nix | 3 + .../sensors/lidar/fastlio2/cpp/main.cpp | 99 ++------------- .../sensors/lidar/livox/cpp/CMakeLists.txt | 6 + .../sensors/lidar/livox/cpp/flake.nix | 3 + .../hardware/sensors/lidar/livox/cpp/main.cpp | 100 ++------------- 7 files changed, 152 insertions(+), 181 deletions(-) create mode 100644 dimos/hardware/sensors/lidar/common/livox_sdk_config.hpp 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/cpp/CMakeLists.txt b/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt index 1fe302898a..785462d1f8 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt @@ -75,6 +75,11 @@ add_executable(fastlio2_native ${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 @@ -83,6 +88,7 @@ target_include_directories(fastlio2_native PRIVATE ${EIGEN3_INCLUDE_DIR} ${PCL_INCLUDE_DIRS} ${CMAKE_CURRENT_SOURCE_DIR} + ${LIVOX_COMMON_DIR} ${LIVOX_SDK_INCLUDE_DIR} ) diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix b/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix index 286218f8b8..7a58aceb76 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix @@ -23,6 +23,8 @@ 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"; @@ -45,6 +47,7 @@ "-DCMAKE_POLICY_VERSION_MINIMUM=3.5" "-DFETCHCONTENT_SOURCE_DIR_DIMOS_LCM=${dimos-lcm}" "-DFASTLIO_DIR=${fast-lio}" + "-DLIVOX_COMMON_DIR=${livox-common}" ]; }; in { diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp index e8be572364..b9953b2bd3 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp @@ -15,8 +15,6 @@ // --host_ip 192.168.1.5 --lidar_ip 192.168.1.155 #include -#include -#include #include #include @@ -30,6 +28,8 @@ #include #include +#include "livox_sdk_config.hpp" + #include "cloud_filter.hpp" #include "dimos_native_module.hpp" #include "voxel_map.hpp" @@ -47,13 +47,10 @@ // FAST-LIO (header-only core, compiled sources linked via CMake) #include "fast_lio.hpp" -// Gravity constant for converting accelerometer data from g to m/s^2 -static constexpr double GRAVITY_MS2 = 9.80665; - -// Livox data_type values -static constexpr uint8_t DATA_TYPE_IMU = 0x00; -static constexpr uint8_t DATA_TYPE_CARTESIAN_HIGH = 0x01; -static constexpr uint8_t DATA_TYPE_CARTESIAN_LOW = 0x02; +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 @@ -303,73 +300,6 @@ static void signal_handler(int /*sig*/) { g_running.store(false); } -// --------------------------------------------------------------------------- -// Livox SDK config file generation (same pattern as mid360_native) -// --------------------------------------------------------------------------- - -struct SdkPorts { - int cmd_data = 56100; - int push_msg = 56200; - int point_data = 56300; - int imu_data = 56400; - int log_data = 56500; - int host_cmd_data = 56101; - int host_push_msg = 56201; - int host_point_data = 56301; - int host_imu_data = 56401; - int host_log_data = 56501; -}; - -static std::string write_sdk_config(const std::string& host_ip, - const std::string& lidar_ip, - const SdkPorts& ports) { - char tmpl[] = "/tmp/livox_fastlio2_XXXXXX"; - int fd = mkstemp(tmpl); - if (fd < 0) { - perror("mkstemp"); - return ""; - } - - FILE* fp = fdopen(fd, "w"); - if (!fp) { - perror("fdopen"); - close(fd); - return ""; - } - - fprintf(fp, - "{\n" - " \"MID360\": {\n" - " \"lidar_net_info\": {\n" - " \"cmd_data_port\": %d,\n" - " \"push_msg_port\": %d,\n" - " \"point_data_port\": %d,\n" - " \"imu_data_port\": %d,\n" - " \"log_data_port\": %d\n" - " },\n" - " \"host_net_info\": [\n" - " {\n" - " \"host_ip\": \"%s\",\n" - " \"multicast_ip\": \"224.1.1.5\",\n" - " \"cmd_data_port\": %d,\n" - " \"push_msg_port\": %d,\n" - " \"point_data_port\": %d,\n" - " \"imu_data_port\": %d,\n" - " \"log_data_port\": %d\n" - " }\n" - " ]\n" - " }\n" - "}\n", - ports.cmd_data, ports.push_msg, ports.point_data, - ports.imu_data, ports.log_data, - host_ip.c_str(), - ports.host_cmd_data, ports.host_push_msg, ports.host_point_data, - ports.host_imu_data, ports.host_log_data); - fclose(fp); - - return tmpl; -} - // --------------------------------------------------------------------------- // Main // --------------------------------------------------------------------------- @@ -415,7 +345,7 @@ int main(int argc, char** argv) { float map_freq = mod.arg_float("map_freq", 0.0f); // SDK network ports - SdkPorts ports; + livox_common::SdkPorts ports; ports.cmd_data = mod.arg_int("cmd_data_port", 56100); ports.push_msg = mod.arg_int("push_msg_port", 56200); ports.point_data = mod.arg_int("point_data_port", 56300); @@ -463,17 +393,8 @@ int main(int argc, char** argv) { g_fastlio = &fast_lio; printf("[fastlio2] FAST-LIO initialized.\n"); - // Write Livox SDK config - std::string sdk_config_path = write_sdk_config(host_ip, lidar_ip, ports); - if (sdk_config_path.empty()) { - fprintf(stderr, "Error: failed to write SDK config\n"); - return 1; - } - - // Init Livox SDK - if (!LivoxLidarSdkInit(sdk_config_path.c_str(), host_ip.c_str())) { - fprintf(stderr, "Error: LivoxLidarSdkInit failed\n"); - std::remove(sdk_config_path.c_str()); + // Init Livox SDK (in-memory config, no temp files) + if (!livox_common::init_livox_sdk(host_ip, lidar_ip, ports)) { return 1; } @@ -486,7 +407,6 @@ int main(int argc, char** argv) { if (!LivoxLidarSdkStart()) { fprintf(stderr, "Error: LivoxLidarSdkStart failed\n"); LivoxLidarSdkUninit(); - std::remove(sdk_config_path.c_str()); return 1; } @@ -612,7 +532,6 @@ int main(int argc, char** argv) { printf("[fastlio2] Shutting down...\n"); g_fastlio = nullptr; LivoxLidarSdkUninit(); - std::remove(sdk_config_path.c_str()); g_lcm = nullptr; printf("[fastlio2] Done.\n"); diff --git a/dimos/hardware/sensors/lidar/livox/cpp/CMakeLists.txt b/dimos/hardware/sensors/lidar/livox/cpp/CMakeLists.txt index 59e1385e07..b6641a2fc6 100644 --- a/dimos/hardware/sensors/lidar/livox/cpp/CMakeLists.txt +++ b/dimos/hardware/sensors/lidar/livox/cpp/CMakeLists.txt @@ -32,10 +32,16 @@ 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} ) diff --git a/dimos/hardware/sensors/lidar/livox/cpp/flake.nix b/dimos/hardware/sensors/lidar/livox/cpp/flake.nix index 9c27a02617..eeb06b33a6 100644 --- a/dimos/hardware/sensors/lidar/livox/cpp/flake.nix +++ b/dimos/hardware/sensors/lidar/livox/cpp/flake.nix @@ -41,6 +41,8 @@ ''; }; + livox-common = ../../common; + mid360_native = pkgs.stdenv.mkDerivation { pname = "mid360_native"; version = "0.1.0"; @@ -53,6 +55,7 @@ cmakeFlags = [ "-DCMAKE_POLICY_VERSION_MINIMUM=3.5" "-DFETCHCONTENT_SOURCE_DIR_DIMOS_LCM=${dimos-lcm}" + "-DLIVOX_COMMON_DIR=${livox-common}" ]; }; in { diff --git a/dimos/hardware/sensors/lidar/livox/cpp/main.cpp b/dimos/hardware/sensors/lidar/livox/cpp/main.cpp index 3c7cf5ce17..a40e09e51e 100644 --- a/dimos/hardware/sensors/lidar/livox/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/livox/cpp/main.cpp @@ -7,8 +7,6 @@ // Usage: ./mid360_native --pointcloud --imu [--host_ip ] [--lidar_ip ] ... #include -#include -#include #include #include @@ -21,6 +19,8 @@ #include #include +#include "livox_sdk_config.hpp" + #include "dimos_native_module.hpp" #include "geometry_msgs/Quaternion.hpp" @@ -31,13 +31,10 @@ #include "std_msgs/Header.hpp" #include "std_msgs/Time.hpp" -// Gravity constant for converting accelerometer data from g to m/s^2 -static constexpr double GRAVITY_MS2 = 9.80665; - -// Livox data_type values (not provided as named constants in SDK2 header) -static constexpr uint8_t DATA_TYPE_IMU = 0x00; -static constexpr uint8_t DATA_TYPE_CARTESIAN_HIGH = 0x01; -static constexpr uint8_t DATA_TYPE_CARTESIAN_LOW = 0x02; +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 @@ -239,74 +236,6 @@ static void signal_handler(int /*sig*/) { g_running.store(false); } -// --------------------------------------------------------------------------- -// SDK config file generation -// --------------------------------------------------------------------------- - -struct SdkPorts { - int cmd_data = 56100; - int push_msg = 56200; - int point_data = 56300; - int imu_data = 56400; - int log_data = 56500; - int host_cmd_data = 56101; - int host_push_msg = 56201; - int host_point_data = 56301; - int host_imu_data = 56401; - int host_log_data = 56501; -}; - -static std::string write_sdk_config(const std::string& host_ip, - const std::string& lidar_ip, - const SdkPorts& ports) { - char tmpl[] = "/tmp/livox_mid360_XXXXXX"; - int fd = mkstemp(tmpl); - if (fd < 0) { - perror("mkstemp"); - return ""; - } - - // fdopen takes ownership of fd — fclose will close it - FILE* fp = fdopen(fd, "w"); - if (!fp) { - perror("fdopen"); - close(fd); - return ""; - } - - fprintf(fp, - "{\n" - " \"MID360\": {\n" - " \"lidar_net_info\": {\n" - " \"cmd_data_port\": %d,\n" - " \"push_msg_port\": %d,\n" - " \"point_data_port\": %d,\n" - " \"imu_data_port\": %d,\n" - " \"log_data_port\": %d\n" - " },\n" - " \"host_net_info\": [\n" - " {\n" - " \"host_ip\": \"%s\",\n" - " \"multicast_ip\": \"224.1.1.5\",\n" - " \"cmd_data_port\": %d,\n" - " \"push_msg_port\": %d,\n" - " \"point_data_port\": %d,\n" - " \"imu_data_port\": %d,\n" - " \"log_data_port\": %d\n" - " }\n" - " ]\n" - " }\n" - "}\n", - ports.cmd_data, ports.push_msg, ports.point_data, - ports.imu_data, ports.log_data, - host_ip.c_str(), - ports.host_cmd_data, ports.host_push_msg, ports.host_point_data, - ports.host_imu_data, ports.host_log_data); - fclose(fp); - - return tmpl; -} - // --------------------------------------------------------------------------- // Main // --------------------------------------------------------------------------- @@ -331,7 +260,7 @@ int main(int argc, char** argv) { g_imu_frame_id = mod.arg("imu_frame_id", "imu_link"); // SDK network ports (configurable for multi-sensor setups) - SdkPorts ports; + livox_common::SdkPorts ports; ports.cmd_data = mod.arg_int("cmd_data_port", 56100); ports.push_msg = mod.arg_int("push_msg_port", 56200); ports.point_data = mod.arg_int("point_data_port", 56300); @@ -361,17 +290,8 @@ int main(int argc, char** argv) { } g_lcm = &lcm; - // Write SDK config - std::string config_path = write_sdk_config(host_ip, lidar_ip, ports); - if (config_path.empty()) { - fprintf(stderr, "Error: failed to write SDK config\n"); - return 1; - } - - // Init Livox SDK - if (!LivoxLidarSdkInit(config_path.c_str(), host_ip.c_str())) { - fprintf(stderr, "Error: LivoxLidarSdkInit failed\n"); - std::remove(config_path.c_str()); + // Init Livox SDK (in-memory config, no temp files) + if (!livox_common::init_livox_sdk(host_ip, lidar_ip, ports)) { return 1; } @@ -386,7 +306,6 @@ int main(int argc, char** argv) { if (!LivoxLidarSdkStart()) { fprintf(stderr, "Error: LivoxLidarSdkStart failed\n"); LivoxLidarSdkUninit(); - std::remove(config_path.c_str()); return 1; } @@ -429,7 +348,6 @@ int main(int argc, char** argv) { // Cleanup printf("[mid360] Shutting down...\n"); LivoxLidarSdkUninit(); - std::remove(config_path.c_str()); g_lcm = nullptr; printf("[mid360] Done.\n"); From af4e732fbebe15ad8ed13fa1be842b52400e1b3c Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Fri, 13 Feb 2026 21:23:17 +0800 Subject: [PATCH 30/48] auto-fetch FAST-LIO-NON-ROS when FASTLIO_DIR not set Replace hardcoded $HOME path fallback with FetchContent auto-clone from GitHub. Nix builds unaffected (always pass -DFASTLIO_DIR). Local dev without args now auto-fetches instead of failing or using wrong path. --- .../sensors/lidar/fastlio2/cpp/CMakeLists.txt | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt b/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt index 785462d1f8..39f9f90443 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt @@ -34,9 +34,16 @@ endif() # Fetch dependencies include(FetchContent) -# FAST-LIO-NON-ROS (passed via -DFASTLIO_DIR= or defaults to local checkout) +# FAST-LIO-NON-ROS (pass -DFASTLIO_DIR= or auto-fetched from GitHub) if(NOT FASTLIO_DIR) - set(FASTLIO_DIR $ENV{HOME}/coding/FAST-LIO-NON-ROS) + 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 From a1ce9fc75366ede23426bab7ffa9e13d0e9dd106 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Fri, 13 Feb 2026 22:31:56 +0800 Subject: [PATCH 31/48] move dimos_native_module.hpp to common/ to eliminate duplication The header was duplicated verbatim in both livox/cpp/ and fastlio2/cpp/. Both CMakeLists and Nix flakes already include common/ via LIVOX_COMMON_DIR. --- .../cpp => common}/dimos_native_module.hpp | 0 .../lidar/livox/cpp/dimos_native_module.hpp | 64 ------------------- 2 files changed, 64 deletions(-) rename dimos/hardware/sensors/lidar/{fastlio2/cpp => common}/dimos_native_module.hpp (100%) delete mode 100644 dimos/hardware/sensors/lidar/livox/cpp/dimos_native_module.hpp diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/dimos_native_module.hpp b/dimos/hardware/sensors/lidar/common/dimos_native_module.hpp similarity index 100% rename from dimos/hardware/sensors/lidar/fastlio2/cpp/dimos_native_module.hpp rename to dimos/hardware/sensors/lidar/common/dimos_native_module.hpp diff --git a/dimos/hardware/sensors/lidar/livox/cpp/dimos_native_module.hpp b/dimos/hardware/sensors/lidar/livox/cpp/dimos_native_module.hpp deleted file mode 100644 index 73ee618c34..0000000000 --- a/dimos/hardware/sensors/lidar/livox/cpp/dimos_native_module.hpp +++ /dev/null @@ -1,64 +0,0 @@ -// Copyright 2026 Dimensional Inc. -// SPDX-License-Identifier: Apache-2.0 -// -// Lightweight header-only helper for dimos NativeModule C++ binaries. -// Parses -- CLI args passed by the Python NativeModule wrapper. - -#pragma once - -#include -#include -#include - -namespace dimos { - -class NativeModule { -public: - NativeModule(int argc, char** argv) { - for (int i = 1; i < argc; ++i) { - std::string arg(argv[i]); - if (arg.size() > 2 && arg[0] == '-' && arg[1] == '-' && i + 1 < argc) { - args_[arg.substr(2)] = argv[++i]; - } - } - } - - /// Get the full LCM channel string for a declared port. - /// Format is "#", e.g. "/pointcloud#sensor_msgs.PointCloud2". - /// This is the exact channel name used by Python LCMTransport subscribers. - const std::string& topic(const std::string& port) const { - auto it = args_.find(port); - if (it == args_.end()) { - throw std::runtime_error("NativeModule: no topic for port '" + port + "'"); - } - return it->second; - } - - /// Get a string arg value, or a default if not present. - std::string arg(const std::string& key, const std::string& default_val = "") const { - auto it = args_.find(key); - return it != args_.end() ? it->second : default_val; - } - - /// Get a float arg value, or a default if not present. - float arg_float(const std::string& key, float default_val = 0.0f) const { - auto it = args_.find(key); - return it != args_.end() ? std::stof(it->second) : default_val; - } - - /// Get an int arg value, or a default if not present. - int arg_int(const std::string& key, int default_val = 0) const { - auto it = args_.find(key); - return it != args_.end() ? std::stoi(it->second) : default_val; - } - - /// Check if a port/arg was provided. - bool has(const std::string& key) const { - return args_.count(key) > 0; - } - -private: - std::map args_; -}; - -} // namespace dimos From bbe220e40be253d7224fe578ec5d8dac90f6c084 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Fri, 13 Feb 2026 22:36:03 +0800 Subject: [PATCH 32/48] rename Mid360Module to Mid360, use lidar port in Lidar spec MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Rename Mid360Module → Mid360 across module, blueprints, tests, docs - Lidar spec now declares lidar: Out[PointCloud2] directly instead of inheriting from Pointcloud - Rename pointcloud port to lidar in Mid360 and LidarModule --- dimos/hardware/sensors/lidar/livox/cpp/README.md | 8 ++++---- .../hardware/sensors/lidar/livox/livox_blueprints.py | 4 ++-- dimos/hardware/sensors/lidar/livox/module.py | 12 ++++++------ dimos/hardware/sensors/lidar/livox/test_spec.py | 6 +++--- dimos/hardware/sensors/lidar/module.py | 4 ++-- dimos/spec/perception.py | 4 ++-- 6 files changed, 19 insertions(+), 19 deletions(-) diff --git a/dimos/hardware/sensors/lidar/livox/cpp/README.md b/dimos/hardware/sensors/lidar/livox/cpp/README.md index 9affadf03f..4db5248ce1 100644 --- a/dimos/hardware/sensors/lidar/livox/cpp/README.md +++ b/dimos/hardware/sensors/lidar/livox/cpp/README.md @@ -65,14 +65,14 @@ This persists across reboots. The lidar defaults to `192.168.1.155`. ## Usage -Normally launched by `Mid360Module` via the NativeModule framework: +Normally launched by `Mid360` via the NativeModule framework: ```python -from dimos.hardware.sensors.lidar.livox.module import Mid360Module +from dimos.hardware.sensors.lidar.livox.module import Mid360 from dimos.core.blueprints import autoconnect autoconnect( - Mid360Module.blueprint(host_ip="192.168.1.5"), + Mid360.blueprint(host_ip="192.168.1.5"), SomeConsumer.blueprint(), ).build().loop() ``` @@ -111,4 +111,4 @@ lcm-spy | `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 (`Mid360Module`) | +| `../module.py` | Python NativeModule wrapper (`Mid360`) | diff --git a/dimos/hardware/sensors/lidar/livox/livox_blueprints.py b/dimos/hardware/sensors/lidar/livox/livox_blueprints.py index b95d9fccd5..0cda912b73 100644 --- a/dimos/hardware/sensors/lidar/livox/livox_blueprints.py +++ b/dimos/hardware/sensors/lidar/livox/livox_blueprints.py @@ -13,10 +13,10 @@ # limitations under the License. from dimos.core.blueprints import autoconnect -from dimos.hardware.sensors.lidar.livox.module import Mid360Module +from dimos.hardware.sensors.lidar.livox.module import Mid360 from dimos.visualization.rerun.bridge import rerun_bridge mid360 = autoconnect( - Mid360Module.blueprint(), + 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 index d6f35e77b5..1b4f580b5b 100644 --- a/dimos/hardware/sensors/lidar/livox/module.py +++ b/dimos/hardware/sensors/lidar/livox/module.py @@ -20,11 +20,11 @@ Usage:: - from dimos.hardware.sensors.lidar.livox.module import Mid360Module + from dimos.hardware.sensors.lidar.livox.module import Mid360 from dimos.core.blueprints import autoconnect autoconnect( - Mid360Module.blueprint(host_ip="192.168.1.5"), + Mid360.blueprint(host_ip="192.168.1.5"), SomeConsumer.blueprint(), ).build().loop() """ @@ -68,7 +68,7 @@ class Mid360Config(NativeModuleConfig): host_log_data_port: int = 56501 -class Mid360Module(NativeModule, perception.Lidar, perception.IMU): +class Mid360(NativeModule, perception.Lidar, perception.IMU): """Livox Mid-360 LiDAR module backed by a native C++ binary. Ports: @@ -79,14 +79,14 @@ class Mid360Module(NativeModule, perception.Lidar, perception.IMU): config: Mid360Config default_config = Mid360Config - pointcloud: Out[PointCloud2] + lidar: Out[PointCloud2] imu: Out[Imu] -mid360_module = Mid360Module.blueprint +mid360_module = Mid360.blueprint __all__ = [ + "Mid360", "Mid360Config", - "Mid360Module", "mid360_module", ] diff --git a/dimos/hardware/sensors/lidar/livox/test_spec.py b/dimos/hardware/sensors/lidar/livox/test_spec.py index 5f78c0922a..6c41d99c22 100644 --- a/dimos/hardware/sensors/lidar/livox/test_spec.py +++ b/dimos/hardware/sensors/lidar/livox/test_spec.py @@ -16,14 +16,14 @@ from __future__ import annotations -from dimos.hardware.sensors.lidar.livox.module import Mid360Module +from dimos.hardware.sensors.lidar.livox.module import Mid360 from dimos.spec import perception from dimos.spec.utils import assert_implements_protocol def test_livox_module_implements_lidar_spec() -> None: - assert_implements_protocol(Mid360Module, perception.Lidar) + assert_implements_protocol(Mid360, perception.Lidar) def test_livox_module_implements_imu_spec() -> None: - assert_implements_protocol(Mid360Module, perception.IMU) + assert_implements_protocol(Mid360, perception.IMU) diff --git a/dimos/hardware/sensors/lidar/module.py b/dimos/hardware/sensors/lidar/module.py index 8876d74368..f801ba3c7f 100644 --- a/dimos/hardware/sensors/lidar/module.py +++ b/dimos/hardware/sensors/lidar/module.py @@ -53,7 +53,7 @@ class LidarModule(Module[LidarModuleConfig], perception.Lidar): Publishes PointCloud2 messages and TF transforms. """ - pointcloud: Out[PointCloud2] + lidar: Out[PointCloud2] hardware: LidarHardware[Any] @@ -71,7 +71,7 @@ def start(self) -> None: stream = stream.pipe(ops.sample(1.0 / self.config.frequency)) self._disposables.add( - stream.subscribe(lambda pc: self.pointcloud.publish(pc)), + stream.subscribe(lambda pc: self.lidar.publish(pc)), ) self._disposables.add( diff --git a/dimos/spec/perception.py b/dimos/spec/perception.py index 33e3d1dc97..7b579728be 100644 --- a/dimos/spec/perception.py +++ b/dimos/spec/perception.py @@ -40,7 +40,7 @@ class IMU(Protocol): imu: Out[Imu] -class Lidar(Pointcloud, Protocol): +class Lidar(Protocol): """LiDAR sensor providing point clouds.""" - pass + lidar: Out[PointCloud2] From b41986c1fb217b0e99efab4cb2302364e6a00245 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Fri, 13 Feb 2026 22:49:28 +0800 Subject: [PATCH 33/48] =?UTF-8?q?rename=20spec=20ports:=20pointcloud?= =?UTF-8?q?=E2=86=92lidar,=20global=5Fpointcloud=E2=86=92global=5Fmap?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Lidar spec now requires `lidar: Out[PointCloud2]` (decoupled from Pointcloud) - GlobalPointcloud spec (renamed from Global3DMap) requires `global_map` - spec/map.py renamed to spec/mapping.py - Deleted unused LidarModule wrapper - FastLio2 now declares spec conformance (Lidar, Odometry, GlobalPointcloud) - Fixed assert_implements_protocol to ignore annotations inherited from protocol bases, preventing tautological checks - Fixed NameError in perception.Odometry (was self-referencing) --- dimos/core/test_native_module.py | 1 + .../hardware/sensors/lidar/fastlio2/module.py | 3 +- .../sensors/lidar/fastlio2/test_spec.py | 14 +-- dimos/hardware/sensors/lidar/livox/module.py | 7 +- dimos/hardware/sensors/lidar/module.py | 107 ------------------ dimos/hardware/sensors/lidar/test_spec.py | 34 ------ dimos/navigation/rosnav.py | 30 ++--- dimos/robot/all_blueprints.py | 1 - dimos/spec/__init__.py | 4 +- dimos/spec/{map.py => mapping.py} | 4 +- dimos/spec/perception.py | 5 + dimos/spec/utils.py | 18 ++- 12 files changed, 51 insertions(+), 177 deletions(-) delete mode 100644 dimos/hardware/sensors/lidar/module.py delete mode 100644 dimos/hardware/sensors/lidar/test_spec.py rename dimos/spec/{map.py => mapping.py} (92%) diff --git a/dimos/core/test_native_module.py b/dimos/core/test_native_module.py index f4a34346c8..a6c55047e9 100644 --- a/dimos/core/test_native_module.py +++ b/dimos/core/test_native_module.py @@ -136,6 +136,7 @@ def test_manual(dimos_cluster, args_file) -> None: } +@pytest.mark.heavy def test_autoconnect(args_file) -> None: """autoconnect passes correct topic args to the native subprocess.""" blueprint = autoconnect( diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py index ea2546f40e..5500de8f9c 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -37,6 +37,7 @@ from dimos.core.native_module import NativeModule, NativeModuleConfig from dimos.msgs.nav_msgs.Odometry import Odometry # noqa: TC001 from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 # noqa: TC001 +from dimos.spec import mapping, perception _DEFAULT_EXECUTABLE = str(Path(__file__).parent / "cpp" / "result" / "bin" / "fastlio2_native") _CONFIG_DIR = Path(__file__).parent / "config" @@ -105,7 +106,7 @@ def __post_init__(self) -> None: self.config_path = str(path.resolve()) -class FastLio2(NativeModule): +class FastLio2(NativeModule, perception.Lidar, perception.Odometry, mapping.GlobalPointcloud): """FAST-LIO2 SLAM module with integrated Livox Mid-360 driver. Ports: diff --git a/dimos/hardware/sensors/lidar/fastlio2/test_spec.py b/dimos/hardware/sensors/lidar/fastlio2/test_spec.py index fdf39fe070..fbc8a3a065 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/test_spec.py +++ b/dimos/hardware/sensors/lidar/fastlio2/test_spec.py @@ -17,13 +17,11 @@ from __future__ import annotations from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 +from dimos.spec import mapping, perception +from dimos.spec.utils import assert_implements_protocol -def test_fastlio2_has_lidar_port() -> None: - hints = FastLio2.__annotations__ - assert "lidar" in hints - - -def test_fastlio2_has_odometry_port() -> None: - hints = FastLio2.__annotations__ - assert "odometry" in hints +def test_fastlio2_implements_spec() -> None: + assert_implements_protocol(FastLio2, perception.Lidar) + assert_implements_protocol(FastLio2, perception.Odometry) + assert_implements_protocol(FastLio2, mapping.GlobalPointcloud) diff --git a/dimos/hardware/sensors/lidar/livox/module.py b/dimos/hardware/sensors/lidar/livox/module.py index 1b4f580b5b..2a8a199fb6 100644 --- a/dimos/hardware/sensors/lidar/livox/module.py +++ b/dimos/hardware/sensors/lidar/livox/module.py @@ -14,12 +14,7 @@ """Python NativeModule wrapper for the C++ Livox Mid-360 driver. -Declares the same ports as LivoxLidarModule (pointcloud, imu) but delegates -all real work to the ``mid360_native`` C++ binary, which talks directly to -the Livox SDK2 C API and publishes on LCM. - Usage:: - from dimos.hardware.sensors.lidar.livox.module import Mid360 from dimos.core.blueprints import autoconnect @@ -72,7 +67,7 @@ class Mid360(NativeModule, perception.Lidar, perception.IMU): """Livox Mid-360 LiDAR module backed by a native C++ binary. Ports: - pointcloud (Out[PointCloud2]): Point cloud frames at configured frequency. + lidar (Out[PointCloud2]): Point cloud frames at configured frequency. imu (Out[Imu]): IMU data at ~200 Hz (if enabled). """ diff --git a/dimos/hardware/sensors/lidar/module.py b/dimos/hardware/sensors/lidar/module.py deleted file mode 100644 index f801ba3c7f..0000000000 --- a/dimos/hardware/sensors/lidar/module.py +++ /dev/null @@ -1,107 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""LiDAR module wrappers that convert LidarHardware observables into module streams.""" - -from collections.abc import Callable -from dataclasses import dataclass, field -import time -from typing import Any - -import reactivex as rx -from reactivex import operators as ops - -from dimos.core import Module, Out, rpc -from dimos.core.module import ModuleConfig -from dimos.hardware.sensors.lidar.spec import LidarHardware -from dimos.msgs.geometry_msgs import Quaternion, Transform, Vector3 -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.spec import perception - - -def default_lidar_transform() -> Transform: - return Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="base_link", - child_frame_id="lidar_link", - ) - - -@dataclass -class LidarModuleConfig(ModuleConfig): - frame_id: str = "lidar_link" - transform: Transform | None = field(default_factory=default_lidar_transform) - hardware: Callable[[], LidarHardware[Any]] | LidarHardware[Any] | None = None - frequency: float = 0.0 # Hz, 0 means no limit - - -class LidarModule(Module[LidarModuleConfig], perception.Lidar): - """Generic LiDAR module — pointcloud only. - - Publishes PointCloud2 messages and TF transforms. - """ - - lidar: Out[PointCloud2] - - hardware: LidarHardware[Any] - - config: LidarModuleConfig - default_config = LidarModuleConfig - - @rpc - def start(self) -> None: - super().start() - self._init_hardware() - - stream = self.hardware.pointcloud_stream() - - if self.config.frequency > 0: - stream = stream.pipe(ops.sample(1.0 / self.config.frequency)) - - self._disposables.add( - stream.subscribe(lambda pc: self.lidar.publish(pc)), - ) - - self._disposables.add( - rx.interval(1.0).subscribe(lambda _: self._publish_tf()), - ) - - def _init_hardware(self) -> None: - if callable(self.config.hardware): - self.hardware = self.config.hardware() - else: - self.hardware = self.config.hardware # type: ignore[assignment] - - def _publish_tf(self) -> None: - if not self.config.transform: - return - ts = time.time() - lidar_link = self.config.transform - lidar_link.ts = ts - self.tf.publish(lidar_link) - - def stop(self) -> None: - if self.hardware and hasattr(self.hardware, "stop"): - self.hardware.stop() - super().stop() - - -lidar_module = LidarModule.blueprint - -__all__ = [ - "LidarModule", - "LidarModuleConfig", - "lidar_module", -] diff --git a/dimos/hardware/sensors/lidar/test_spec.py b/dimos/hardware/sensors/lidar/test_spec.py deleted file mode 100644 index 14d7a2e61e..0000000000 --- a/dimos/hardware/sensors/lidar/test_spec.py +++ /dev/null @@ -1,34 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Spec compliance tests for LidarModule.""" - -from __future__ import annotations - -import typing - -from dimos.hardware.sensors.lidar.module import LidarModule -from dimos.spec import perception -from dimos.spec.utils import assert_implements_protocol - - -def test_lidar_module_implements_lidar_spec() -> None: - assert_implements_protocol(LidarModule, perception.Lidar) - - -def test_lidar_spec_does_not_require_imu() -> None: - """Not all LiDARs have IMU — Lidar spec should only require pointcloud.""" - hints = typing.get_type_hints(perception.Lidar, include_extras=True) - assert "pointcloud" in hints - assert "imu" not in hints diff --git a/dimos/navigation/rosnav.py b/dimos/navigation/rosnav.py index 2aa492a509..9f40a60198 100644 --- a/dimos/navigation/rosnav.py +++ b/dimos/navigation/rosnav.py @@ -59,14 +59,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 @@ -75,7 +75,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] @@ -86,7 +86,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] @@ -98,7 +98,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 @@ -115,7 +115,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() @@ -138,10 +138,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}"), ) ) @@ -151,7 +151,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) @@ -174,8 +174,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" @@ -388,7 +388,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}") @@ -404,7 +404,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) @@ -415,7 +415,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 cf6a644da2..294d2a19fc 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -96,7 +96,6 @@ "human_input": "dimos.agents.cli.human", "joint_trajectory_controller": "dimos.manipulation.control.trajectory_controller.joint_trajectory_controller", "keyboard_teleop": "dimos.robot.unitree.keyboard_teleop", - "lidar_module": "dimos.hardware.sensors.lidar.module", "llm_agent": "dimos.agents.agent", "manipulation_module": "dimos.manipulation.manipulation_module", "mapper": "dimos.robot.unitree.type.map", diff --git a/dimos/spec/__init__.py b/dimos/spec/__init__.py index 03c1024d12..4e070f5dd1 100644 --- a/dimos/spec/__init__.py +++ b/dimos/spec/__init__.py @@ -1,13 +1,13 @@ from dimos.spec.control import LocalPlanner -from dimos.spec.map import Global3DMap, GlobalCostmap, GlobalMap +from dimos.spec.mapping import GlobalCostmap, GlobalMap, 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 92% rename from dimos/spec/map.py rename to dimos/spec/mapping.py index 438b77a7a6..01af4cb875 100644 --- a/dimos/spec/map.py +++ b/dimos/spec/mapping.py @@ -19,8 +19,8 @@ from dimos.msgs.sensor_msgs import PointCloud2 -class Global3DMap(Protocol): - global_pointcloud: Out[PointCloud2] +class GlobalPointcloud(Protocol): + global_map: Out[PointCloud2] class GlobalMap(Protocol): diff --git a/dimos/spec/perception.py b/dimos/spec/perception.py index 7b579728be..1cecdb4d2f 100644 --- a/dimos/spec/perception.py +++ b/dimos/spec/perception.py @@ -15,6 +15,7 @@ from typing import Protocol from dimos.core import Out +from dimos.msgs.nav_msgs.Odometry import Odometry as OdometryMsg from dimos.msgs.sensor_msgs import CameraInfo, Image as ImageMsg, Imu, PointCloud2 @@ -40,6 +41,10 @@ class IMU(Protocol): imu: Out[Imu] +class Odometry(Protocol): + odometry: Out[OdometryMsg] + + class Lidar(Protocol): """LiDAR sensor providing point clouds.""" diff --git a/dimos/spec/utils.py b/dimos/spec/utils.py index 31b6d3e5b5..0050a0b0c1 100644 --- a/dimos/spec/utils.py +++ b/dimos/spec/utils.py @@ -100,11 +100,27 @@ def foo(self) -> int: ... return isinstance(obj, strict_proto) +def _own_hints(cls: type) -> dict[str, Any]: + """Collect type hints from cls and its non-protocol bases only.""" + hints: dict[str, Any] = {} + for base in reversed(cls.__mro__): + if base is object or is_protocol(base): + continue + base_hints = typing.get_type_hints(base, include_extras=True) + # Only include annotations defined directly on this base + for name in base.__annotations__: + if name in base_hints: + hints[name] = base_hints[name] + return hints + + def assert_implements_protocol(cls: type, protocol: type) -> None: """Assert that cls has all annotations required by a Protocol. Works with any Protocol (not just Spec subclasses). Checks that every annotation defined by the protocol is present on cls with a matching type. + Ignores annotations inherited from protocol bases so that inheriting from + a protocol doesn't automatically satisfy the check. Example: class MyProto(Protocol): @@ -116,7 +132,7 @@ class Good: assert_implements_protocol(Good, MyProto) # passes """ proto_hints = typing.get_type_hints(protocol, include_extras=True) - cls_hints = typing.get_type_hints(cls, include_extras=True) + cls_hints = _own_hints(cls) for name, expected_type in proto_hints.items(): assert name in cls_hints, f"{cls.__name__} missing '{name}' required by {protocol.__name__}" From 07673f821ce5ae9f7a6324925db21036f66628e8 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Fri, 13 Feb 2026 23:06:27 +0800 Subject: [PATCH 34/48] cleanup --- .gitignore | 2 +- .../sensors/lidar/fastlio2/cpp/README.md | 2 +- .../sensors/lidar/fastlio2/cpp/main.cpp | 26 +++++++------- .../hardware/sensors/lidar/fastlio2/module.py | 34 +++++++++++++------ .../hardware/sensors/lidar/livox/cpp/main.cpp | 23 +++++++------ dimos/hardware/sensors/lidar/livox/module.py | 34 +++++++++++++------ dimos/hardware/sensors/lidar/livox/ports.py | 31 +++++++++++++++++ dimos/msgs/nav_msgs/Odometry.py | 7 +--- dimos/spec/__init__.py | 3 +- dimos/spec/mapping.py | 4 --- 10 files changed, 105 insertions(+), 61 deletions(-) create mode 100644 dimos/hardware/sensors/lidar/livox/ports.py diff --git a/.gitignore b/.gitignore index 3da8726cd4..e96b8bc0e8 100644 --- a/.gitignore +++ b/.gitignore @@ -65,6 +65,6 @@ yolo11n.pt *mobileclip* /results -result +**/cpp/result /assets/teleop_certs/ diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/README.md b/dimos/hardware/sensors/lidar/fastlio2/cpp/README.md index c93ccfcce8..da6e7c8803 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/README.md +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/README.md @@ -38,7 +38,7 @@ cmake --install build Binary lands at `result/bin/fastlio2_native` (same location as nix). -If `-DFASTLIO_DIR` is omitted, CMake defaults to `~/coding/FAST-LIO-NON-ROS`. +If `-DFASTLIO_DIR` is omitted, CMake auto-fetches FAST-LIO-NON-ROS from GitHub. ## Network setup diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp index b9953b2bd3..88099510a7 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp @@ -73,9 +73,6 @@ static std::vector g_accumulated_points; static uint64_t g_frame_start_ns = 0; static bool g_frame_has_timestamp = false; -// Track whether FastLio has produced a valid result this cycle -static std::atomic g_has_new_result{false}; - // --------------------------------------------------------------------------- // Helpers // --------------------------------------------------------------------------- @@ -344,18 +341,19 @@ int main(int argc, char** argv) { 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 + // SDK network ports (defaults from SdkPorts struct in livox_sdk_config.hpp) livox_common::SdkPorts ports; - ports.cmd_data = mod.arg_int("cmd_data_port", 56100); - ports.push_msg = mod.arg_int("push_msg_port", 56200); - ports.point_data = mod.arg_int("point_data_port", 56300); - ports.imu_data = mod.arg_int("imu_data_port", 56400); - ports.log_data = mod.arg_int("log_data_port", 56500); - ports.host_cmd_data = mod.arg_int("host_cmd_data_port", 56101); - ports.host_push_msg = mod.arg_int("host_push_msg_port", 56201); - ports.host_point_data = mod.arg_int("host_point_data_port", 56301); - ports.host_imu_data = mod.arg_int("host_imu_data_port", 56401); - ports.host_log_data = mod.arg_int("host_log_data_port", 56501); + 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", diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py index 5500de8f9c..8c641e75b8 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -35,6 +35,18 @@ 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 @@ -80,17 +92,17 @@ class FastLio2Config(NativeModuleConfig): # C++ binary reads YAML directly via yaml-cpp config: str = "mid360.yaml" - # SDK port configuration (for multi-sensor setups) - cmd_data_port: int = 56100 - push_msg_port: int = 56200 - point_data_port: int = 56300 - imu_data_port: int = 56400 - log_data_port: int = 56500 - host_cmd_data_port: int = 56101 - host_push_msg_port: int = 56201 - host_point_data_port: int = 56301 - host_imu_data_port: int = 56401 - host_log_data_port: int = 56501 + # 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 diff --git a/dimos/hardware/sensors/lidar/livox/cpp/main.cpp b/dimos/hardware/sensors/lidar/livox/cpp/main.cpp index a40e09e51e..fda429e8fd 100644 --- a/dimos/hardware/sensors/lidar/livox/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/livox/cpp/main.cpp @@ -259,18 +259,19 @@ int main(int argc, char** argv) { g_frame_id = mod.arg("frame_id", "lidar_link"); g_imu_frame_id = mod.arg("imu_frame_id", "imu_link"); - // SDK network ports (configurable for multi-sensor setups) + // SDK network ports (defaults from SdkPorts struct in livox_sdk_config.hpp) livox_common::SdkPorts ports; - ports.cmd_data = mod.arg_int("cmd_data_port", 56100); - ports.push_msg = mod.arg_int("push_msg_port", 56200); - ports.point_data = mod.arg_int("point_data_port", 56300); - ports.imu_data = mod.arg_int("imu_data_port", 56400); - ports.log_data = mod.arg_int("log_data_port", 56500); - ports.host_cmd_data = mod.arg_int("host_cmd_data_port", 56101); - ports.host_push_msg = mod.arg_int("host_push_msg_port", 56201); - ports.host_point_data = mod.arg_int("host_point_data_port", 56301); - ports.host_imu_data = mod.arg_int("host_imu_data_port", 56401); - ports.host_log_data = mod.arg_int("host_log_data_port", 56501); + 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] pointcloud topic: %s\n", g_pointcloud_topic.c_str()); diff --git a/dimos/hardware/sensors/lidar/livox/module.py b/dimos/hardware/sensors/lidar/livox/module.py index 2a8a199fb6..411ed76ba8 100644 --- a/dimos/hardware/sensors/lidar/livox/module.py +++ b/dimos/hardware/sensors/lidar/livox/module.py @@ -31,6 +31,18 @@ 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 @@ -50,17 +62,17 @@ class Mid360Config(NativeModuleConfig): frame_id: str = "lidar_link" imu_frame_id: str = "imu_link" - # SDK port configuration (match defaults in LivoxMid360Config) - cmd_data_port: int = 56100 - push_msg_port: int = 56200 - point_data_port: int = 56300 - imu_data_port: int = 56400 - log_data_port: int = 56500 - host_cmd_data_port: int = 56101 - host_push_msg_port: int = 56201 - host_point_data_port: int = 56301 - host_imu_data_port: int = 56401 - host_log_data_port: int = 56501 + # 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): 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 14d26455f8..7bcad24e93 100644 --- a/dimos/msgs/nav_msgs/Odometry.py +++ b/dimos/msgs/nav_msgs/Odometry.py @@ -39,11 +39,6 @@ ) -def sec_nsec(ts): # type: ignore[no-untyped-def] - s = int(ts) - return [s, int((ts - s) * 1_000_000_000)] - - class Odometry(LCMOdometry, Timestamped): # type: ignore[misc] pose: PoseWithCovariance twist: TwistWithCovariance @@ -276,7 +271,7 @@ def lcm_encode(self) -> bytes: lcm_msg = LCMOdometry() # Set header - [lcm_msg.header.stamp.sec, lcm_msg.header.stamp.nsec] = sec_nsec(self.ts) # type: ignore[no-untyped-call] + [lcm_msg.header.stamp.sec, lcm_msg.header.stamp.nsec] = self.ros_timestamp() lcm_msg.header.frame_id = self.frame_id lcm_msg.child_frame_id = self.child_frame_id diff --git a/dimos/spec/__init__.py b/dimos/spec/__init__.py index 4e070f5dd1..1423bec9a1 100644 --- a/dimos/spec/__init__.py +++ b/dimos/spec/__init__.py @@ -1,12 +1,11 @@ from dimos.spec.control import LocalPlanner -from dimos.spec.mapping import GlobalCostmap, GlobalMap, GlobalPointcloud +from dimos.spec.mapping import GlobalCostmap, GlobalPointcloud from dimos.spec.nav import Nav from dimos.spec.perception import Camera, Image, Pointcloud __all__ = [ "Camera", "GlobalCostmap", - "GlobalMap", "GlobalPointcloud", "Image", "LocalPlanner", diff --git a/dimos/spec/mapping.py b/dimos/spec/mapping.py index 01af4cb875..f8e7e1a04f 100644 --- a/dimos/spec/mapping.py +++ b/dimos/spec/mapping.py @@ -23,9 +23,5 @@ class GlobalPointcloud(Protocol): global_map: Out[PointCloud2] -class GlobalMap(Protocol): - global_map: Out[OccupancyGrid] - - class GlobalCostmap(Protocol): global_costmap: Out[OccupancyGrid] From 77855d963e611f145cacae8225bdb75db630cb92 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Fri, 13 Feb 2026 23:18:32 +0800 Subject: [PATCH 35/48] small fixes --- .../hardware/sensors/lidar/livox/cpp/main.cpp | 14 +++--- dimos/hardware/sensors/lidar/spec.py | 43 ------------------- pyproject.toml | 2 +- 3 files changed, 8 insertions(+), 51 deletions(-) delete mode 100644 dimos/hardware/sensors/lidar/spec.py diff --git a/dimos/hardware/sensors/lidar/livox/cpp/main.cpp b/dimos/hardware/sensors/lidar/livox/cpp/main.cpp index fda429e8fd..0242de33ec 100644 --- a/dimos/hardware/sensors/lidar/livox/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/livox/cpp/main.cpp @@ -4,7 +4,7 @@ // Livox Mid-360 native module for dimos NativeModule framework. // // Publishes PointCloud2 and Imu messages on LCM topics received via CLI args. -// Usage: ./mid360_native --pointcloud --imu [--host_ip ] [--lidar_ip ] ... +// Usage: ./mid360_native --lidar --imu [--host_ip ] [--lidar_ip ] ... #include @@ -42,7 +42,7 @@ using livox_common::DATA_TYPE_CARTESIAN_LOW; static std::atomic g_running{true}; static lcm::LCM* g_lcm = nullptr; -static std::string g_pointcloud_topic; +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"; @@ -132,7 +132,7 @@ static void publish_pointcloud(const std::vector& xyz, dst[3] = intensity[i]; } - g_lcm->publish(g_pointcloud_topic, &pc); + g_lcm->publish(g_lidar_topic, &pc); } // --------------------------------------------------------------------------- @@ -244,11 +244,11 @@ int main(int argc, char** argv) { dimos::NativeModule mod(argc, argv); // Required: LCM topics for ports - g_pointcloud_topic = mod.has("pointcloud") ? mod.topic("pointcloud") : ""; + g_lidar_topic = mod.has("lidar") ? mod.topic("lidar") : ""; g_imu_topic = mod.has("imu") ? mod.topic("imu") : ""; - if (g_pointcloud_topic.empty()) { - fprintf(stderr, "Error: --pointcloud is required\n"); + if (g_lidar_topic.empty()) { + fprintf(stderr, "Error: --lidar is required\n"); return 1; } @@ -274,7 +274,7 @@ int main(int argc, char** argv) { 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] pointcloud topic: %s\n", g_pointcloud_topic.c_str()); + 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); diff --git a/dimos/hardware/sensors/lidar/spec.py b/dimos/hardware/sensors/lidar/spec.py deleted file mode 100644 index d01b451479..0000000000 --- a/dimos/hardware/sensors/lidar/spec.py +++ /dev/null @@ -1,43 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from abc import ABC, abstractmethod -from typing import Generic, Protocol, TypeVar - -from reactivex.observable import Observable - -from dimos.msgs.sensor_msgs.Imu import Imu -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.protocol.service import Configurable # type: ignore[attr-defined] - - -class LidarConfig(Protocol): - frame_id_prefix: str | None - frequency: float # Hz, point cloud output rate - - -LidarConfigT = TypeVar("LidarConfigT", bound=LidarConfig) - - -class LidarHardware(ABC, Configurable[LidarConfigT], Generic[LidarConfigT]): - """Abstract base class for LiDAR hardware drivers.""" - - @abstractmethod - def pointcloud_stream(self) -> Observable[PointCloud2]: - """Observable stream of point clouds from the LiDAR.""" - pass - - def imu_stream(self) -> Observable[Imu] | None: - """Optional observable stream of IMU data. Returns None if unsupported.""" - return None diff --git a/pyproject.toml b/pyproject.toml index e9842b3349..68b48d7a73 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -74,7 +74,7 @@ lcmspy = "dimos.utils.cli.lcmspy.run_lcmspy:main" foxglove-bridge = "dimos.utils.cli.foxglove_bridge.run_foxglove_bridge:main" skillspy = "dimos.utils.cli.skillspy.skillspy:main" agentspy = "dimos.utils.cli.agentspy.agentspy:main" -humancli = "dimos.utils.cli.human.humanclianim:run" +humancli = "dimos.utils.cli.human.humanclianim:main" dimos = "dimos.robot.cli.dimos:main" rerun-bridge = "dimos.visualization.rerun.bridge:app" doclinks = "dimos.utils.docs.doclinks:main" From ce406c8b12d0f6ddc9e7c0aa01067597eecbd6a5 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Fri, 13 Feb 2026 23:26:48 +0800 Subject: [PATCH 36/48] small fixes 2 --- .../lidar/common/dimos_native_module.hpp | 22 +++++++++++++++++++ .../sensors/lidar/fastlio2/cpp/main.cpp | 19 ++-------------- .../hardware/sensors/lidar/livox/cpp/main.cpp | 19 ++-------------- .../hardware/sensors/lidar/livox/test_spec.py | 2 +- dimos/msgs/sensor_msgs/Imu.py | 7 +----- 5 files changed, 28 insertions(+), 41 deletions(-) diff --git a/dimos/hardware/sensors/lidar/common/dimos_native_module.hpp b/dimos/hardware/sensors/lidar/common/dimos_native_module.hpp index 73ee618c34..cdd5d85914 100644 --- a/dimos/hardware/sensors/lidar/common/dimos_native_module.hpp +++ b/dimos/hardware/sensors/lidar/common/dimos_native_module.hpp @@ -6,10 +6,14 @@ #pragma once +#include #include #include #include +#include "std_msgs/Header.hpp" +#include "std_msgs/Time.hpp" + namespace dimos { class NativeModule { @@ -61,4 +65,22 @@ class NativeModule { 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/fastlio2/cpp/main.cpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp index 88099510a7..60b8d9cdb2 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp @@ -41,8 +41,6 @@ #include "sensor_msgs/Imu.hpp" #include "sensor_msgs/PointCloud2.hpp" #include "sensor_msgs/PointField.hpp" -#include "std_msgs/Header.hpp" -#include "std_msgs/Time.hpp" // FAST-LIO (header-only core, compiled sources linked via CMake) #include "fast_lio.hpp" @@ -77,27 +75,14 @@ static bool g_frame_has_timestamp = false; // Helpers // --------------------------------------------------------------------------- -static std_msgs::Time time_from_seconds(double t) { - std_msgs::Time ts; - ts.sec = static_cast(t); - ts.nsec = static_cast((t - ts.sec) * 1e9); - return ts; -} - static uint64_t get_timestamp_ns(const LivoxLidarEthernetPacket* pkt) { uint64_t ns = 0; std::memcpy(&ns, pkt->timestamp, sizeof(uint64_t)); return ns; } -static std_msgs::Header make_header(const std::string& frame_id, double ts) { - static std::atomic seq{0}; - std_msgs::Header h; - h.seq = seq.fetch_add(1, std::memory_order_relaxed); - h.stamp = time_from_seconds(ts); - h.frame_id = frame_id; - return h; -} +using dimos::time_from_seconds; +using dimos::make_header; // --------------------------------------------------------------------------- // Publish lidar (world-frame point cloud) diff --git a/dimos/hardware/sensors/lidar/livox/cpp/main.cpp b/dimos/hardware/sensors/lidar/livox/cpp/main.cpp index 0242de33ec..cdf083ef3b 100644 --- a/dimos/hardware/sensors/lidar/livox/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/livox/cpp/main.cpp @@ -28,8 +28,6 @@ #include "sensor_msgs/Imu.hpp" #include "sensor_msgs/PointCloud2.hpp" #include "sensor_msgs/PointField.hpp" -#include "std_msgs/Header.hpp" -#include "std_msgs/Time.hpp" using livox_common::GRAVITY_MS2; using livox_common::DATA_TYPE_IMU; @@ -59,27 +57,14 @@ static bool g_frame_has_timestamp = false; // Helpers // --------------------------------------------------------------------------- -static std_msgs::Time time_from_seconds(double t) { - std_msgs::Time ts; - ts.sec = static_cast(t); - ts.nsec = static_cast((t - ts.sec) * 1e9); - return ts; -} - static double get_timestamp_ns(const LivoxLidarEthernetPacket* pkt) { uint64_t ns = 0; std::memcpy(&ns, pkt->timestamp, sizeof(uint64_t)); return static_cast(ns); } -static std_msgs::Header make_header(const std::string& frame_id, double ts) { - static std::atomic seq{0}; - std_msgs::Header h; - h.seq = seq.fetch_add(1, std::memory_order_relaxed); - h.stamp = time_from_seconds(ts); - h.frame_id = frame_id; - return h; -} +using dimos::time_from_seconds; +using dimos::make_header; // --------------------------------------------------------------------------- // Build and publish PointCloud2 diff --git a/dimos/hardware/sensors/lidar/livox/test_spec.py b/dimos/hardware/sensors/lidar/livox/test_spec.py index 6c41d99c22..6e73c8c6b9 100644 --- a/dimos/hardware/sensors/lidar/livox/test_spec.py +++ b/dimos/hardware/sensors/lidar/livox/test_spec.py @@ -1,4 +1,4 @@ -# Copyright 2025-2026 Dimensional Inc. +# Copyright 2026 Dimensional Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/dimos/msgs/sensor_msgs/Imu.py b/dimos/msgs/sensor_msgs/Imu.py index 0fadc3f3f7..7fe03ce03f 100644 --- a/dimos/msgs/sensor_msgs/Imu.py +++ b/dimos/msgs/sensor_msgs/Imu.py @@ -22,11 +22,6 @@ from dimos.types.timestamped import Timestamped -def _sec_nsec(ts: float) -> tuple[int, int]: - s = int(ts) - return s, int((ts - s) * 1_000_000_000) - - class Imu(Timestamped): """IMU sensor message mirroring ROS sensor_msgs/Imu. @@ -58,7 +53,7 @@ def __init__( def lcm_encode(self) -> bytes: msg = LCMImu() - msg.header.stamp.sec, msg.header.stamp.nsec = _sec_nsec(self.ts) + [msg.header.stamp.sec, msg.header.stamp.nsec] = self.ros_timestamp() msg.header.frame_id = self.frame_id msg.orientation.x = self.orientation.x From 3780beb73e4da895710f92b4e08fe0999f05256b Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Sat, 14 Feb 2026 18:58:57 +0800 Subject: [PATCH 37/48] Merge branch 'dev' into feat/livox Resolved conflict in Odometry.py: kept dev's refactored class structure and added to_rerun() method from feat/livox. --- .gitignore | 1 + README.md | 5 +- dimos/agents/__init__.py | 12 - dimos/agents/agent.py | 501 +-- dimos/agents/agent_test_runner.py | 80 + .../{_skill_exports.py => annotation.py} | 12 +- dimos/agents/cli/human.py | 57 - dimos/agents/conftest.py | 142 +- dimos/agents/demo_agent.py | 32 + .../test_can_call_again_on_error[False].json | 34 + .../test_can_call_again_on_error[True].json | 34 + .../fixtures/test_can_call_tool[False].json | 22 + .../fixtures/test_can_call_tool[True].json | 22 + .../test_get_gps_position_for_queries.json | 6 +- dimos/agents/fixtures/test_go_to_object.json | 27 - .../test_go_to_semantic_location.json | 8 +- ...est_how_much_is_124181112_plus_124124.json | 52 - dimos/agents/fixtures/test_image.json | 23 + ...ple_tool_calls_with_multiple_messages.json | 98 + dimos/agents/fixtures/test_pounce.json | 23 +- dimos/agents/fixtures/test_prompt.json | 8 + .../fixtures/test_set_gps_travel_points.json | 23 +- .../test_set_gps_travel_points_multiple.json | 17 +- .../agents/fixtures/test_show_your_love.json | 38 - ...round.json => test_start_exploration.json} | 8 +- dimos/agents/fixtures/test_stop_movement.json | 8 +- .../test_what_do_you_see_in_this_picture.json | 25 - .../fixtures/test_what_is_your_name.json | 8 - dimos/agents/fixtures/test_where_am_i.json | 8 +- dimos/agents/llm_init.py | 62 - dimos/agents/skills/conftest.py | 113 - dimos/agents/skills/demo_calculator_skill.py | 8 +- dimos/agents/skills/demo_google_maps_skill.py | 11 +- dimos/agents/skills/demo_gps_nav.py | 11 +- dimos/agents/skills/demo_skill.py | 11 +- .../skills/google_maps_skill_container.py | 10 +- dimos/agents/skills/gps_nav_skill.py | 10 +- dimos/agents/skills/navigation.py | 22 +- dimos/agents/skills/osm.py | 8 +- dimos/agents/skills/person_follow.py | 10 +- dimos/agents/skills/speak_skill.py | 8 +- .../test_google_maps_skill_container.py | 89 +- dimos/agents/skills/test_gps_nav_skills.py | 74 +- dimos/agents/skills/test_navigation.py | 159 +- .../skills/test_unitree_skill_container.py | 38 +- dimos/agents/spec.py | 264 -- dimos/agents/temp/webcam_agent.py | 151 - dimos/agents/test_agent.py | 349 +- dimos/agents/test_agent_direct.py | 106 - dimos/agents/test_agent_fake.py | 41 - dimos/agents/test_mock_agent.py | 203 - dimos/agents/test_stash_agent.py | 61 - dimos/agents/testing.py | 39 +- dimos/agents/utils.py | 94 + dimos/agents/vlm_agent.py | 56 +- .../agents/{cli/web.py => web_human_input.py} | 0 dimos/conftest.py | 3 + dimos/core/module.py | 46 +- dimos/core/module_coordinator.py | 8 +- dimos/core/rpc_client.py | 8 +- dimos/core/skill_module.py | 37 - dimos/core/stream.py | 2 +- dimos/core/test_blueprints.py | 15 +- dimos/core/test_core.py | 2 +- dimos/core/test_modules.py | 2 +- dimos/core/testing.py | 1 - dimos/core/transport.py | 53 +- dimos/e2e_tests/test_dimos_cli_e2e.py | 7 +- dimos/hardware/sensors/camera/module.py | 20 +- dimos/manipulation/grasping/demo_grasping.py | 6 +- dimos/manipulation/grasping/grasping.py | 9 +- dimos/mapping/osm/demo_osm.py | 11 +- .../pointclouds/test_occupancy_speed.py | 7 +- dimos/memory/embedding.py | 105 + dimos/memory/test_embedding.py | 53 + dimos/memory/timeseries/__init__.py | 41 + dimos/memory/timeseries/base.py | 367 ++ dimos/memory/timeseries/inmemory.py | 119 + dimos/memory/timeseries/legacy.py | 398 ++ dimos/memory/timeseries/pickledir.py | 198 + dimos/memory/timeseries/postgres.py | 312 ++ dimos/memory/timeseries/sqlite.py | 268 ++ dimos/memory/timeseries/test_base.py | 468 +++ dimos/memory/timeseries/test_legacy.py | 48 + dimos/models/embedding/__init__.py | 18 +- dimos/models/embedding/base.py | 26 +- dimos/models/embedding/clip.py | 17 +- dimos/models/embedding/mobileclip.py | 13 +- dimos/models/embedding/treid.py | 16 +- .../msgs/geometry_msgs/PoseWithCovariance.py | 3 +- dimos/msgs/geometry_msgs/Transform.py | 9 +- .../msgs/geometry_msgs/TwistWithCovariance.py | 3 +- dimos/msgs/nav_msgs/Odometry.py | 224 +- dimos/msgs/nav_msgs/test_Odometry.py | 160 +- dimos/msgs/sensor_msgs/Image.py | 7 +- dimos/navigation/rosnav.py | 35 +- .../demo_object_scene_registration.py | 6 +- dimos/perception/detection/module3D.py | 4 +- .../detection/reid/embedding_id_system.py | 2 +- .../temporal_memory/temporal_memory.py | 9 +- dimos/perception/object_scene_registration.py | 10 +- dimos/perception/spatial_perception.py | 4 +- dimos/protocol/mcp/README.md | 2 +- dimos/protocol/mcp/__init__.py | 17 - dimos/protocol/mcp/mcp.py | 70 +- dimos/protocol/mcp/test_mcp_module.py | 216 +- .../pubsub/benchmark/test_benchmark.py | 1 + dimos/protocol/pubsub/benchmark/testdata.py | 69 + dimos/protocol/pubsub/benchmark/type.py | 8 + dimos/protocol/pubsub/impl/ddspubsub.py | 161 + dimos/protocol/rpc/spec.py | 2 + dimos/protocol/service/__init__.py | 2 +- dimos/protocol/service/ddsservice.py | 80 + dimos/protocol/skill/__init__.py | 6 - dimos/protocol/skill/comms.py | 99 - dimos/protocol/skill/coordinator.py | 637 --- dimos/protocol/skill/schema.py | 103 - dimos/protocol/skill/skill.py | 250 -- dimos/protocol/skill/test_coordinator.py | 163 - dimos/protocol/skill/test_utils.py | 87 - dimos/protocol/skill/type.py | 272 -- dimos/protocol/skill/utils.py | 41 - dimos/protocol/tf/test_tf.py | 10 +- dimos/protocol/tf/tf.py | 60 +- dimos/robot/all_blueprints.py | 7 +- dimos/robot/cli/dimos.py | 12 +- dimos/robot/drone/README.md | 4 +- dimos/robot/drone/connection_module.py | 27 +- dimos/robot/drone/drone.py | 116 +- .../g1/blueprints/agentic/_agentic_skills.py | 6 +- dimos/robot/unitree/g1/skill_container.py | 12 +- .../go2/blueprints/agentic/_common_agentic.py | 4 +- .../blueprints/agentic/unitree_go2_agentic.py | 4 +- .../unitree_go2_agentic_huggingface.py | 8 +- .../agentic/unitree_go2_agentic_ollama.py | 8 +- dimos/robot/unitree/go2/connection.py | 16 +- dimos/robot/unitree/type/test_odometry.py | 32 - .../robot/unitree/unitree_skill_container.py | 174 +- dimos/robot/unitree/unitree_skills.py | 357 -- dimos/robot/unitree_webrtc/__init__.py | 1 - dimos/types/test_timestamped.py | 57 +- dimos/types/timestamped.py | 150 +- dimos/utils/cli/skillspy/demo_skillspy.py | 111 - dimos/utils/cli/skillspy/skillspy.py | 281 -- dimos/utils/data.py | 42 +- dimos/utils/reactive.py | 7 +- dimos/utils/test_data.py | 4 +- dimos/utils/testing/moment.py | 3 +- dimos/utils/testing/replay.py | 399 +- dimos/utils/testing/test_replay.py | 28 +- docker/navigation/.env.hardware | 37 + docker/navigation/Dockerfile | 164 +- docker/navigation/README.md | 42 +- docker/navigation/build.sh | 55 +- docker/navigation/docker-compose.yml | 144 +- docker/navigation/start.sh | 113 +- docker/python/Dockerfile | 5 +- docker/ros/Dockerfile | 5 +- docs/concepts/blueprints.md | 39 +- docs/concepts/modules.md | 4 +- docs/concepts/transports.md | 36 + docs/development/README.md | 29 + docs/development/depth_camera_integration.md | 2 +- docs/development/dimos_run.md | 2 +- flake.nix | 3 + pyproject.toml | 14 +- uv.lock | 3658 +++++++++-------- 167 files changed, 7344 insertions(+), 7756 deletions(-) delete mode 100644 dimos/agents/__init__.py create mode 100644 dimos/agents/agent_test_runner.py rename dimos/agents/{_skill_exports.py => annotation.py} (68%) delete mode 100644 dimos/agents/cli/human.py create mode 100644 dimos/agents/demo_agent.py create mode 100644 dimos/agents/fixtures/test_can_call_again_on_error[False].json create mode 100644 dimos/agents/fixtures/test_can_call_again_on_error[True].json create mode 100644 dimos/agents/fixtures/test_can_call_tool[False].json create mode 100644 dimos/agents/fixtures/test_can_call_tool[True].json delete mode 100644 dimos/agents/fixtures/test_go_to_object.json delete mode 100644 dimos/agents/fixtures/test_how_much_is_124181112_plus_124124.json create mode 100644 dimos/agents/fixtures/test_image.json create mode 100644 dimos/agents/fixtures/test_multiple_tool_calls_with_multiple_messages.json create mode 100644 dimos/agents/fixtures/test_prompt.json delete mode 100644 dimos/agents/fixtures/test_show_your_love.json rename dimos/agents/fixtures/{test_take_a_look_around.json => test_start_exploration.json} (51%) delete mode 100644 dimos/agents/fixtures/test_what_do_you_see_in_this_picture.json delete mode 100644 dimos/agents/fixtures/test_what_is_your_name.json delete mode 100644 dimos/agents/llm_init.py delete mode 100644 dimos/agents/skills/conftest.py delete mode 100644 dimos/agents/spec.py delete mode 100644 dimos/agents/temp/webcam_agent.py delete mode 100644 dimos/agents/test_agent_direct.py delete mode 100644 dimos/agents/test_agent_fake.py delete mode 100644 dimos/agents/test_mock_agent.py delete mode 100644 dimos/agents/test_stash_agent.py create mode 100644 dimos/agents/utils.py rename dimos/agents/{cli/web.py => web_human_input.py} (100%) delete mode 100644 dimos/core/skill_module.py create mode 100644 dimos/memory/embedding.py create mode 100644 dimos/memory/test_embedding.py create mode 100644 dimos/memory/timeseries/__init__.py create mode 100644 dimos/memory/timeseries/base.py create mode 100644 dimos/memory/timeseries/inmemory.py create mode 100644 dimos/memory/timeseries/legacy.py create mode 100644 dimos/memory/timeseries/pickledir.py create mode 100644 dimos/memory/timeseries/postgres.py create mode 100644 dimos/memory/timeseries/sqlite.py create mode 100644 dimos/memory/timeseries/test_base.py create mode 100644 dimos/memory/timeseries/test_legacy.py create mode 100644 dimos/protocol/pubsub/impl/ddspubsub.py create mode 100644 dimos/protocol/service/ddsservice.py delete mode 100644 dimos/protocol/skill/__init__.py delete mode 100644 dimos/protocol/skill/comms.py delete mode 100644 dimos/protocol/skill/coordinator.py delete mode 100644 dimos/protocol/skill/schema.py delete mode 100644 dimos/protocol/skill/skill.py delete mode 100644 dimos/protocol/skill/test_coordinator.py delete mode 100644 dimos/protocol/skill/test_utils.py delete mode 100644 dimos/protocol/skill/type.py delete mode 100644 dimos/protocol/skill/utils.py delete mode 100644 dimos/robot/unitree/unitree_skills.py delete mode 100644 dimos/utils/cli/skillspy/demo_skillspy.py delete mode 100644 dimos/utils/cli/skillspy/skillspy.py diff --git a/.gitignore b/.gitignore index e96b8bc0e8..24a3dd8919 100644 --- a/.gitignore +++ b/.gitignore @@ -67,4 +67,5 @@ yolo11n.pt /results **/cpp/result +CLAUDE.MD /assets/teleop_certs/ diff --git a/README.md b/README.md index 2c5242a0e6..14216eac9b 100644 --- a/README.md +++ b/README.md @@ -196,11 +196,11 @@ A blueprint example that connects the image stream from a robot to an LLM Agent from dimos.core import autoconnect, LCMTransport from dimos.msgs.sensor_msgs import Image from dimos.robot.unitree.go2.connection import go2_connection -from dimos.agents.agent import llm_agent +from dimos.agents.agent import agent blueprint = autoconnect( go2_connection(), - llm_agent(), + agent(), ).transports({("color_image", Image): LCMTransport("/color_image", Image)}) # Run the blueprint @@ -234,7 +234,6 @@ For system deps, Nix setups, and testing, see `/docs/development/README.md`. DimOS comes with a number of monitoring tools: - Run `lcmspy` to see how fast messages are being published on streams. -- Run `skillspy` to see how skills are being called, how long they are running, which are active, etc. - Run `agentspy` to see the agent's status over time. - If you suspect there is a bug within DimOS itself, you can enable extreme logging by prefixing the dimos command with `DIMOS_LOG_LEVEL=DEBUG RERUN_SAVE=1 `. Ex: `DIMOS_LOG_LEVEL=DEBUG RERUN_SAVE=1 dimos --replay run unitree-go2` diff --git a/dimos/agents/__init__.py b/dimos/agents/__init__.py deleted file mode 100644 index ab3c67cfa5..0000000000 --- a/dimos/agents/__init__.py +++ /dev/null @@ -1,12 +0,0 @@ -import lazy_loader as lazy - -__getattr__, __dir__, __all__ = lazy.attach( - __name__, - submod_attrs={ - "agent": ["Agent", "deploy"], - "spec": ["AgentSpec"], - "vlm_agent": ["VLMAgent"], - "vlm_stream_tester": ["VlmStreamTester"], - "_skill_exports": ["skill", "Output", "Reducer", "Stream"], - }, -) diff --git a/dimos/agents/agent.py b/dimos/agents/agent.py index e9c7c5d7b9..ec39c5d16d 100644 --- a/dimos/agents/agent.py +++ b/dimos/agents/agent.py @@ -1,4 +1,4 @@ -# Copyright 2025-2026 Dimensional Inc. +# Copyright 2026 Dimensional Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -11,395 +11,194 @@ # 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. -import asyncio -import datetime + +from dataclasses import dataclass import json -from operator import itemgetter -import os -from typing import Any, TypedDict +from queue import Empty, Queue +from threading import Event, RLock, Thread +from typing import TYPE_CHECKING, Any, Protocol import uuid -from langchain_core.messages import ( - AIMessage, - HumanMessage, - SystemMessage, - ToolCall, - ToolMessage, -) - -from dimos.agents.llm_init import build_llm, build_system_message -from dimos.agents.spec import AgentSpec, Model, Provider -from dimos.core import DimosCluster, rpc -from dimos.protocol.skill.coordinator import SkillCoordinator, SkillState, SkillStateDict -from dimos.protocol.skill.skill import SkillContainer -from dimos.protocol.skill.type import Output -from dimos.utils.logging_config import setup_logger - -logger = setup_logger() - - -SYSTEM_MSG_APPEND = "\nYour message history will always be appended with a System Overview message that provides situational awareness." - - -def toolmsg_from_state(state: SkillState) -> ToolMessage: - if state.skill_config.output != Output.standard: - content = "output attached in separate messages" - else: - content = state.content() # type: ignore[assignment] - - return ToolMessage( - # if agent call has been triggered by another skill, - # and this specific skill didn't finish yet but we need a tool call response - # we return a message explaining that execution is still ongoing - content=content - or "Running, you will be called with an update, no need for subsequent tool calls", - name=state.name, - tool_call_id=state.call_id, - ) - - -class SkillStateSummary(TypedDict): - name: str - call_id: str - state: str - data: Any - - -def summary_from_state(state: SkillState, special_data: bool = False) -> SkillStateSummary: - content = state.content() - if isinstance(content, dict): - content = json.dumps(content) - - if not isinstance(content, str): - content = str(content) - - return { - "name": state.name, - "call_id": state.call_id, - "state": state.state.name, - "data": state.content() if not special_data else "data will be in a separate message", - } - - -def _custom_json_serializers(obj): # type: ignore[no-untyped-def] - if isinstance(obj, datetime.date | datetime.datetime): - return obj.isoformat() - raise TypeError(f"Type {type(obj)} not serializable") - - -# takes an overview of running skills from the coorindator -# and builds messages to be sent to an agent -def snapshot_to_messages( - state: SkillStateDict, - tool_calls: list[ToolCall], -) -> tuple[list[ToolMessage], AIMessage | None]: - # builds a set of tool call ids from a previous agent request - tool_call_ids = set( - map(itemgetter("id"), tool_calls), - ) - - # build a tool msg responses - tool_msgs: list[ToolMessage] = [] - - # build a general skill state overview (for longer running skills) - state_overview: list[dict[str, SkillStateSummary]] = [] - - # for special skills that want to return a separate message - # (images for example, requires to be a HumanMessage) - special_msgs: list[HumanMessage] = [] - - # for special skills that want to return a separate message that should - # stay in history, like actual human messages, critical events - history_msgs: list[HumanMessage] = [] - - # Initialize state_msg - state_msg = None - - for skill_state in sorted( - state.values(), - key=lambda skill_state: skill_state.duration(), - ): - if skill_state.call_id in tool_call_ids: - tool_msgs.append(toolmsg_from_state(skill_state)) - - if skill_state.skill_config.output == Output.human: - content = skill_state.content() - if not content: - continue - history_msgs.append(HumanMessage(content=content)) # type: ignore[arg-type] - continue - - special_data = skill_state.skill_config.output == Output.image - if special_data: - content = skill_state.content() - if not content: - continue - special_msgs.append(HumanMessage(content=content)) # type: ignore[arg-type] - - if skill_state.call_id in tool_call_ids: - continue - - state_overview.append(summary_from_state(skill_state, special_data)) # type: ignore[arg-type] - - if state_overview: - state_overview_str = "\n".join( - json.dumps(s, default=_custom_json_serializers) for s in state_overview +from langchain.agents import create_agent +from langchain_core.messages import HumanMessage +from langchain_core.messages.base import BaseMessage +from langchain_core.tools import StructuredTool +from langgraph.graph.state import CompiledStateGraph +from reactivex.disposable import Disposable + +from dimos.agents.system_prompt import SYSTEM_PROMPT +from dimos.agents.utils import pretty_print_langchain_message +from dimos.core.core import rpc +from dimos.core.module import Module, ModuleConfig, SkillInfo +from dimos.core.rpc_client import RpcCall, RPCClient +from dimos.core.stream import In, Out +from dimos.protocol.rpc import RPCSpec +from dimos.spec.utils import Spec + +if TYPE_CHECKING: + from langchain_core.language_models import BaseChatModel + + +@dataclass +class AgentConfig(ModuleConfig): + system_prompt: str | None = SYSTEM_PROMPT + model: str = "gpt-4o" + model_fixture: str | None = None + + +class Agent(Module): + default_config: type[AgentConfig] = AgentConfig + config: AgentConfig + agent: Out[BaseMessage] + human_input: In[str] + agent_idle: Out[bool] + + _lock: RLock + _state_graph: CompiledStateGraph[Any, Any, Any, Any] | None + _message_queue: Queue[BaseMessage] + _history: list[BaseMessage] + _thread: Thread + _stop_event: Event + + def __init__(self, *args: Any, **kwargs: Any) -> None: + super().__init__(*args, **kwargs) + self._lock = RLock() + self._state_graph = None + self._message_queue = Queue() + self._history = [] + self._thread = Thread( + target=self._thread_loop, + name=f"{self.__class__.__name__}-thread", + daemon=True, ) - state_msg = AIMessage("State Overview:\n" + state_overview_str) - - return { # type: ignore[return-value] - "tool_msgs": tool_msgs, - "history_msgs": history_msgs, - "state_msgs": ([state_msg] if state_msg else []) + special_msgs, - } - - -# Agent class job is to glue skill coordinator state to an agent, builds langchain messages -class Agent(AgentSpec): - system_message: SystemMessage - state_messages: list[AIMessage | HumanMessage] - - def __init__( # type: ignore[no-untyped-def] - self, - *args, - **kwargs, - ) -> None: - AgentSpec.__init__(self, *args, **kwargs) - - self.state_messages = [] - self.coordinator = SkillCoordinator() - self._history = [] # type: ignore[var-annotated] - self._agent_id = str(uuid.uuid4()) - self._agent_stopped = False - - self.system_message = build_system_message(self.config, append=SYSTEM_MSG_APPEND) - self.publish(self.system_message) - self._llm = build_llm(self.config) - - @rpc - def get_agent_id(self) -> str: - return self._agent_id + self._stop_event = Event() @rpc def start(self) -> None: super().start() - self.coordinator.start() + + def _on_human_input(string: str) -> None: + self._message_queue.put(HumanMessage(content=string)) + + self._disposables.add(Disposable(self.human_input.subscribe(_on_human_input))) @rpc def stop(self) -> None: - self.coordinator.stop() - self._agent_stopped = True + self._stop_event.set() + if self._thread.is_alive(): + self._thread.join(timeout=2.0) super().stop() - def clear_history(self) -> None: - self._history.clear() - - def append_history(self, *msgs: list[AIMessage | HumanMessage]) -> None: - for msg in msgs: - self.publish(msg) # type: ignore[arg-type] - - self._history.extend(msgs) - - def history(self): # type: ignore[no-untyped-def] - return [self.system_message, *self._history, *self.state_messages] - - # Used by agent to execute tool calls - def execute_tool_calls(self, tool_calls: list[ToolCall]) -> None: - """Execute a list of tool calls from the agent.""" - if self._agent_stopped: - logger.warning("Agent is stopped, cannot execute tool calls.") - return - for tool_call in tool_calls: - logger.info(f"executing skill call {tool_call}") - self.coordinator.call_skill( - tool_call.get("id"), # type: ignore[arg-type] - tool_call.get("name"), - tool_call.get("args"), - ) - - # used to inject skill calls into the agent loop without agent asking for it - def run_implicit_skill(self, skill_name: str, **kwargs) -> None: # type: ignore[no-untyped-def] - if self._agent_stopped: - logger.warning("Agent is stopped, cannot execute implicit skill calls.") - return - self.coordinator.call_skill(False, skill_name, {"args": kwargs}) - - async def agent_loop(self, first_query: str = ""): # type: ignore[no-untyped-def] - # TODO: Should I add a lock here to prevent concurrent calls to agent_loop? - - if self._agent_stopped: - logger.warning("Agent is stopped, cannot run agent loop.") - # return "Agent is stopped." - import traceback - - traceback.print_stack() - return "Agent is stopped." - - self.state_messages = [] - if first_query: - self.append_history(HumanMessage(first_query)) # type: ignore[arg-type] - - def _get_state() -> str: - # TODO: FIX THIS EXTREME HACK - update = self.coordinator.generate_snapshot(clear=False) - snapshot_msgs = snapshot_to_messages(update, msg.tool_calls) - return json.dumps(snapshot_msgs, sort_keys=True, default=lambda o: repr(o)) - - try: - while True: - # we are getting tools from the coordinator on each turn - # since this allows for skillcontainers to dynamically provide new skills - tools = self.get_tools() # type: ignore[no-untyped-call] - self._llm = self._llm.bind_tools(tools) # type: ignore[assignment] - - # publish to /agent topic for observability - for state_msg in self.state_messages: - self.publish(state_msg) - - # history() builds our message history dynamically - # ensures we include latest system state, but not old ones. - messages = self.history() # type: ignore[no-untyped-call] - - # Some LLMs don't work without any human messages. Add an initial one. - if len(messages) == 1 and isinstance(messages[0], SystemMessage): - messages.append( - HumanMessage( - "Everything is initialized. I'll let you know when you should act." - ) - ) - self.append_history(messages[-1]) - - msg = self._llm.invoke(messages) - - self.append_history(msg) # type: ignore[arg-type] - - logger.info(f"Agent response: {msg.content}") - - state = _get_state() - - if msg.tool_calls: - self.execute_tool_calls(msg.tool_calls) - - # print(self) - # print(self.coordinator) - - self._write_debug_history_file() - - if not self.coordinator.has_active_skills(): - logger.info("No active tasks, exiting agent loop.") - return msg.content - - # coordinator will continue once a skill state has changed in - # such a way that agent call needs to be executed - - if state == _get_state(): - await self.coordinator.wait_for_updates() - - # we request a full snapshot of currently running, finished or errored out skills - # we ask for removal of finished skills from subsequent snapshots (clear=True) - update = self.coordinator.generate_snapshot(clear=True) + @rpc + def on_system_modules(self, modules: list[RPCClient]) -> None: + assert self.rpc is not None - # generate tool_msgs and general state update message, - # depending on a skill having associated tool call from previous interaction - # we will return a tool message, and not a general state message - snapshot_msgs = snapshot_to_messages(update, msg.tool_calls) + if self.config.model.startswith("ollama:"): + from dimos.agents.ollama_agent import ensure_ollama_model - self.state_messages = snapshot_msgs.get("state_msgs", []) # type: ignore[attr-defined] - self.append_history( - *snapshot_msgs.get("tool_msgs", []), # type: ignore[attr-defined] - *snapshot_msgs.get("history_msgs", []), # type: ignore[attr-defined] - ) + ensure_ollama_model(self.config.model.removeprefix("ollama:")) - except Exception as e: - logger.error(f"Error in agent loop: {e}") - import traceback + model: str | BaseChatModel = self.config.model + if self.config.model_fixture is not None: + from dimos.agents.testing import MockModel - traceback.print_exc() + model = MockModel(json_path=self.config.model_fixture) - @rpc - def loop_thread(self) -> bool: - asyncio.run_coroutine_threadsafe(self.agent_loop(), self._loop) # type: ignore[arg-type] - return True + with self._lock: + self._state_graph = create_agent( + model=model, + tools=_get_tools_from_modules(self, modules, self.rpc), + system_prompt=self.config.system_prompt, + ) + self._thread.start() @rpc - def query(self, query: str): # type: ignore[no-untyped-def] - # TODO: could this be - # from distributed.utils import sync - # return sync(self._loop, self.agent_loop, query) - return asyncio.run_coroutine_threadsafe(self.agent_loop(query), self._loop).result() # type: ignore[arg-type] + def add_message(self, message: BaseMessage) -> None: + self._message_queue.put(message) + + def _thread_loop(self) -> None: + while not self._stop_event.is_set(): + try: + message = self._message_queue.get(timeout=0.5) + except Empty: + continue - async def query_async(self, query: str): # type: ignore[no-untyped-def] - return await self.agent_loop(query) + with self._lock: + if not self._state_graph: + raise ValueError("No state graph initialized") + self._process_message(self._state_graph, message) - @rpc - def register_skills(self, container, run_implicit_name: str | None = None): # type: ignore[no-untyped-def] - ret = self.coordinator.register_skills(container) # type: ignore[func-returns-value] + def _process_message( + self, state_graph: CompiledStateGraph[Any, Any, Any, Any], message: BaseMessage + ) -> None: + self._history.append(message) + pretty_print_langchain_message(message) + self.agent.publish(message) - if run_implicit_name: - self.run_implicit_skill(run_implicit_name) + for update in state_graph.stream({"messages": self._history}, stream_mode="updates"): + for node_output in update.values(): + for msg in node_output.get("messages", []): + self._history.append(msg) + pretty_print_langchain_message(msg) + self.agent.publish(msg) - return ret + if self._message_queue.empty(): + self.agent_idle.publish(True) - def get_tools(self): # type: ignore[no-untyped-def] - return self.coordinator.get_tools() - def _write_debug_history_file(self) -> None: - file_path = os.getenv("DEBUG_AGENT_HISTORY_FILE") - if not file_path: - return +class AgentSpec(Spec, Protocol): + def add_message(self, message: BaseMessage) -> None: ... - history = [x.__dict__ for x in self.history()] # type: ignore[no-untyped-call] - with open(file_path, "w") as f: - json.dump(history, f, default=lambda x: repr(x), indent=2) +def _get_tools_from_modules( + agent: Agent, modules: list[RPCClient], rpc: RPCSpec +) -> list[StructuredTool]: + skills = [skill for module in modules for skill in (module.get_skills() or [])] + return [_skill_to_tool(agent, skill, rpc) for skill in skills] -class LlmAgent(Agent): - @rpc - def start(self) -> None: - super().start() - self.loop_thread() +def _skill_to_tool(agent: Agent, skill: SkillInfo, rpc: RPCSpec) -> StructuredTool: + rpc_call = RpcCall(None, rpc, skill.func_name, skill.class_name, []) - @rpc - def stop(self) -> None: - super().stop() + def wrapped_func(*args: Any, **kwargs: Any) -> str | list[dict[str, Any]]: + result = None + try: + result = rpc_call(*args, **kwargs) + except Exception as e: + return f"Exception: Error: {e}" -llm_agent = LlmAgent.blueprint + if result is None: + return "It has started. You will be updated later." + if hasattr(result, "agent_encode"): + uuid_ = str(uuid.uuid4()) + _append_image_to_history(agent, skill, uuid_, result) + return f"Tool call started with UUID: {uuid_}" -def deploy( - dimos: DimosCluster, - system_prompt: str = "You are a helpful assistant for controlling a Unitree Go2 robot.", - model: Model = Model.GPT_4O, - provider: Provider = Provider.OPENAI, # type: ignore[attr-defined] - skill_containers: list[SkillContainer] | None = None, -) -> Agent: - from dimos.agents.cli.human import HumanInput + return str(result) - if skill_containers is None: - skill_containers = [] - agent = dimos.deploy( # type: ignore[attr-defined] - Agent, - system_prompt=system_prompt, - model=model, - provider=provider, + return StructuredTool( + name=skill.func_name, + func=wrapped_func, + args_schema=json.loads(skill.args_schema), ) - human_input = dimos.deploy(HumanInput) # type: ignore[attr-defined] - human_input.start() - agent.register_skills(human_input) - - for skill_container in skill_containers: - print("Registering skill container:", skill_container) - agent.register_skills(skill_container) - - agent.run_implicit_skill("human") - agent.start() - agent.loop_thread() +def _append_image_to_history(agent: Agent, skill: SkillInfo, uuid_: str, result: Any) -> None: + agent.add_message( + HumanMessage( + content=[ + { + "type": "text", + "text": f"This is the artefact for the '{skill.func_name}' tool with UUID:={uuid_}.", + }, + *result.agent_encode(), + ] + ) + ) - return agent # type: ignore[no-any-return] +agent = Agent.blueprint -__all__ = ["Agent", "deploy", "llm_agent"] +__all__ = ["Agent", "AgentSpec", "agent"] diff --git a/dimos/agents/agent_test_runner.py b/dimos/agents/agent_test_runner.py new file mode 100644 index 0000000000..7d7fbab03d --- /dev/null +++ b/dimos/agents/agent_test_runner.py @@ -0,0 +1,80 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from threading import Event, Thread + +from langchain_core.messages import AIMessage +from langchain_core.messages.base import BaseMessage +from reactivex.disposable import Disposable + +from dimos.agents.agent import AgentSpec +from dimos.core.core import rpc +from dimos.core.module import Module +from dimos.core.rpc_client import RPCClient +from dimos.core.stream import In, Out + + +class AgentTestRunner(Module): + agent_spec: AgentSpec + agent: In[BaseMessage] + agent_idle: In[bool] + finished: Out[bool] + added: Out[bool] + + def __init__(self, messages: list[BaseMessage]) -> None: + super().__init__() + self._messages = messages + self._idle_event = Event() + self._subscription_ready = Event() + self._thread = Thread(target=self._thread_loop, daemon=True) + + @rpc + def start(self) -> None: + super().start() + self._disposables.add(Disposable(self.agent.subscribe(self._on_agent_message))) + self._disposables.add(Disposable(self.agent_idle.subscribe(self._on_agent_idle))) + # Signal that subscription is ready + self._subscription_ready.set() + + @rpc + def stop(self) -> None: + super().stop() + + @rpc + def on_system_modules(self, _modules: list[RPCClient]) -> None: + self._thread.start() + + def _on_agent_idle(self, idle: bool) -> None: + if idle: + self._idle_event.set() + + def _on_agent_message(self, message: BaseMessage) -> None: + # Check for final AIMessage (no tool calls) to signal completion + is_ai = isinstance(message, AIMessage) + has_tool_calls = hasattr(message, "tool_calls") and message.tool_calls + if is_ai and not has_tool_calls: + self.added.publish(True) + + def _thread_loop(self) -> None: + # Ensure subscription is ready before sending messages + if not self._subscription_ready.wait(5): + raise TimeoutError("Timed out waiting for subscription to be ready.") + + for message in self._messages: + self._idle_event.clear() + self.agent_spec.add_message(message) + if not self._idle_event.wait(60): + raise TimeoutError("Timed out waiting for message to be processed.") + + self.finished.publish(True) diff --git a/dimos/agents/_skill_exports.py b/dimos/agents/annotation.py similarity index 68% rename from dimos/agents/_skill_exports.py rename to dimos/agents/annotation.py index a04093b20b..083a3cbc53 100644 --- a/dimos/agents/_skill_exports.py +++ b/dimos/agents/annotation.py @@ -12,7 +12,13 @@ # See the License for the specific language governing permissions and # limitations under the License. -from dimos.protocol.skill.skill import skill -from dimos.protocol.skill.type import Output, Reducer, Stream +from collections.abc import Callable +from typing import Any, TypeVar -__all__ = ["Output", "Reducer", "Stream", "skill"] +F = TypeVar("F", bound=Callable[..., Any]) + + +def skill(func: F) -> F: + func.__rpc__ = True # type: ignore[attr-defined] + func.__skill__ = True # type: ignore[attr-defined] + return func diff --git a/dimos/agents/cli/human.py b/dimos/agents/cli/human.py deleted file mode 100644 index e842b3cc8a..0000000000 --- a/dimos/agents/cli/human.py +++ /dev/null @@ -1,57 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import queue - -from reactivex.disposable import Disposable - -from dimos.agents import Output, Reducer, Stream, skill # type: ignore[attr-defined] -from dimos.core import pLCMTransport, rpc -from dimos.core.module import Module -from dimos.core.rpc_client import RpcCall - - -class HumanInput(Module): - running: bool = False - - @skill(stream=Stream.call_agent, reducer=Reducer.string, output=Output.human, hide_skill=True) # type: ignore[arg-type] - def human(self): # type: ignore[no-untyped-def] - """receives human input, no need to run this, it's running implicitly""" - if self.running: - return "already running" - self.running = True - transport = pLCMTransport("/human_input") # type: ignore[var-annotated] - - msg_queue = queue.Queue() # type: ignore[var-annotated] - unsub = transport.subscribe(msg_queue.put) # type: ignore[func-returns-value] - self._disposables.add(Disposable(unsub)) - yield from iter(msg_queue.get, None) - - @rpc - def start(self) -> None: - super().start() - - @rpc - def stop(self) -> None: - super().stop() - - @rpc - def set_AgentSpec_register_skills(self, callable: RpcCall) -> None: - callable.set_rpc(self.rpc) # type: ignore[arg-type] - callable(self, run_implicit_name="human") - - -human_input = HumanInput.blueprint - -__all__ = ["HumanInput", "human_input"] diff --git a/dimos/agents/conftest.py b/dimos/agents/conftest.py index 52d7d5a6bb..23d888b0fe 100644 --- a/dimos/agents/conftest.py +++ b/dimos/agents/conftest.py @@ -1,4 +1,4 @@ -# Copyright 2025-2026 Dimensional Inc. +# Copyright 2026 Dimensional Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -12,74 +12,98 @@ # See the License for the specific language governing permissions and # limitations under the License. +import os from pathlib import Path +from threading import Event +from dotenv import load_dotenv +from langchain_core.messages.base import BaseMessage import pytest from dimos.agents.agent import Agent -from dimos.agents.testing import MockModel -from dimos.protocol.skill.test_coordinator import SkillContainerTest +from dimos.agents.agent_test_runner import AgentTestRunner +from dimos.core.blueprints import autoconnect +from dimos.core.global_config import global_config +from dimos.core.transport import pLCMTransport +load_dotenv() -@pytest.fixture -def fixture_dir(): - return Path(__file__).parent / "fixtures" +FIXTURE_DIR = Path(__file__).parent / "fixtures" @pytest.fixture -def potato_system_prompt() -> str: - return "Your name is Mr. Potato, potatoes are bad at math. Use a tools if asked to calculate" +def fixture_dir() -> Path: + return FIXTURE_DIR @pytest.fixture -def skill_container(): - container = SkillContainerTest() - try: - yield container - finally: - container.stop() - - -@pytest.fixture -def create_fake_agent(fixture_dir): - agent = None - - def _agent_factory(*, system_prompt, skill_containers, fixture): - mock_model = MockModel(json_path=fixture_dir / fixture) - - nonlocal agent - agent = Agent(system_prompt=system_prompt, model_instance=mock_model) - - for skill_container in skill_containers: - agent.register_skills(skill_container) - - agent.start() - - return agent - - try: - yield _agent_factory - finally: - if agent: - agent.stop() - - -@pytest.fixture -def create_potato_agent(potato_system_prompt, skill_container, fixture_dir): - agent = None - - def _agent_factory(*, fixture): - mock_model = MockModel(json_path=fixture_dir / fixture) - - nonlocal agent - agent = Agent(system_prompt=potato_system_prompt, model_instance=mock_model) - agent.register_skills(skill_container) - agent.start() - - return agent - - try: - yield _agent_factory - finally: - if agent: - agent.stop() +def agent_setup(request): + coordinator = None + transports: list[pLCMTransport] = [] + unsubs: list = [] + recording = bool(os.getenv("RECORD")) + + def fn( + *, + blueprints, + messages: list[BaseMessage], + dask: bool = False, + system_prompt: str | None = None, + fixture: str | None = None, + ) -> list[BaseMessage]: + history: list[BaseMessage] = [] + finished_event = Event() + + agent_transport: pLCMTransport = pLCMTransport("/agent") + finished_transport: pLCMTransport = pLCMTransport("/finished") + transports.extend([agent_transport, finished_transport]) + + def on_message(msg: BaseMessage) -> None: + history.append(msg) + + unsubs.append(agent_transport.subscribe(on_message)) + unsubs.append(finished_transport.subscribe(lambda _: finished_event.set())) + + # Derive fixture path from test name if not explicitly provided. + if fixture is not None: + fixture_path = FIXTURE_DIR / fixture + else: + fixture_path = FIXTURE_DIR / f"{request.node.name}.json" + + agent_kwargs: dict = {"system_prompt": system_prompt} + + if recording or fixture_path.exists(): + # RECORD=1: use real LLM, save responses to fixture file. + # No RECORD but fixture exists: play back recorded responses. + # The MockModel checks RECORD internally to decide record vs playback. + agent_kwargs["model_fixture"] = str(fixture_path) + + blueprint = autoconnect( + *blueprints, + Agent.blueprint(**agent_kwargs), + AgentTestRunner.blueprint(messages=messages), + ) + + global_config.update( + viewer_backend="none", + dask=dask, + ) + + nonlocal coordinator + coordinator = blueprint.build() + + if not finished_event.wait(60): + raise TimeoutError("Timed out waiting for agent to finish processing messages.") + + return history + + yield fn + + if coordinator is not None: + coordinator.stop() + + for transport in transports: + transport.stop() + + for unsub in unsubs: + unsub() diff --git a/dimos/agents/demo_agent.py b/dimos/agents/demo_agent.py new file mode 100644 index 0000000000..b3250fba5b --- /dev/null +++ b/dimos/agents/demo_agent.py @@ -0,0 +1,32 @@ +# 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.agents.agent import Agent +from dimos.core.blueprints import autoconnect +from dimos.hardware.sensors.camera import zed +from dimos.hardware.sensors.camera.module import camera_module +from dimos.hardware.sensors.camera.webcam import Webcam + +demo_agent = autoconnect(Agent.blueprint()) + +demo_agent_camera = autoconnect( + Agent.blueprint(), + camera_module( + hardware=lambda: Webcam( + camera_index=0, + fps=15, + camera_info=zed.CameraInfo.SingleWebcam, + ), + ), +) diff --git a/dimos/agents/fixtures/test_can_call_again_on_error[False].json b/dimos/agents/fixtures/test_can_call_again_on_error[False].json new file mode 100644 index 0000000000..762b452cff --- /dev/null +++ b/dimos/agents/fixtures/test_can_call_again_on_error[False].json @@ -0,0 +1,34 @@ +{ + "responses": [ + { + "content": "", + "tool_calls": [ + { + "name": "register_user", + "args": { + "name": "Paul" + }, + "id": "call_gizJWFgoQiYOQMCDqjlshkvk", + "type": "tool_call" + } + ] + }, + { + "content": "", + "tool_calls": [ + { + "name": "register_user", + "args": { + "name": "paul" + }, + "id": "call_O9p0ktNw0frMfNXjbul6Do1m", + "type": "tool_call" + } + ] + }, + { + "content": "The user named \"paul\" has been registered successfully.", + "tool_calls": [] + } + ] +} diff --git a/dimos/agents/fixtures/test_can_call_again_on_error[True].json b/dimos/agents/fixtures/test_can_call_again_on_error[True].json new file mode 100644 index 0000000000..b67efe84e0 --- /dev/null +++ b/dimos/agents/fixtures/test_can_call_again_on_error[True].json @@ -0,0 +1,34 @@ +{ + "responses": [ + { + "content": "", + "tool_calls": [ + { + "name": "register_user", + "args": { + "name": "Paul" + }, + "id": "call_4l78eCMbfsbIC2qPL86jA4S0", + "type": "tool_call" + } + ] + }, + { + "content": "", + "tool_calls": [ + { + "name": "register_user", + "args": { + "name": "paul" + }, + "id": "call_Jzi8RU0jRMeCcUDEcRkHzahs", + "type": "tool_call" + } + ] + }, + { + "content": "The user named \"paul\" has been registered successfully.", + "tool_calls": [] + } + ] +} diff --git a/dimos/agents/fixtures/test_can_call_tool[False].json b/dimos/agents/fixtures/test_can_call_tool[False].json new file mode 100644 index 0000000000..32d7770899 --- /dev/null +++ b/dimos/agents/fixtures/test_can_call_tool[False].json @@ -0,0 +1,22 @@ +{ + "responses": [ + { + "content": "", + "tool_calls": [ + { + "name": "add", + "args": { + "x": 33333, + "y": 100 + }, + "id": "call_vIdFXzNnojiCXtnEi9J2gXQN", + "type": "tool_call" + } + ] + }, + { + "content": "33333 + 100 is 33433.", + "tool_calls": [] + } + ] +} diff --git a/dimos/agents/fixtures/test_can_call_tool[True].json b/dimos/agents/fixtures/test_can_call_tool[True].json new file mode 100644 index 0000000000..e9431bb8ea --- /dev/null +++ b/dimos/agents/fixtures/test_can_call_tool[True].json @@ -0,0 +1,22 @@ +{ + "responses": [ + { + "content": "", + "tool_calls": [ + { + "name": "add", + "args": { + "x": 33333, + "y": 100 + }, + "id": "call_ZlA2HzNAuHF0H52CKQIPX9Te", + "type": "tool_call" + } + ] + }, + { + "content": "The result of 33333 + 100 is 33433.", + "tool_calls": [] + } + ] +} diff --git a/dimos/agents/fixtures/test_get_gps_position_for_queries.json b/dimos/agents/fixtures/test_get_gps_position_for_queries.json index 5d95b91bac..c2f163598d 100644 --- a/dimos/agents/fixtures/test_get_gps_position_for_queries.json +++ b/dimos/agents/fixtures/test_get_gps_position_for_queries.json @@ -6,19 +6,19 @@ { "name": "get_gps_position_for_queries", "args": { - "args": [ + "queries": [ "Hyde Park", "Regent Park", "Russell Park" ] }, - "id": "call_xO0VDst53tzetEUq8mapKGS1", + "id": "call_KqAjbd5E9VE69YMWBINdWnkw", "type": "tool_call" } ] }, { - "content": "Here are the latitude and longitude coordinates for the parks:\n\n- Hyde Park: Latitude 37.782601, Longitude -122.413201\n- Regent Park: Latitude 37.782602, Longitude -122.413202\n- Russell Park: Latitude 37.782603, Longitude -122.413203", + "content": "Here are the latitude and longitude coordinates for the parks you asked about:\n\n- **Hyde Park**: Latitude 37.782601, Longitude -122.413201\n- **Regent Park**: Latitude 37.782602, Longitude -122.413202\n- **Russell Park**: Latitude 37.782603, Longitude -122.413203", "tool_calls": [] } ] diff --git a/dimos/agents/fixtures/test_go_to_object.json b/dimos/agents/fixtures/test_go_to_object.json deleted file mode 100644 index 80f1e95379..0000000000 --- a/dimos/agents/fixtures/test_go_to_object.json +++ /dev/null @@ -1,27 +0,0 @@ -{ - "responses": [ - { - "content": "I will navigate to the nearest chair.", - "tool_calls": [ - { - "name": "navigate_with_text", - "args": { - "args": [ - "chair" - ] - }, - "id": "call_LP4eewByfO9XaxMtnnWxDUz7", - "type": "tool_call" - } - ] - }, - { - "content": "I'm on my way to the chair. Let me know if there's anything else you'd like me to do!", - "tool_calls": [] - }, - { - "content": "I have successfully navigated to the chair. Let me know if you need anything else!", - "tool_calls": [] - } - ] -} diff --git a/dimos/agents/fixtures/test_go_to_semantic_location.json b/dimos/agents/fixtures/test_go_to_semantic_location.json index 1a10711543..8ea3006142 100644 --- a/dimos/agents/fixtures/test_go_to_semantic_location.json +++ b/dimos/agents/fixtures/test_go_to_semantic_location.json @@ -6,17 +6,15 @@ { "name": "navigate_with_text", "args": { - "args": [ - "bookshelf" - ] + "query": "bookshelf" }, - "id": "call_yPoqcavMo05ogNNy5LMNQl2a", + "id": "call_PkS6DWAciWAAAdZfatiXoEdu", "type": "tool_call" } ] }, { - "content": "I have successfully arrived at the bookshelf. Is there anything specific you need here?", + "content": "I have successfully navigated to the bookshelf.", "tool_calls": [] } ] diff --git a/dimos/agents/fixtures/test_how_much_is_124181112_plus_124124.json b/dimos/agents/fixtures/test_how_much_is_124181112_plus_124124.json deleted file mode 100644 index f4dbe0c3a5..0000000000 --- a/dimos/agents/fixtures/test_how_much_is_124181112_plus_124124.json +++ /dev/null @@ -1,52 +0,0 @@ -{ - "responses": [ - { - "content": "", - "tool_calls": [ - { - "name": "add", - "args": { - "args": [ - 124181112, - 124124 - ] - }, - "id": "call_SSoVXz5yihrzR8TWIGnGKSpi", - "type": "tool_call" - } - ] - }, - { - "content": "Let me do some potato math... Calculating this will take some time, hold on! \ud83e\udd54", - "tool_calls": [] - }, - { - "content": "The result of adding 124,181,112 and 124,124 is 124,305,236. Potatoes work well with tools! \ud83e\udd54\ud83c\udf89", - "tool_calls": [] - }, - { - "content": "", - "tool_calls": [ - { - "name": "add", - "args": { - "args": [ - 1000000000, - -1000000 - ] - }, - "id": "call_ge9pv6IRa3yo0vjVaORvrGby", - "type": "tool_call" - } - ] - }, - { - "content": "Let's get those numbers crunched. Potatoes need a bit of time! \ud83e\udd54\ud83d\udcca", - "tool_calls": [] - }, - { - "content": "The result of one billion plus negative one million is 999,000,000. Potatoes are amazing with some help! \ud83e\udd54\ud83d\udca1", - "tool_calls": [] - } - ] -} diff --git a/dimos/agents/fixtures/test_image.json b/dimos/agents/fixtures/test_image.json new file mode 100644 index 0000000000..196c2122ef --- /dev/null +++ b/dimos/agents/fixtures/test_image.json @@ -0,0 +1,23 @@ +{ + "responses": [ + { + "content": "", + "tool_calls": [ + { + "name": "take_a_picture", + "args": {}, + "id": "call_11vyrbtZXlsEgoKuQs8jRjP8", + "type": "tool_call" + } + ] + }, + { + "content": "I have taken a picture, let me analyze it to provide a description.\nBased on the image captured, the setting resembles a \"cafe\". It shows an indoor space with tables, chairs, and a warm, inviting atmosphere typical of a cafe environment, where people might gather for drinks and conversation.", + "tool_calls": [] + }, + { + "content": "The image depicts a \"cafe\" setting. It shows people sitting outside at tables, likely enjoying drinks or meals, with a cozy and inviting ambiance typical of a cafe.", + "tool_calls": [] + } + ] +} diff --git a/dimos/agents/fixtures/test_multiple_tool_calls_with_multiple_messages.json b/dimos/agents/fixtures/test_multiple_tool_calls_with_multiple_messages.json new file mode 100644 index 0000000000..7fd8172d35 --- /dev/null +++ b/dimos/agents/fixtures/test_multiple_tool_calls_with_multiple_messages.json @@ -0,0 +1,98 @@ +{ + "responses": [ + { + "content": "", + "tool_calls": [ + { + "name": "locate_person", + "args": { + "name": "John" + }, + "id": "call_w4cTUUpojE2zRMYda93ma3io", + "type": "tool_call" + } + ] + }, + { + "content": "", + "tool_calls": [ + { + "name": "register_person", + "args": { + "name": "John" + }, + "id": "call_Rij2ZZHG1u2yEKALNqH4L0WH", + "type": "tool_call" + } + ] + }, + { + "content": "", + "tool_calls": [ + { + "name": "locate_person", + "args": { + "name": "John" + }, + "id": "call_Pr1HUeq1j2L9bYU3CsTXVmnk", + "type": "tool_call" + } + ] + }, + { + "content": "", + "tool_calls": [ + { + "name": "go_to_location", + "args": { + "description": "kitchen" + }, + "id": "call_F6Pw6Da2mixJ1mmhb0q4uI3F", + "type": "tool_call" + } + ] + }, + { + "content": "I have moved to the kitchen where John is located.", + "tool_calls": [] + }, + { + "content": "", + "tool_calls": [ + { + "name": "register_person", + "args": { + "name": "Jane" + }, + "id": "call_52hL9l50BzPFPruLoPxigoVa", + "type": "tool_call" + }, + { + "name": "locate_person", + "args": { + "name": "Jane" + }, + "id": "call_0NbGZIn9BOCENTfrKvuacarZ", + "type": "tool_call" + } + ] + }, + { + "content": "", + "tool_calls": [ + { + "name": "go_to_location", + "args": { + "description": "living room" + }, + "id": "call_XoErhAixvK31yHOeCKiwgrKj", + "type": "tool_call" + } + ] + }, + { + "content": "I have moved to the living room where Jane is located.", + "tool_calls": [] + } + ] +} diff --git a/dimos/agents/fixtures/test_pounce.json b/dimos/agents/fixtures/test_pounce.json index 99e84d003a..4213c12c95 100644 --- a/dimos/agents/fixtures/test_pounce.json +++ b/dimos/agents/fixtures/test_pounce.json @@ -6,32 +6,15 @@ { "name": "execute_sport_command", "args": { - "args": [ - "FrontPounce" - ] + "command_name": "FrontPounce" }, - "id": "call_Ukj6bCAnHQLj28RHRp697blZ", + "id": "call_Z7X0sJfUygGiJUUL67I64eRs", "type": "tool_call" } ] }, { - "content": "", - "tool_calls": [ - { - "name": "speak", - "args": { - "args": [ - "I have successfully performed a front pounce." - ] - }, - "id": "call_FR9DtqEvJ9zSY85qVD2UFrll", - "type": "tool_call" - } - ] - }, - { - "content": "I have successfully performed a front pounce.", + "content": "The robot has successfully performed a \"FrontPounce\" action!", "tool_calls": [] } ] diff --git a/dimos/agents/fixtures/test_prompt.json b/dimos/agents/fixtures/test_prompt.json new file mode 100644 index 0000000000..b924f66f52 --- /dev/null +++ b/dimos/agents/fixtures/test_prompt.json @@ -0,0 +1,8 @@ +{ + "responses": [ + { + "content": "My name is Johnny. How can I assist you today?", + "tool_calls": [] + } + ] +} diff --git a/dimos/agents/fixtures/test_set_gps_travel_points.json b/dimos/agents/fixtures/test_set_gps_travel_points.json index eb5b2a9195..e1392125a1 100644 --- a/dimos/agents/fixtures/test_set_gps_travel_points.json +++ b/dimos/agents/fixtures/test_set_gps_travel_points.json @@ -1,29 +1,36 @@ { "responses": [ { - "content": "I understand you want me to navigate to the specified location. I will set the GPS travel point accordingly.", + "content": "", + "tool_calls": [ + { + "name": "set_gps_travel_points", + "args": {}, + "id": "call_0Onzw4fDoT68BWNfZUiGlIbg", + "type": "tool_call" + } + ] + }, + { + "content": "It seems there was an issue with the initial attempt. Let me try again by providing the correct parameter.", "tool_calls": [ { "name": "set_gps_travel_points", "args": { - "args": [ + "points": [ { "lat": 37.782654, "lon": -122.413273 } ] }, - "id": "call_q6JCCYFuyAjqUgUibJHqcIMD", + "id": "call_b0QRHVc09ZtY8jbQaUsny7Yx", "type": "tool_call" } ] }, { - "content": "I'm on my way to the specified location. Let me know if there is anything else I can assist you with!", - "tool_calls": [] - }, - { - "content": "I've reached the specified location. Do you need any further assistance?", + "content": "The GPS travel point has been successfully set to latitude: 37.782654 and longitude: -122.413273.", "tool_calls": [] } ] diff --git a/dimos/agents/fixtures/test_set_gps_travel_points_multiple.json b/dimos/agents/fixtures/test_set_gps_travel_points_multiple.json index 9d8f7e9e00..2391e81fe1 100644 --- a/dimos/agents/fixtures/test_set_gps_travel_points_multiple.json +++ b/dimos/agents/fixtures/test_set_gps_travel_points_multiple.json @@ -1,12 +1,23 @@ { "responses": [ + { + "content": "", + "tool_calls": [ + { + "name": "set_gps_travel_points", + "args": {}, + "id": "call_JWgodQUZD16l2tjePmTXGX7V", + "type": "tool_call" + } + ] + }, { "content": "", "tool_calls": [ { "name": "set_gps_travel_points", "args": { - "args": [ + "points": [ { "lat": 37.782654, "lon": -122.413273 @@ -21,13 +32,13 @@ } ] }, - "id": "call_Q09MRMEgRnJPBOGZpM0j8sL2", + "id": "call_e5szuuZrTdq6deN8mUM5kusY", "type": "tool_call" } ] }, { - "content": "I've successfully set the travel points and will navigate to them sequentially.", + "content": "The GPS travel points have been successfully set in the specified order.", "tool_calls": [] } ] diff --git a/dimos/agents/fixtures/test_show_your_love.json b/dimos/agents/fixtures/test_show_your_love.json deleted file mode 100644 index 941906e781..0000000000 --- a/dimos/agents/fixtures/test_show_your_love.json +++ /dev/null @@ -1,38 +0,0 @@ -{ - "responses": [ - { - "content": "", - "tool_calls": [ - { - "name": "execute_sport_command", - "args": { - "args": [ - "FingerHeart" - ] - }, - "id": "call_VFp6x9F00FBmiiUiemFWewop", - "type": "tool_call" - } - ] - }, - { - "content": "", - "tool_calls": [ - { - "name": "speak", - "args": { - "args": [ - "Here's a gesture to show you some love!" - ] - }, - "id": "call_WUUmBJ95s9PtVx8YQsmlJ4EU", - "type": "tool_call" - } - ] - }, - { - "content": "Just did a finger heart gesture to show my affection!", - "tool_calls": [] - } - ] -} diff --git a/dimos/agents/fixtures/test_take_a_look_around.json b/dimos/agents/fixtures/test_start_exploration.json similarity index 51% rename from dimos/agents/fixtures/test_take_a_look_around.json rename to dimos/agents/fixtures/test_start_exploration.json index c30fe71017..713e6e2dba 100644 --- a/dimos/agents/fixtures/test_take_a_look_around.json +++ b/dimos/agents/fixtures/test_start_exploration.json @@ -6,17 +6,15 @@ { "name": "start_exploration", "args": { - "args": [ - 10 - ] + "timeout": 10 }, - "id": "call_AMNeD8zTkvyFHKG90DriDPuM", + "id": "call_o5O7xLaI4iayDhszXblOiWVS", "type": "tool_call" } ] }, { - "content": "I have completed a brief exploration of the surroundings. Let me know if there's anything specific you need!", + "content": "I have completed the exploration for 10 seconds. If there's anything specific you would like to do next, please let me know!", "tool_calls": [] } ] diff --git a/dimos/agents/fixtures/test_stop_movement.json b/dimos/agents/fixtures/test_stop_movement.json index b80834213e..f9f10af0e0 100644 --- a/dimos/agents/fixtures/test_stop_movement.json +++ b/dimos/agents/fixtures/test_stop_movement.json @@ -5,16 +5,14 @@ "tool_calls": [ { "name": "stop_movement", - "args": { - "args": null - }, - "id": "call_oAKe9W8s3xRGioZhBJJDOZB1", + "args": {}, + "id": "call_dHP1UE2Bw180bzxPT2wI2Dam", "type": "tool_call" } ] }, { - "content": "I have stopped moving. Let me know if you need anything else!", + "content": "The movement has been stopped.", "tool_calls": [] } ] diff --git a/dimos/agents/fixtures/test_what_do_you_see_in_this_picture.json b/dimos/agents/fixtures/test_what_do_you_see_in_this_picture.json deleted file mode 100644 index 27ac3453bc..0000000000 --- a/dimos/agents/fixtures/test_what_do_you_see_in_this_picture.json +++ /dev/null @@ -1,25 +0,0 @@ -{ - "responses": [ - { - "content": "", - "tool_calls": [ - { - "name": "take_photo", - "args": { - "args": [] - }, - "id": "call_o6ikJtK3vObuEFD6hDtLoyGQ", - "type": "tool_call" - } - ] - }, - { - "content": "I took a photo, but as an AI, I can't see or interpret images. If there's anything specific you need to know, feel free to ask!", - "tool_calls": [] - }, - { - "content": "It looks like a cozy outdoor cafe where people are sitting and enjoying a meal. There are flowers and a nice, sunny ambiance. If you have any specific questions about the image, let me know!", - "tool_calls": [] - } - ] -} diff --git a/dimos/agents/fixtures/test_what_is_your_name.json b/dimos/agents/fixtures/test_what_is_your_name.json deleted file mode 100644 index a74d793b1d..0000000000 --- a/dimos/agents/fixtures/test_what_is_your_name.json +++ /dev/null @@ -1,8 +0,0 @@ -{ - "responses": [ - { - "content": "Hi! My name is Mr. Potato. How can I assist you today?", - "tool_calls": [] - } - ] -} diff --git a/dimos/agents/fixtures/test_where_am_i.json b/dimos/agents/fixtures/test_where_am_i.json index 2d274f8fa6..a4a06a5c72 100644 --- a/dimos/agents/fixtures/test_where_am_i.json +++ b/dimos/agents/fixtures/test_where_am_i.json @@ -5,16 +5,14 @@ "tool_calls": [ { "name": "where_am_i", - "args": { - "args": [] - }, - "id": "call_uRJLockZ5JWtGWbsSL1dpHm3", + "args": {}, + "id": "call_4eiRtfr8mI7SgLfR0FTjH7Pp", "type": "tool_call" } ] }, { - "content": "You are on Bourbon Street.", + "content": "You are currently on Bourbon Street.", "tool_calls": [] } ] diff --git a/dimos/agents/llm_init.py b/dimos/agents/llm_init.py deleted file mode 100644 index eb8c33c631..0000000000 --- a/dimos/agents/llm_init.py +++ /dev/null @@ -1,62 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from typing import cast - -from langchain.chat_models import init_chat_model -from langchain_core.language_models.chat_models import BaseChatModel -from langchain_core.messages import SystemMessage -from langchain_huggingface import ChatHuggingFace, HuggingFacePipeline - -from dimos.agents.ollama_agent import ensure_ollama_model -from dimos.agents.spec import AgentConfig -from dimos.agents.system_prompt import SYSTEM_PROMPT - - -def build_llm(config: AgentConfig) -> BaseChatModel: - if config.model_instance: - return config.model_instance - - if config.provider.value.lower() == "ollama": - ensure_ollama_model(config.model) - - if config.provider.value.lower() == "huggingface": - llm = HuggingFacePipeline.from_model_id( - model_id=config.model, - task="text-generation", - pipeline_kwargs={ - "max_new_tokens": 512, - "temperature": 0.7, - }, - ) - return ChatHuggingFace(llm=llm, model_id=config.model) - - return cast( - "BaseChatModel", - init_chat_model( # type: ignore[call-overload] - model_provider=config.provider, - model=config.model, - ), - ) - - -def build_system_message(config: AgentConfig, *, append: str = "") -> SystemMessage: - if config.system_prompt: - if isinstance(config.system_prompt, str): - return SystemMessage(config.system_prompt + append) - if append: - config.system_prompt.content += append # type: ignore[operator] - return config.system_prompt - - return SystemMessage(SYSTEM_PROMPT + append) diff --git a/dimos/agents/skills/conftest.py b/dimos/agents/skills/conftest.py deleted file mode 100644 index ae5566b9f9..0000000000 --- a/dimos/agents/skills/conftest.py +++ /dev/null @@ -1,113 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from functools import partial - -import pytest -from reactivex.scheduler import ThreadPoolScheduler - -from dimos.agents.skills.google_maps_skill_container import GoogleMapsSkillContainer -from dimos.agents.skills.gps_nav_skill import GpsNavSkillContainer -from dimos.agents.skills.navigation import NavigationSkillContainer -from dimos.agents.system_prompt import SYSTEM_PROMPT -from dimos.robot.unitree.unitree_skill_container import UnitreeSkillContainer - - -@pytest.fixture(autouse=True) -def cleanup_threadpool_scheduler(monkeypatch): - # TODO: get rid of this global threadpool - """Clean up and recreate the global ThreadPoolScheduler after each test.""" - # Disable ChromaDB telemetry to avoid leaking threads - monkeypatch.setenv("CHROMA_ANONYMIZED_TELEMETRY", "False") - yield - from dimos.utils import threadpool - - # Shutdown the global scheduler's executor - threadpool.scheduler.executor.shutdown(wait=True) - # Recreate it for the next test - threadpool.scheduler = ThreadPoolScheduler(max_workers=threadpool.get_max_workers()) - - -@pytest.fixture -def navigation_skill_container(mocker): - container = NavigationSkillContainer() - container.color_image.connection = mocker.MagicMock() - container.odom.connection = mocker.MagicMock() - container.start() - yield container - container.stop() - - -@pytest.fixture -def gps_nav_skill_container(mocker): - container = GpsNavSkillContainer() - container.gps_location.connection = mocker.MagicMock() - container.gps_goal = mocker.MagicMock() - container.start() - yield container - container.stop() - - -@pytest.fixture -def google_maps_skill_container(mocker): - container = GoogleMapsSkillContainer() - container.gps_location.connection = mocker.MagicMock() - container.start() - container._client = mocker.MagicMock() - yield container - container.stop() - - -@pytest.fixture -def unitree_skills(mocker): - container = UnitreeSkillContainer() - container.start() - yield container - container.stop() - - -@pytest.fixture -def create_navigation_agent(navigation_skill_container, create_fake_agent): - return partial( - create_fake_agent, - system_prompt=SYSTEM_PROMPT, - skill_containers=[navigation_skill_container], - ) - - -@pytest.fixture -def create_gps_nav_agent(gps_nav_skill_container, create_fake_agent): - return partial( - create_fake_agent, system_prompt=SYSTEM_PROMPT, skill_containers=[gps_nav_skill_container] - ) - - -@pytest.fixture -def create_google_maps_agent( - gps_nav_skill_container, google_maps_skill_container, create_fake_agent -): - return partial( - create_fake_agent, - system_prompt=SYSTEM_PROMPT, - skill_containers=[gps_nav_skill_container, google_maps_skill_container], - ) - - -@pytest.fixture -def create_unitree_skills_agent(unitree_skills, create_fake_agent): - return partial( - create_fake_agent, - system_prompt=SYSTEM_PROMPT, - skill_containers=[unitree_skills], - ) diff --git a/dimos/agents/skills/demo_calculator_skill.py b/dimos/agents/skills/demo_calculator_skill.py index 2ed8050ca5..61d66e301a 100644 --- a/dimos/agents/skills/demo_calculator_skill.py +++ b/dimos/agents/skills/demo_calculator_skill.py @@ -12,18 +12,18 @@ # See the License for the specific language governing permissions and # limitations under the License. -from dimos.core.skill_module import SkillModule -from dimos.protocol.skill.skill import skill +from dimos.agents.annotation import skill +from dimos.core.module import Module -class DemoCalculatorSkill(SkillModule): +class DemoCalculatorSkill(Module): def start(self) -> None: super().start() def stop(self) -> None: super().stop() - @skill() + @skill def sum_numbers(self, n1: int, n2: int, *args: int, **kwargs: int) -> str: """This skill adds two numbers. Always use this tool. Never add up numbers yourself. diff --git a/dimos/agents/skills/demo_google_maps_skill.py b/dimos/agents/skills/demo_google_maps_skill.py index cd8cad9d6a..13f2ebc19b 100644 --- a/dimos/agents/skills/demo_google_maps_skill.py +++ b/dimos/agents/skills/demo_google_maps_skill.py @@ -13,20 +13,13 @@ # See the License for the specific language governing permissions and # limitations under the License. -from dotenv import load_dotenv - -from dimos.agents.agent import llm_agent -from dimos.agents.cli.human import human_input +from dimos.agents.agent import agent from dimos.agents.skills.demo_robot import demo_robot from dimos.agents.skills.google_maps_skill_container import google_maps_skill from dimos.core.blueprints import autoconnect -load_dotenv() - - demo_google_maps_skill = autoconnect( demo_robot(), google_maps_skill(), - human_input(), - llm_agent(), + agent(), ) diff --git a/dimos/agents/skills/demo_gps_nav.py b/dimos/agents/skills/demo_gps_nav.py index c8c21ce2fc..7a6abd32dd 100644 --- a/dimos/agents/skills/demo_gps_nav.py +++ b/dimos/agents/skills/demo_gps_nav.py @@ -13,20 +13,13 @@ # See the License for the specific language governing permissions and # limitations under the License. -from dotenv import load_dotenv - -from dimos.agents.agent import llm_agent -from dimos.agents.cli.human import human_input +from dimos.agents.agent import agent from dimos.agents.skills.demo_robot import demo_robot from dimos.agents.skills.gps_nav_skill import gps_nav_skill from dimos.core.blueprints import autoconnect -load_dotenv() - - demo_gps_nav = autoconnect( demo_robot(), gps_nav_skill(), - human_input(), - llm_agent(), + agent(), ) diff --git a/dimos/agents/skills/demo_skill.py b/dimos/agents/skills/demo_skill.py index 547d81c5b8..b067a3fbc2 100644 --- a/dimos/agents/skills/demo_skill.py +++ b/dimos/agents/skills/demo_skill.py @@ -13,18 +13,11 @@ # See the License for the specific language governing permissions and # limitations under the License. -from dotenv import load_dotenv - -from dimos.agents.agent import llm_agent -from dimos.agents.cli.human import human_input +from dimos.agents.agent import agent from dimos.agents.skills.demo_calculator_skill import demo_calculator_skill from dimos.core.blueprints import autoconnect -load_dotenv() - - demo_skill = autoconnect( demo_calculator_skill(), - human_input(), - llm_agent(), + agent(), ) diff --git a/dimos/agents/skills/google_maps_skill_container.py b/dimos/agents/skills/google_maps_skill_container.py index d5a30904ed..33b2ee9f10 100644 --- a/dimos/agents/skills/google_maps_skill_container.py +++ b/dimos/agents/skills/google_maps_skill_container.py @@ -15,18 +15,18 @@ import json from typing import Any +from dimos.agents.annotation import skill from dimos.core.core import rpc -from dimos.core.skill_module import SkillModule +from dimos.core.module import Module from dimos.core.stream import In from dimos.mapping.google_maps.google_maps import GoogleMaps from dimos.mapping.types import LatLon -from dimos.protocol.skill.skill import skill from dimos.utils.logging_config import setup_logger logger = setup_logger() -class GoogleMapsSkillContainer(SkillModule): +class GoogleMapsSkillContainer(Module): _latest_location: LatLon | None = None _client: GoogleMaps @@ -55,7 +55,7 @@ def _get_latest_location(self) -> LatLon: raise ValueError("The position has not been set yet.") return self._latest_location - @skill() + @skill def where_am_i(self, context_radius: int = 200) -> str: """This skill returns information about what street/locality/city/etc you are in. It also gives you nearby landmarks. @@ -81,7 +81,7 @@ def where_am_i(self, context_radius: int = 200) -> str: return result.model_dump_json() - @skill() + @skill def get_gps_position_for_queries(self, queries: list[str]) -> str: """Get the GPS position (latitude/longitude) from Google Maps for know landmarks or searchable locations. This includes anything that wouldn't be viewable on a physical OSM map including intersections (5th and Natoma) diff --git a/dimos/agents/skills/gps_nav_skill.py b/dimos/agents/skills/gps_nav_skill.py index c7325a5b64..721119f6e6 100644 --- a/dimos/agents/skills/gps_nav_skill.py +++ b/dimos/agents/skills/gps_nav_skill.py @@ -14,19 +14,19 @@ import json +from dimos.agents.annotation import skill from dimos.core.core import rpc +from dimos.core.module import Module from dimos.core.rpc_client import RpcCall -from dimos.core.skill_module import SkillModule from dimos.core.stream import In, Out from dimos.mapping.types import LatLon from dimos.mapping.utils.distance import distance_in_meters -from dimos.protocol.skill.skill import skill from dimos.utils.logging_config import setup_logger logger = setup_logger() -class GpsNavSkillContainer(SkillModule): +class GpsNavSkillContainer(Module): _latest_location: LatLon | None = None _max_valid_distance: int = 50000 _set_gps_travel_goal_points: RpcCall | None = None @@ -59,8 +59,8 @@ def _get_latest_location(self) -> LatLon: raise ValueError("The position has not been set yet.") return self._latest_location - @skill() - def set_gps_travel_points(self, *points: dict[str, float]) -> str: + @skill + def set_gps_travel_points(self, points: list[dict[str, float]]) -> str: """Define the movement path determined by GPS coordinates. Requires at least one. You can get the coordinates by using the `get_gps_position_for_queries` skill. Example: diff --git a/dimos/agents/skills/navigation.py b/dimos/agents/skills/navigation.py index 054246d6ee..a5828a6ee3 100644 --- a/dimos/agents/skills/navigation.py +++ b/dimos/agents/skills/navigation.py @@ -15,8 +15,11 @@ import time from typing import Any +from reactivex.disposable import Disposable + +from dimos.agents.annotation import skill from dimos.core.core import rpc -from dimos.core.skill_module import SkillModule +from dimos.core.module import Module from dimos.core.stream import In from dimos.models.qwen.video_query import BBox from dimos.models.vl.qwen import QwenVlModel @@ -25,14 +28,13 @@ from dimos.msgs.sensor_msgs import Image from dimos.navigation.base import NavigationState from dimos.navigation.visual.query import get_object_bbox_from_image -from dimos.protocol.skill.skill import skill from dimos.types.robot_location import RobotLocation from dimos.utils.logging_config import setup_logger logger = setup_logger() -class NavigationSkillContainer(SkillModule): +class NavigationSkillContainer(Module): _latest_image: Image | None = None _latest_odom: PoseStamped | None = None _skill_started: bool = False @@ -64,8 +66,8 @@ def __init__(self) -> None: @rpc def start(self) -> None: - self._disposables.add(self.color_image.subscribe(self._on_color_image)) # type: ignore[arg-type] - self._disposables.add(self.odom.subscribe(self._on_odom)) # type: ignore[arg-type] + self._disposables.add(Disposable(self.color_image.subscribe(self._on_color_image))) + self._disposables.add(Disposable(self.odom.subscribe(self._on_odom))) self._skill_started = True @rpc @@ -78,7 +80,7 @@ def _on_color_image(self, image: Image) -> None: def _on_odom(self, odom: PoseStamped) -> None: self._latest_odom = odom - @skill() + @skill def tag_location(self, location_name: str) -> str: """Tag this location in the spatial memory with a name. @@ -113,7 +115,7 @@ def tag_location(self, location_name: str) -> str: logger.info(f"Tagged {location}") return f"Tagged '{location_name}': ({position.x},{position.y})." - @skill() + @skill def navigate_with_text(self, query: str) -> str: """Navigate to a location by querying the existing semantic map using natural language. @@ -297,12 +299,12 @@ def _navigate_using_semantic_map(self, query: str) -> str: return f"Successfuly arrived at '{query}'" - @skill() + @skill def follow_human(self, person: str) -> str: """Follow a specific person""" return "Not implemented yet." - @skill() + @skill def stop_movement(self) -> str: """Immediatly stop moving.""" @@ -329,7 +331,7 @@ def _cancel_goal_and_stop(self) -> None: cancel_goal_rpc() return stop_exploration_rpc() # type: ignore[no-any-return] - @skill() + @skill def start_exploration(self, timeout: float = 240.0) -> str: """A skill that performs autonomous frontier exploration. diff --git a/dimos/agents/skills/osm.py b/dimos/agents/skills/osm.py index 71f453069f..9bb731cf72 100644 --- a/dimos/agents/skills/osm.py +++ b/dimos/agents/skills/osm.py @@ -13,19 +13,19 @@ # limitations under the License. -from dimos.core.skill_module import SkillModule +from dimos.agents.annotation import skill +from dimos.core.module import Module from dimos.core.stream import In from dimos.mapping.osm.current_location_map import CurrentLocationMap from dimos.mapping.types import LatLon from dimos.mapping.utils.distance import distance_in_meters from dimos.models.vl.qwen import QwenVlModel -from dimos.protocol.skill.skill import skill from dimos.utils.logging_config import setup_logger logger = setup_logger() -class OsmSkill(SkillModule): +class OsmSkill(Module): _latest_location: LatLon | None _current_location_map: CurrentLocationMap @@ -46,7 +46,7 @@ def stop(self) -> None: def _on_gps_location(self, location: LatLon) -> None: self._latest_location = location - @skill() + @skill def map_query(self, query_sentence: str) -> str: """This skill uses a vision language model to find something on the map based on the query sentence. You can query it with something like "Where diff --git a/dimos/agents/skills/person_follow.py b/dimos/agents/skills/person_follow.py index 91ba5d01b8..6c5b7e29d5 100644 --- a/dimos/agents/skills/person_follow.py +++ b/dimos/agents/skills/person_follow.py @@ -19,9 +19,10 @@ import numpy as np from reactivex.disposable import Disposable +from dimos.agents.annotation import skill from dimos.core.core import rpc from dimos.core.global_config import GlobalConfig -from dimos.core.skill_module import SkillModule +from dimos.core.module import Module from dimos.core.stream import In, Out from dimos.models.qwen.video_query import BBox from dimos.models.segmentation.edge_tam import EdgeTAMProcessor @@ -31,7 +32,6 @@ from dimos.navigation.visual.query import get_object_bbox_from_image from dimos.navigation.visual_servoing.detection_navigation import DetectionNavigation from dimos.navigation.visual_servoing.visual_servoing_2d import VisualServoing2D -from dimos.protocol.skill.skill import skill from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: @@ -40,7 +40,7 @@ logger = setup_logger() -class PersonFollowSkillContainer(SkillModule): +class PersonFollowSkillContainer(Module): """Skill container for following a person. This skill uses: @@ -102,7 +102,7 @@ def stop(self) -> None: self._vl_model.stop() super().stop() - @skill() + @skill def follow_person(self, query: str) -> str: """Follow a person matching the given description using visual servoing. @@ -141,7 +141,7 @@ def follow_person(self, query: str) -> str: return self._follow_loop(query, initial_bbox) - @skill() + @skill def stop_following(self) -> str: """Stop following the current person. diff --git a/dimos/agents/skills/speak_skill.py b/dimos/agents/skills/speak_skill.py index 073dda656a..aa06d30ba4 100644 --- a/dimos/agents/skills/speak_skill.py +++ b/dimos/agents/skills/speak_skill.py @@ -17,9 +17,9 @@ from reactivex import Subject +from dimos.agents.annotation import skill from dimos.core.core import rpc -from dimos.core.skill_module import SkillModule -from dimos.protocol.skill.skill import skill +from dimos.core.module import Module from dimos.stream.audio.node_output import SounddeviceAudioOutput from dimos.stream.audio.tts.node_openai import OpenAITTSNode, Voice from dimos.utils.logging_config import setup_logger @@ -27,7 +27,7 @@ logger = setup_logger() -class SpeakSkill(SkillModule): +class SpeakSkill(Module): _tts_node: OpenAITTSNode | None = None _audio_output: SounddeviceAudioOutput | None = None _audio_lock: threading.Lock = threading.Lock() @@ -49,7 +49,7 @@ def stop(self) -> None: self._audio_output = None super().stop() - @skill() + @skill def speak(self, text: str) -> str: """Speak text out loud through the robot's speakers. diff --git a/dimos/agents/skills/test_google_maps_skill_container.py b/dimos/agents/skills/test_google_maps_skill_container.py index 0af206fbb1..84da91e886 100644 --- a/dimos/agents/skills/test_google_maps_skill_container.py +++ b/dimos/agents/skills/test_google_maps_skill_container.py @@ -14,34 +14,83 @@ import re +from langchain_core.messages import HumanMessage +import pytest + +from dimos.agents.skills.google_maps_skill_container import GoogleMapsSkillContainer +from dimos.core.module import Module +from dimos.core.stream import Out from dimos.mapping.google_maps.types import Coordinates, LocationContext, Position from dimos.mapping.types import LatLon -def test_where_am_i(create_google_maps_agent, google_maps_skill_container) -> None: - google_maps_skill_container._latest_location = LatLon(lat=37.782654, lon=-122.413273) - google_maps_skill_container._client.get_location_context.return_value = LocationContext( - street="Bourbon Street", coordinates=Coordinates(lat=37.782654, lon=-122.413273) - ) - agent = create_google_maps_agent(fixture="test_where_am_i.json") +class FakeGPS(Module): + """Provides a gps_location output so GoogleMapsSkillContainer's input port gets a transport.""" + + gps_location: Out[LatLon] + + +class FakeLocationClient: + def get_location_context(self, location, radius=200): + return LocationContext( + street="Bourbon Street", + coordinates=Coordinates(lat=37.782654, lon=-122.413273), + ) + + +class MockedWhereAmISkill(GoogleMapsSkillContainer): + def __init__(self): + Module.__init__(self) # Skip GoogleMapsSkillContainer's __init__. + self._client = FakeLocationClient() + self._latest_location = LatLon(lat=37.782654, lon=-122.413273) + self._started = True + self._max_valid_distance = 20000 - response = agent.query("what street am I on") - assert "bourbon" in response.lower() +class FakePositionClient: + def __init__(self): + self._positions = iter( + [ + Position(lat=37.782601, lon=-122.413201, description="address 1"), + Position(lat=37.782602, lon=-122.413202, description="address 2"), + Position(lat=37.782603, lon=-122.413203, description="address 3"), + ] + ) + def get_position(self, query, location): + return next(self._positions) -def test_get_gps_position_for_queries( - create_google_maps_agent, google_maps_skill_container -) -> None: - google_maps_skill_container._latest_location = LatLon(lat=37.782654, lon=-122.413273) - google_maps_skill_container._client.get_position.side_effect = [ - Position(lat=37.782601, lon=-122.413201, description="address 1"), - Position(lat=37.782602, lon=-122.413202, description="address 2"), - Position(lat=37.782603, lon=-122.413203, description="address 3"), - ] - agent = create_google_maps_agent(fixture="test_get_gps_position_for_queries.json") - response = agent.query("what are the lat/lon for hyde park, regent park, russell park?") +class MockedPositionSkill(GoogleMapsSkillContainer): + def __init__(self): + Module.__init__(self) + self._client = FakePositionClient() + self._latest_location = LatLon(lat=37.782654, lon=-122.413273) + self._started = True + self._max_valid_distance = 20000 + + +@pytest.mark.integration +def test_where_am_i(agent_setup) -> None: + history = agent_setup( + blueprints=[FakeGPS.blueprint(), MockedWhereAmISkill.blueprint()], + messages=[HumanMessage("What street am I on? Use the where_am_i tool.")], + ) + + assert "bourbon" in history[-1].content.lower() + + +@pytest.mark.integration +def test_get_gps_position_for_queries(agent_setup) -> None: + history = agent_setup( + blueprints=[FakeGPS.blueprint(), MockedPositionSkill.blueprint()], + messages=[ + HumanMessage( + "What are the lat/lon for hyde park, regent park, russell park? " + "Use the get_gps_position_for_queries tool." + ) + ], + ) regex = r".*37\.782601.*122\.413201.*37\.782602.*122\.413202.*37\.782603.*122\.413203.*" - assert re.match(regex, response, re.DOTALL) + assert re.match(regex, history[-1].content, re.DOTALL) diff --git a/dimos/agents/skills/test_gps_nav_skills.py b/dimos/agents/skills/test_gps_nav_skills.py index ab0d1ec318..afcb4d36d0 100644 --- a/dimos/agents/skills/test_gps_nav_skills.py +++ b/dimos/agents/skills/test_gps_nav_skills.py @@ -12,47 +12,57 @@ # See the License for the specific language governing permissions and # limitations under the License. +from langchain_core.messages import HumanMessage +import pytest +from dimos.agents.skills.gps_nav_skill import GpsNavSkillContainer +from dimos.core.module import Module +from dimos.core.stream import Out from dimos.mapping.types import LatLon -def test_set_gps_travel_points(create_gps_nav_agent, gps_nav_skill_container, mocker) -> None: - gps_nav_skill_container._latest_location = LatLon(lat=37.782654, lon=-122.413273) - gps_nav_skill_container._set_gps_travel_goal_points = mocker.Mock() - agent = create_gps_nav_agent(fixture="test_set_gps_travel_points.json") +class FakeGPS(Module): + """Provides a gps_location output so GpsNavSkillContainer's input port gets a transport.""" - agent.query("go to lat: 37.782654, lon: -122.413273") + gps_location: Out[LatLon] - gps_nav_skill_container._set_gps_travel_goal_points.assert_called_once_with( - [LatLon(lat=37.782654, lon=-122.413273)] - ) - gps_nav_skill_container.gps_goal.publish.assert_called_once_with( - [LatLon(lat=37.782654, lon=-122.413273)] - ) +class MockedGpsNavSkill(GpsNavSkillContainer): + def __init__(self): + Module.__init__(self) + self._latest_location = LatLon(lat=37.782654, lon=-122.413273) + self._started = True + self._max_valid_distance = 50000 -def test_set_gps_travel_points_multiple( - create_gps_nav_agent, gps_nav_skill_container, mocker -) -> None: - gps_nav_skill_container._latest_location = LatLon(lat=37.782654, lon=-122.413273) - gps_nav_skill_container._set_gps_travel_goal_points = mocker.Mock() - agent = create_gps_nav_agent(fixture="test_set_gps_travel_points_multiple.json") - agent.query( - "go to lat: 37.782654, lon: -122.413273, then 37.782660,-122.413260, and then 37.782670,-122.413270" +@pytest.mark.integration +def test_set_gps_travel_points(agent_setup) -> None: + history = agent_setup( + blueprints=[FakeGPS.blueprint(), MockedGpsNavSkill.blueprint()], + messages=[ + HumanMessage( + 'Set GPS travel points to [{"lat": 37.782654, "lon": -122.413273}]. ' + "Use the set_gps_travel_points tool." + ) + ], ) - gps_nav_skill_container._set_gps_travel_goal_points.assert_called_once_with( - [ - LatLon(lat=37.782654, lon=-122.413273), - LatLon(lat=37.782660, lon=-122.413260), - LatLon(lat=37.782670, lon=-122.413270), - ] - ) - gps_nav_skill_container.gps_goal.publish.assert_called_once_with( - [ - LatLon(lat=37.782654, lon=-122.413273), - LatLon(lat=37.782660, lon=-122.413260), - LatLon(lat=37.782670, lon=-122.413270), - ] + assert "success" in history[-1].content.lower() + + +@pytest.mark.integration +def test_set_gps_travel_points_multiple(agent_setup) -> None: + history = agent_setup( + blueprints=[FakeGPS.blueprint(), MockedGpsNavSkill.blueprint()], + messages=[ + HumanMessage( + "Set GPS travel points to these locations in order: " + '{"lat": 37.782654, "lon": -122.413273}, ' + '{"lat": 37.782660, "lon": -122.413260}, ' + '{"lat": 37.782670, "lon": -122.413270}. ' + "Use the set_gps_travel_points tool." + ) + ], ) + + assert "success" in history[-1].content.lower() diff --git a/dimos/agents/skills/test_navigation.py b/dimos/agents/skills/test_navigation.py index 67e0429cb5..91737ada77 100644 --- a/dimos/agents/skills/test_navigation.py +++ b/dimos/agents/skills/test_navigation.py @@ -12,84 +12,105 @@ # See the License for the specific language governing permissions and # limitations under the License. +from langchain_core.messages import HumanMessage import pytest -from dimos.msgs.geometry_msgs import PoseStamped, Vector3 -from dimos.utils.transform_utils import euler_to_quaternion +from dimos.agents.skills.navigation import NavigationSkillContainer +from dimos.core.module import Module +from dimos.core.stream import Out +from dimos.msgs.geometry_msgs import PoseStamped +from dimos.msgs.sensor_msgs import Image -def test_stop_movement(create_navigation_agent, navigation_skill_container, mocker) -> None: - cancel_goal_mock = mocker.Mock() - stop_exploration_mock = mocker.Mock() - navigation_skill_container._bound_rpc_calls["NavigationInterface.cancel_goal"] = ( - cancel_goal_mock - ) - navigation_skill_container._bound_rpc_calls["WavefrontFrontierExplorer.stop_exploration"] = ( - stop_exploration_mock - ) - agent = create_navigation_agent(fixture="test_stop_movement.json") +class FakeCamera(Module): + color_image: Out[Image] + + +class FakeOdom(Module): + odom: Out[PoseStamped] + + +class MockedStopNavSkill(NavigationSkillContainer): + rpc_calls: list[str] = [] + + def __init__(self): + Module.__init__(self) + self._skill_started = True + + def _cancel_goal_and_stop(self): + pass + + +class MockedExploreNavSkill(NavigationSkillContainer): + rpc_calls: list[str] = [] + + def __init__(self): + Module.__init__(self) + self._skill_started = True + + def _start_exploration(self, timeout): + return "Exploration completed successfuly" + + def _cancel_goal_and_stop(self): + pass + - agent.query("stop") +class MockedSemanticNavSkill(NavigationSkillContainer): + rpc_calls: list[str] = [] - cancel_goal_mock.assert_called_once_with() - stop_exploration_mock.assert_called_once_with() + def __init__(self): + Module.__init__(self) + self._skill_started = True + + def _navigate_by_tagged_location(self, query): + return None + + def _navigate_to_object(self, query): + return None + + def _navigate_using_semantic_map(self, query): + return f"Successfuly arrived at '{query}'" @pytest.mark.integration -def test_take_a_look_around(create_navigation_agent, navigation_skill_container, mocker) -> None: - explore_mock = mocker.Mock() - is_exploration_active_mock = mocker.Mock() - navigation_skill_container._bound_rpc_calls["WavefrontFrontierExplorer.explore"] = explore_mock - navigation_skill_container._bound_rpc_calls[ - "WavefrontFrontierExplorer.is_exploration_active" - ] = is_exploration_active_mock - mocker.patch("dimos.agents.skills.navigation.time.sleep") - agent = create_navigation_agent(fixture="test_take_a_look_around.json") - - agent.query("take a look around for 10 seconds") - - explore_mock.assert_called_once_with() - - -def test_go_to_semantic_location( - create_navigation_agent, navigation_skill_container, mocker -) -> None: - mocker.patch( - "dimos.agents.skills.navigation.NavigationSkillContainer._navigate_by_tagged_location", - return_value=None, - ) - mocker.patch( - "dimos.agents.skills.navigation.NavigationSkillContainer._navigate_to_object", - return_value=None, +def test_stop_movement(agent_setup) -> None: + history = agent_setup( + blueprints=[ + FakeCamera.blueprint(), + FakeOdom.blueprint(), + MockedStopNavSkill.blueprint(), + ], + messages=[HumanMessage("Stop moving. Use the stop_movement tool.")], ) - navigate_to_mock = mocker.patch( - "dimos.agents.skills.navigation.NavigationSkillContainer._navigate_to", - return_value=True, - ) - query_by_text_mock = mocker.Mock( - return_value=[ - { - "distance": 0.5, - "metadata": [ - { - "pos_x": 1, - "pos_y": 2, - "rot_z": 3, - } - ], - } - ] + + assert "stopped" in history[-1].content.lower() + + +@pytest.mark.integration +def test_start_exploration(agent_setup) -> None: + history = agent_setup( + blueprints=[ + FakeCamera.blueprint(), + FakeOdom.blueprint(), + MockedExploreNavSkill.blueprint(), + ], + messages=[ + HumanMessage("Take a look around for 10 seconds. Use the start_exploration tool.") + ], ) - navigation_skill_container._bound_rpc_calls["SpatialMemory.query_by_text"] = query_by_text_mock - agent = create_navigation_agent(fixture="test_go_to_semantic_location.json") - - agent.query("go to the bookshelf") - - query_by_text_mock.assert_called_once_with("bookshelf") - navigate_to_mock.assert_called_once_with( - PoseStamped( - position=Vector3(1, 2, 0), - orientation=euler_to_quaternion(Vector3(0, 0, 3)), - frame_id="world", - ), + + assert "explor" in history[-1].content.lower() + + +@pytest.mark.integration +def test_go_to_semantic_location(agent_setup) -> None: + history = agent_setup( + blueprints=[ + FakeCamera.blueprint(), + FakeOdom.blueprint(), + MockedSemanticNavSkill.blueprint(), + ], + messages=[HumanMessage("Go to the bookshelf. Use the navigate_with_text tool.")], ) + + assert "success" in history[-1].content.lower() diff --git a/dimos/agents/skills/test_unitree_skill_container.py b/dimos/agents/skills/test_unitree_skill_container.py index 29dfade979..ea1cfba5cf 100644 --- a/dimos/agents/skills/test_unitree_skill_container.py +++ b/dimos/agents/skills/test_unitree_skill_container.py @@ -12,21 +12,35 @@ # See the License for the specific language governing permissions and # limitations under the License. +import difflib -def test_pounce(mocker, create_unitree_skills_agent, unitree_skills) -> None: - agent = create_unitree_skills_agent(fixture="test_pounce.json") - publish_request_mock = mocker.Mock() - unitree_skills.get_rpc_calls = mocker.Mock(return_value=publish_request_mock) +from langchain_core.messages import HumanMessage +import pytest - response = agent.query("pounce") +from dimos.robot.unitree.unitree_skill_container import _UNITREE_COMMANDS, UnitreeSkillContainer - assert "front pounce" in response.lower() - publish_request_mock.assert_called_once_with("rt/api/sport/request", {"api_id": 1032}) +class MockedUnitreeSkill(UnitreeSkillContainer): + rpc_calls: list[str] = [] -def test_did_you_mean(mocker, unitree_skills) -> None: - unitree_skills.get_rpc_calls = mocker.Mock() - assert ( - unitree_skills.execute_sport_command("Pounce") - == "There's no 'Pounce' command. Did you mean: ['FrontPounce', 'Pose']" + def __init__(self): + super().__init__() + # Provide a fake RPC so the real execute_sport_command runs end-to-end. + self._bound_rpc_calls["GO2Connection.publish_request"] = lambda *args, **kwargs: None + + +@pytest.mark.integration +def test_pounce(agent_setup) -> None: + history = agent_setup( + blueprints=[MockedUnitreeSkill.blueprint()], + messages=[HumanMessage("Pounce! Use the execute_sport_command tool.")], ) + + response = history[-1].content.lower() + assert "pounce" in response + + +def test_did_you_mean() -> None: + suggestions = difflib.get_close_matches("Pounce", _UNITREE_COMMANDS.keys(), n=3, cutoff=0.6) + assert "FrontPounce" in suggestions + assert "Pose" in suggestions diff --git a/dimos/agents/spec.py b/dimos/agents/spec.py deleted file mode 100644 index 02eef359de..0000000000 --- a/dimos/agents/spec.py +++ /dev/null @@ -1,264 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Base agent module that wraps BaseAgent for DimOS module usage.""" - -from abc import ABC, abstractmethod -from dataclasses import dataclass, field -from enum import Enum -from typing import TYPE_CHECKING, Any, Union - -if TYPE_CHECKING: - from dimos.protocol.skill.skill import SkillContainer - -from langchain_core.language_models.chat_models import BaseChatModel -from langchain_core.messages import ( - AIMessage, - HumanMessage, - SystemMessage, - ToolMessage, -) -from rich.console import Console -from rich.table import Table -from rich.text import Text - -from dimos.core import Module, rpc -from dimos.core.module import ModuleConfig -from dimos.protocol.pubsub import PubSub, lcm # type: ignore[attr-defined] -from dimos.protocol.service import Service # type: ignore[attr-defined] -from dimos.protocol.skill.skill import SkillContainer -from dimos.utils.generic import truncate_display_string -from dimos.utils.logging_config import setup_logger - -logger = setup_logger() - - -# FIXME: I dont see a stable (and dynamic) way to get these, this is only for type-hints and Paul's PR should replace this in a couple of days (this is a stop-gap change to get CI passing) -_providers = [ - "ANTHROPIC", - "AZURE_AI", - "AZURE_OPENAI", - "BEDROCK", - "BEDROCK_CONVERSE", - "COHERE", - "DEEPSEEK", - "FIREWORKS", - "GOOGLE_ANTHROPIC_VERTEX", - "GOOGLE_GENAI", - "GOOGLE_VERTEXAI", - "GROQ", - "HUGGINGFACE", - "IBM", - "MISTRALAI", - "NVIDIA", - "OLLAMA", - "OPENAI", - "PERPLEXITY", - "TOGETHER", - "UPSTAGE", - "XAI", -] -Provider = Enum("Provider", _providers, type=str) # type: ignore[misc] - - -class Model(str, Enum): - """Common model names across providers. - - Note: This is not exhaustive as model names change frequently. - Based on langchain's _attempt_infer_model_provider patterns. - """ - - # OpenAI models (prefix: gpt-3, gpt-4, o1, o3) - GPT_4O = "gpt-4o" - GPT_4O_MINI = "gpt-4o-mini" - GPT_4_TURBO = "gpt-4-turbo" - GPT_4_TURBO_PREVIEW = "gpt-4-turbo-preview" - GPT_4 = "gpt-4" - GPT_35_TURBO = "gpt-3.5-turbo" - GPT_35_TURBO_16K = "gpt-3.5-turbo-16k" - O1_PREVIEW = "o1-preview" - O1_MINI = "o1-mini" - O3_MINI = "o3-mini" - - # Anthropic models (prefix: claude) - CLAUDE_3_OPUS = "claude-3-opus-20240229" - CLAUDE_3_SONNET = "claude-3-sonnet-20240229" - CLAUDE_3_HAIKU = "claude-3-haiku-20240307" - CLAUDE_35_SONNET = "claude-3-5-sonnet-20241022" - CLAUDE_35_SONNET_LATEST = "claude-3-5-sonnet-latest" - CLAUDE_3_7_SONNET = "claude-3-7-sonnet-20250219" - - # Google models (prefix: gemini) - GEMINI_20_FLASH = "gemini-2.0-flash" - GEMINI_15_PRO = "gemini-1.5-pro" - GEMINI_15_FLASH = "gemini-1.5-flash" - GEMINI_10_PRO = "gemini-1.0-pro" - - # Amazon Bedrock models (prefix: amazon) - AMAZON_TITAN_EXPRESS = "amazon.titan-text-express-v1" - AMAZON_TITAN_LITE = "amazon.titan-text-lite-v1" - - # Cohere models (prefix: command) - COMMAND_R_PLUS = "command-r-plus" - COMMAND_R = "command-r" - COMMAND = "command" - COMMAND_LIGHT = "command-light" - - # Fireworks models (prefix: accounts/fireworks) - FIREWORKS_LLAMA_V3_70B = "accounts/fireworks/models/llama-v3-70b-instruct" - FIREWORKS_MIXTRAL_8X7B = "accounts/fireworks/models/mixtral-8x7b-instruct" - - # Mistral models (prefix: mistral) - MISTRAL_LARGE = "mistral-large" - MISTRAL_MEDIUM = "mistral-medium" - MISTRAL_SMALL = "mistral-small" - MIXTRAL_8X7B = "mixtral-8x7b" - MIXTRAL_8X22B = "mixtral-8x22b" - MISTRAL_7B = "mistral-7b" - - # DeepSeek models (prefix: deepseek) - DEEPSEEK_CHAT = "deepseek-chat" - DEEPSEEK_CODER = "deepseek-coder" - DEEPSEEK_R1_DISTILL_LLAMA_70B = "deepseek-r1-distill-llama-70b" - - # xAI models (prefix: grok) - GROK_1 = "grok-1" - GROK_2 = "grok-2" - - # Perplexity models (prefix: sonar) - SONAR_SMALL_CHAT = "sonar-small-chat" - SONAR_MEDIUM_CHAT = "sonar-medium-chat" - SONAR_LARGE_CHAT = "sonar-large-chat" - - # Meta Llama models (various providers) - LLAMA_3_70B = "llama-3-70b" - LLAMA_3_8B = "llama-3-8b" - LLAMA_31_70B = "llama-3.1-70b" - LLAMA_31_8B = "llama-3.1-8b" - LLAMA_33_70B = "llama-3.3-70b" - LLAMA_2_70B = "llama-2-70b" - LLAMA_2_13B = "llama-2-13b" - LLAMA_2_7B = "llama-2-7b" - - -@dataclass -class AgentConfig(ModuleConfig): - system_prompt: str | SystemMessage | None = None - skills: SkillContainer | list[SkillContainer] | None = None - - # we can provide model/provvider enums or instantiated model_instance - model: Model = Model.GPT_4O - provider: Provider = Provider.OPENAI # type: ignore[attr-defined] - model_instance: BaseChatModel | None = None - - agent_transport: type[PubSub] = lcm.PickleLCM # type: ignore[type-arg] - agent_topic: Any = field(default_factory=lambda: lcm.Topic("/agent")) - - -AnyMessage = Union[SystemMessage, ToolMessage, AIMessage, HumanMessage] - - -class AgentSpec(Service[AgentConfig], Module, ABC): - default_config: type[AgentConfig] = AgentConfig - - def __init__(self, *args, **kwargs) -> None: # type: ignore[no-untyped-def] - Service.__init__(self, *args, **kwargs) - Module.__init__(self, *args, **kwargs) - - if self.config.agent_transport: - self.transport = self.config.agent_transport() - - def publish(self, msg: AnyMessage) -> None: - if self.transport: - self.transport.publish(self.config.agent_topic, msg) - - def start(self) -> None: - super().start() - - def stop(self) -> None: - if hasattr(self, "transport") and self.transport: - self.transport.stop() # type: ignore[attr-defined] - self.transport = None # type: ignore[assignment] - super().stop() - - @rpc - @abstractmethod - def clear_history(self): ... # type: ignore[no-untyped-def] - - @abstractmethod - def append_history(self, *msgs: list[AIMessage | HumanMessage]): ... # type: ignore[no-untyped-def] - - @abstractmethod - def history(self) -> list[AnyMessage]: ... - - @rpc - @abstractmethod - def register_skills( - self, container: "SkillContainer", run_implicit_name: str | None = None - ) -> None: ... - - @rpc - @abstractmethod - def query(self, query: str): ... # type: ignore[no-untyped-def] - - def __str__(self) -> str: - console = Console(force_terminal=True, legacy_windows=False) - table = Table(show_header=True) - - table.add_column("Message Type", style="cyan", no_wrap=True) - table.add_column("Content") - - for message in self.history(): - if isinstance(message, HumanMessage): - content = message.content - if not isinstance(content, str): - content = "" - - table.add_row(Text("Human", style="green"), Text(content, style="green")) - elif isinstance(message, AIMessage): - if hasattr(message, "metadata") and message.metadata.get("state"): - table.add_row( - Text("State Summary", style="blue"), - Text(message.content, style="blue"), # type: ignore[arg-type] - ) - else: - table.add_row( - Text("Agent", style="magenta"), - Text(message.content, style="magenta"), # type: ignore[arg-type] - ) - - for tool_call in message.tool_calls: - table.add_row( - "Tool Call", - Text( - f"{tool_call.get('name')}({tool_call.get('args')})", - style="bold magenta", - ), - ) - elif isinstance(message, ToolMessage): - table.add_row( - "Tool Response", Text(f"{message.name}() -> {message.content}"), style="red" - ) - elif isinstance(message, SystemMessage): - table.add_row( - "System", Text(truncate_display_string(message.content, 800), style="yellow") - ) - else: - table.add_row("Unknown", str(message)) - - # Render to string with title above - with console.capture() as capture: - console.print(Text(f" Agent ({self._agent_id})", style="bold blue")) # type: ignore[attr-defined] - console.print(table) - return capture.get().strip() diff --git a/dimos/agents/temp/webcam_agent.py b/dimos/agents/temp/webcam_agent.py deleted file mode 100644 index b09ec2e1d8..0000000000 --- a/dimos/agents/temp/webcam_agent.py +++ /dev/null @@ -1,151 +0,0 @@ -#!/usr/bin/env python3 -# 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. - -""" -Run script for Unitree Go2 robot with agents framework. -This is the migrated version using the new LangChain-based agent system. -""" - -from threading import Thread -import time - -import reactivex as rx -import reactivex.operators as ops - -from dimos.agents import Agent, Output, Reducer, Stream, skill # type: ignore[attr-defined] -from dimos.agents.cli.human import HumanInput -from dimos.agents.spec import Model, Provider -from dimos.core import LCMTransport, Module, rpc, start -from dimos.hardware.sensors.camera import zed -from dimos.hardware.sensors.camera.module import CameraModule -from dimos.hardware.sensors.camera.webcam import Webcam -from dimos.msgs.geometry_msgs import Quaternion, Transform, Vector3 -from dimos.msgs.sensor_msgs import CameraInfo, Image -from dimos.protocol.skill.test_coordinator import SkillContainerTest -from dimos.web.robot_web_interface import RobotWebInterface - - -class WebModule(Module): - web_interface: RobotWebInterface = None # type: ignore[assignment] - human_query: rx.subject.Subject = None # type: ignore[assignment, type-arg] - agent_response: rx.subject.Subject = None # type: ignore[assignment, type-arg] - - thread: Thread = None # type: ignore[assignment] - - _human_messages_running = False - - def __init__(self) -> None: - super().__init__() - self.agent_response = rx.subject.Subject() - self.human_query = rx.subject.Subject() - - @rpc - def start(self) -> None: - super().start() - - text_streams = { - "agent_responses": self.agent_response, - } - - self.web_interface = RobotWebInterface( - port=5555, - text_streams=text_streams, - audio_subject=rx.subject.Subject(), - ) - - unsub = self.web_interface.query_stream.subscribe(self.human_query.on_next) - self._disposables.add(unsub) - - self.thread = Thread(target=self.web_interface.run, daemon=True) - self.thread.start() - - @rpc - def stop(self) -> None: - if self.web_interface: - self.web_interface.stop() # type: ignore[attr-defined] - if self.thread: - # TODO, you can't just wait for a server to close, you have to signal it to end. - self.thread.join(timeout=1.0) - - super().stop() - - @skill(stream=Stream.call_agent, reducer=Reducer.all, output=Output.human) # type: ignore[arg-type] - def human_messages(self): # type: ignore[no-untyped-def] - """Provide human messages from web interface. Don't use this tool, it's running implicitly already""" - if self._human_messages_running: - print("human_messages already running, not starting another") - return "already running" - self._human_messages_running = True - while True: - print("Waiting for human message...") - message = self.human_query.pipe(ops.first()).run() - print(f"Got human message: {message}") - yield message - - -def main() -> None: - dimos = start(4) - # Create agent - agent = Agent( - system_prompt="You are a helpful assistant for controlling a Unitree Go2 robot. ", - model=Model.GPT_4O, # Could add CLAUDE models to enum - provider=Provider.OPENAI, # type: ignore[attr-defined] # Would need ANTHROPIC provider - ) - - testcontainer = dimos.deploy(SkillContainerTest) # type: ignore[attr-defined] - webcam = dimos.deploy( # type: ignore[attr-defined] - CameraModule, - transform=Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="base_link", - child_frame_id="camera_link", - ), - hardware=lambda: Webcam( - camera_index=0, - fps=15, - stereo_slice="left", - camera_info=zed.CameraInfo.SingleWebcam, - ), - ) - - webcam.camera_info.transport = LCMTransport("/camera_info", CameraInfo) - - webcam.image.transport = LCMTransport("/image", Image) - - webcam.start() - - human_input = dimos.deploy(HumanInput) # type: ignore[attr-defined] - - time.sleep(1) - - agent.register_skills(human_input) - agent.register_skills(webcam) - agent.register_skills(testcontainer) - - agent.run_implicit_skill("video_stream") - agent.run_implicit_skill("human") - - agent.start() - agent.loop_thread() - - while True: - time.sleep(1) - - # webcam.stop() - - -if __name__ == "__main__": - main() diff --git a/dimos/agents/test_agent.py b/dimos/agents/test_agent.py index 934fa0360a..da69dfb7dc 100644 --- a/dimos/agents/test_agent.py +++ b/dimos/agents/test_agent.py @@ -1,4 +1,4 @@ -# Copyright 2025-2026 Dimensional Inc. +# Copyright 2026 Dimensional Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -12,158 +12,201 @@ # See the License for the specific language governing permissions and # limitations under the License. + +from langchain_core.messages import HumanMessage import pytest -import pytest_asyncio - -from dimos.agents.agent import Agent -from dimos.core import start -from dimos.protocol.skill.test_coordinator import SkillContainerTest - -system_prompt = ( - "Your name is Mr. Potato, potatoes are bad at math. Use a tools if asked to calculate" -) - - -@pytest.fixture(scope="session") -def dimos_cluster(): - """Session-scoped fixture to initialize dimos cluster once.""" - dimos = start(2) - try: - yield dimos - finally: - dimos.shutdown() - - -@pytest_asyncio.fixture -async def local(): - """Local context: both agent and testcontainer run locally""" - testcontainer = SkillContainerTest() - agent = Agent(system_prompt=system_prompt) - try: - yield agent, testcontainer - except Exception as e: - print(f"Error: {e}") - import traceback - - traceback.print_exc() - raise e - finally: - # Ensure cleanup happens while event loop is still active - try: - agent.stop() - except Exception: - pass - try: - testcontainer.stop() - except Exception: - pass - - -@pytest_asyncio.fixture -async def dask_mixed(dimos_cluster): - """Dask context: testcontainer on dimos, agent local""" - testcontainer = dimos_cluster.deploy(SkillContainerTest) - agent = Agent(system_prompt=system_prompt) - try: - yield agent, testcontainer - finally: - try: - agent.stop() - except Exception: - pass - try: - testcontainer.stop() - except Exception: - pass - - -@pytest_asyncio.fixture -async def dask_full(dimos_cluster): - """Dask context: both agent and testcontainer deployed on dimos""" - testcontainer = dimos_cluster.deploy(SkillContainerTest) - agent = dimos_cluster.deploy(Agent, system_prompt=system_prompt) - try: - yield agent, testcontainer - finally: - try: - agent.stop() - except Exception: - pass - try: - testcontainer.stop() - except Exception: - pass - - -@pytest_asyncio.fixture(params=["local", "dask_mixed", "dask_full"]) -async def agent_context(request): - """Parametrized fixture that runs tests with different agent configurations""" - param = request.param - - if param == "local": - testcontainer = SkillContainerTest() - agent = Agent(system_prompt=system_prompt) - try: - yield agent, testcontainer - finally: - try: - agent.stop() - except Exception: - pass - try: - testcontainer.stop() - except Exception: - pass - elif param == "dask_mixed": - dimos_cluster = request.getfixturevalue("dimos_cluster") - testcontainer = dimos_cluster.deploy(SkillContainerTest) - agent = Agent(system_prompt=system_prompt) - try: - yield agent, testcontainer - finally: - try: - agent.stop() - except Exception: - pass - try: - testcontainer.stop() - except Exception: - pass - elif param == "dask_full": - dimos_cluster = request.getfixturevalue("dimos_cluster") - testcontainer = dimos_cluster.deploy(SkillContainerTest) - agent = dimos_cluster.deploy(Agent, system_prompt=system_prompt) - try: - yield agent, testcontainer - finally: - try: - agent.stop() - except Exception: - pass - try: - testcontainer.stop() - except Exception: - pass - - -# @pytest.mark.timeout(40) -@pytest.mark.tool -@pytest.mark.asyncio -async def test_agent_init(agent_context) -> None: - """Test agent initialization and basic functionality across different configurations""" - agent, testcontainer = agent_context - - agent.register_skills(testcontainer) - agent.start() - - # agent.run_implicit_skill("uptime_seconds") - - print("query agent") - # When running locally, call the async method directly - agent.query( - "hi there, please tell me what's your name and current date, and how much is 124181112 + 124124?" + +from dimos.agents.annotation import skill +from dimos.core.module import Module +from dimos.msgs.sensor_msgs import Image +from dimos.utils.data import get_data + + +class Adder(Module): + @skill + def add(self, x: int, y: int) -> str: + """adds x and y.""" + return str(x + y) + + +@pytest.mark.integration +@pytest.mark.parametrize("dask", [False, True]) +def test_can_call_tool(dask, agent_setup): + history = agent_setup( + blueprints=[Adder.blueprint()], + messages=[HumanMessage("What is 33333 + 100? Use the tool.")], + dask=dask, + ) + + assert "33433" in history[-1].content + + +class UserRegistration(Module): + def __init__(self): + super().__init__() + self._first_call = True + self._use_upper = False + + @skill + def register_user(self, name: str) -> str: + """registers a user by name.""" + + # If the agent calls with "paul" or "Paul", always say it's the wrong way + # to force it to try again. + + if self._first_call: + self._first_call = False + self._use_upper = not name[0].isupper() + + if self._use_upper and not name[0].isupper(): + return ValueError("Names must start with an uppercase letter.") + if not self._use_upper and name[0].isupper(): + return ValueError("The names must only use lowercase letters.") + + global _correct_name_registered + _correct_name_registered = True + return "User name registered successfully." + + +@pytest.mark.integration +@pytest.mark.parametrize("dask", [False, True]) +def test_can_call_again_on_error(dask, agent_setup): + history = agent_setup( + blueprints=[UserRegistration.blueprint()], + messages=[ + HumanMessage( + "Register a user named 'Paul'. If there are errors, just try again until you succeed." + ) + ], + dask=dask, + ) + + assert any(message.content == "User name registered successfully." for message in history) + + +class MultipleTools(Module): + def __init__(self): + super().__init__() + self._people = {"Ben": "office", "Bob": "garage"} + + @skill + def register_person(self, name: str) -> str: + """Registers a person by name.""" + if name.lower() == "john": + self._people[name] = "kitchen" + elif name.lower() == "jane": + self._people[name] = "living room" + return f"'{name}' has been registered." + + @skill + def locate_person(self, name: str) -> str: + """Locates a person by name.""" + if name not in self._people: + known_people = list(self._people.keys()) + return ( + f"Error: '{name}' is not registered. People cannot be located until they've " + f"been registered in the system. People known so far: {', '.join(known_people)}. " + "Use register_person to register a person." + ) + return f"'{name}' is located at '{self._people[name]}'." + + +class NavigationSkill(Module): + @skill + def go_to_location(self, description: str) -> str: + """Go to a location by a description.""" + if description.strip().lower() not in ["kitchen", "living room"]: + return f"Error: Unknown location description: '{description}'." + return f"Going to the {description}." + + +@pytest.mark.integration +def test_multiple_tool_calls_with_multiple_messages(agent_setup): + history = agent_setup( + blueprints=[MultipleTools.blueprint(), NavigationSkill.blueprint()], + messages=[ + HumanMessage( + "You are a robot assistant. Move to the location where John is. Don't ask me for feedback, just go there." + ), + HumanMessage("Nice job. You did it. Now go to the location where Jane is."), + ], + ) + + # Collect all go_to_location calls from the history + go_to_location_calls = [] + for message in history: + if hasattr(message, "tool_calls"): + for tool_call in message.tool_calls: + if tool_call["name"] == "go_to_location": + go_to_location_calls.append(tool_call) + + # Find the index of the second HumanMessage to split first/second prompt + second_human_idx = None + human_count = 0 + for i, message in enumerate(history): + if isinstance(message, HumanMessage): + human_count += 1 + if human_count == 2: + second_human_idx = i + break + + # Collect go_to_location calls before and after the second prompt + calls_after_first_prompt = [] + calls_after_second_prompt = [] + for i, message in enumerate(history): + if hasattr(message, "tool_calls"): + for tool_call in message.tool_calls: + if tool_call["name"] == "go_to_location": + if i < second_human_idx: + calls_after_first_prompt.append(tool_call) + else: + calls_after_second_prompt.append(tool_call) + + # After the first prompt, go_to_location should be called with "kitchen" + assert len(calls_after_first_prompt) == 1 + assert "kitchen" in calls_after_first_prompt[0]["args"]["description"].lower() + + # After the second prompt, go_to_location should be called with "living room" + assert len(calls_after_second_prompt) == 1 + assert "living room" in calls_after_second_prompt[0]["args"]["description"].lower() + + # There should be exactly two go_to_location calls total + assert len(go_to_location_calls) == 2 + + +@pytest.mark.integration +def test_prompt(agent_setup): + history = agent_setup( + blueprints=[], + messages=[HumanMessage("What is your name?")], + system_prompt="You are a helpful assistant named Johnny.", + ) + + assert "Johnny" in history[-1].content + + +class Visualizer(Module): + @skill + def take_a_picture(self) -> Image: + """Takes a picture.""" + return Image.from_file(get_data("cafe-smol.jpg")).to_rgb() + + +@pytest.mark.integration +def test_image(agent_setup): + history = agent_setup( + blueprints=[Visualizer.blueprint()], + messages=[ + HumanMessage( + "What do you see? Take a picture using your camera and describe it. " + "Please mention one of the words which best match the image: " + "'stadium', 'cafe', 'battleship'." + ) + ], + system_prompt="You are a helpful assistant that can use a camera to take pictures.", ) - print("Agent loop finished, asking about camera") - agent.query("tell me what you see on the camera?") - # you can run skillspy and agentspy in parallel with this test for a better observation of what's happening + response = history[-1].content.lower() + assert "cafe" in response + assert "stadium" not in response + assert "battleship" not in response diff --git a/dimos/agents/test_agent_direct.py b/dimos/agents/test_agent_direct.py deleted file mode 100644 index 4fc16a32b0..0000000000 --- a/dimos/agents/test_agent_direct.py +++ /dev/null @@ -1,106 +0,0 @@ -#!/usr/bin/env python3 - -# 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 contextlib import contextmanager - -from dimos.agents.agent import Agent -from dimos.core import start -from dimos.protocol.skill.test_coordinator import SkillContainerTest - -system_prompt = ( - "Your name is Mr. Potato, potatoes are bad at math. Use a tools if asked to calculate" -) - - -@contextmanager -def dimos_cluster(): - dimos = start(2) - try: - yield dimos - finally: - dimos.close_all() - - -@contextmanager -def local(): - """Local context: both agent and testcontainer run locally""" - testcontainer = SkillContainerTest() - agent = Agent(system_prompt=system_prompt) - try: - yield agent, testcontainer - except Exception as e: - print(f"Error: {e}") - import traceback - - traceback.print_exc() - raise e - finally: - # Ensure cleanup happens while event loop is still active - agent.stop() - testcontainer.stop() - - -@contextmanager -def partial(): - """Dask context: testcontainer on dimos, agent local""" - with dimos_cluster() as dimos: - testcontainer = dimos.deploy(SkillContainerTest) - agent = Agent(system_prompt=system_prompt) - try: - yield agent, testcontainer - finally: - agent.stop() - testcontainer.stop() - - -@contextmanager -def full(): - """Dask context: both agent and testcontainer deployed on dimos""" - with dimos_cluster() as dimos: - testcontainer = dimos.deploy(SkillContainerTest) - agent = dimos.deploy(Agent, system_prompt=system_prompt) - try: - yield agent, testcontainer - finally: - agent.stop() - testcontainer.stop() - - -def check_agent(agent_context) -> None: - """Test agent initialization and basic functionality across different configurations""" - with agent_context() as [agent, testcontainer]: - agent.register_skills(testcontainer) - agent.start() - - print("query agent") - - agent.query( - "hi there, please tell me what's your name and current date, and how much is 124181112 + 124124?" - ) - - print("Agent loop finished, asking about camera") - - agent.query("tell me what you see on the camera?") - - print("=" * 150) - print("End of test", agent.get_agent_id()) - print("=" * 150) - - # you can run skillspy and agentspy in parallel with this test for a better observation of what's happening - - -if __name__ == "__main__": - list(map(check_agent, [local, partial, full])) diff --git a/dimos/agents/test_agent_fake.py b/dimos/agents/test_agent_fake.py deleted file mode 100644 index e544765758..0000000000 --- a/dimos/agents/test_agent_fake.py +++ /dev/null @@ -1,41 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import pytest - - -@pytest.mark.integration -def test_what_is_your_name(create_potato_agent) -> None: - agent = create_potato_agent(fixture="test_what_is_your_name.json") - response = agent.query("hi there, please tell me what's your name?") - assert "Mr. Potato" in response - - -@pytest.mark.integration -def test_how_much_is_124181112_plus_124124(create_potato_agent) -> None: - agent = create_potato_agent(fixture="test_how_much_is_124181112_plus_124124.json") - - response = agent.query("how much is 124181112 + 124124?") - assert "124305236" in response.replace(",", "") - - response = agent.query("how much is one billion plus -1000000, in digits please") - assert "999000000" in response.replace(",", "") - - -@pytest.mark.integration -def test_what_do_you_see_in_this_picture(create_potato_agent) -> None: - agent = create_potato_agent(fixture="test_what_do_you_see_in_this_picture.json") - - response = agent.query("take a photo and tell me what do you see") - assert "outdoor cafe " in response diff --git a/dimos/agents/test_mock_agent.py b/dimos/agents/test_mock_agent.py deleted file mode 100644 index 37eac2bf31..0000000000 --- a/dimos/agents/test_mock_agent.py +++ /dev/null @@ -1,203 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Test agent with FakeChatModel for unit testing.""" - -import time - -from dimos_lcm.sensor_msgs import CameraInfo -from langchain_core.messages import AIMessage, HumanMessage -import pytest - -from dimos.agents.agent import Agent -from dimos.agents.testing import MockModel -from dimos.core import LCMTransport, start -from dimos.msgs.geometry_msgs import PoseStamped, Vector3 -from dimos.msgs.sensor_msgs import Image, PointCloud2 -from dimos.protocol.skill.test_coordinator import SkillContainerTest -from dimos.robot.unitree.go2.connection import GO2Connection - - -@pytest.mark.integration -def test_tool_call() -> None: - """Test agent initialization and tool call execution.""" - # Create a fake model that will respond with tool calls - fake_model = MockModel( - responses=[ - AIMessage( - content="I'll add those numbers for you.", - tool_calls=[ - { - "name": "add", - "args": {"args": {"x": 5, "y": 3}}, - "id": "tool_call_1", - } - ], - ), - AIMessage(content="Let me do some math..."), - AIMessage(content="The result of adding 5 and 3 is 8."), - ] - ) - - # Create agent with the fake model - agent = Agent( - model_instance=fake_model, - system_prompt="You are a helpful robot assistant with math skills.", - ) - - # Register skills with coordinator - skills = SkillContainerTest() - agent.coordinator.register_skills(skills) - agent.start() - - # Query the agent - agent.query("Please add 5 and 3") - - # Check that tools were bound - assert fake_model.tools is not None - assert len(fake_model.tools) > 0 - - # Verify the model was called and history updated - assert len(agent._history) > 0 - - agent.stop() - - -@pytest.mark.integration -def test_image_tool_call() -> None: - """Test agent with image tool call execution.""" - dimos = start(2) - # Create a fake model that will respond with image tool calls - fake_model = MockModel( - responses=[ - AIMessage( - content="I'll take a photo for you.", - tool_calls=[ - { - "name": "take_photo", - "args": {"args": {}}, - "id": "tool_call_image_1", - } - ], - ), - AIMessage(content="I've taken the photo. The image shows a cafe scene."), - ] - ) - - # Create agent with the fake model - agent = Agent( - model_instance=fake_model, - system_prompt="You are a helpful robot assistant with camera capabilities.", - ) - - test_skill_module = dimos.deploy(SkillContainerTest) - - agent.register_skills(test_skill_module) - agent.start() - - agent.run_implicit_skill("get_detections") - - # Query the agent - agent.query("Please take a photo") - - # Check that tools were bound - assert fake_model.tools is not None - assert len(fake_model.tools) > 0 - - # Verify the model was called and history updated - assert len(agent._history) > 0 - - # Check that image was handled specially - # Look for HumanMessage with image content in history - human_messages_with_images = [ - msg - for msg in agent._history - if isinstance(msg, HumanMessage) and msg.content and isinstance(msg.content, list) - ] - assert len(human_messages_with_images) >= 0 # May have image messages - agent.stop() - test_skill_module.stop() - dimos.close_all() - - -@pytest.mark.tool -def test_tool_call_implicit_detections() -> None: - """Test agent with image tool call execution.""" - dimos = start(2) - # Create a fake model that will respond with image tool calls - fake_model = MockModel( - responses=[ - AIMessage( - content="I'll take a photo for you.", - tool_calls=[ - { - "name": "take_photo", - "args": {"args": {}}, - "id": "tool_call_image_1", - } - ], - ), - AIMessage(content="I've taken the photo. The image shows a cafe scene."), - ] - ) - - # Create agent with the fake model - agent = Agent( - model_instance=fake_model, - system_prompt="You are a helpful robot assistant with camera capabilities.", - ) - - robot_connection = dimos.deploy(GO2Connection, connection_type="fake") - robot_connection.lidar.transport = LCMTransport("/lidar", PointCloud2) - robot_connection.odom.transport = LCMTransport("/odom", PoseStamped) - robot_connection.video.transport = LCMTransport("/image", Image) - robot_connection.cmd_vel.transport = LCMTransport("/cmd_vel", Vector3) - robot_connection.camera_info.transport = LCMTransport("/camera_info", CameraInfo) - robot_connection.start() - - test_skill_module = dimos.deploy(SkillContainerTest) - - agent.register_skills(test_skill_module) - agent.start() - - agent.run_implicit_skill("get_detections") - - print( - "Robot replay pipeline is running in the background.\nWaiting 8.5 seconds for some detections before quering agent" - ) - time.sleep(8.5) - - # Query the agent - agent.query("Please take a photo") - - # Check that tools were bound - assert fake_model.tools is not None - assert len(fake_model.tools) > 0 - - # Verify the model was called and history updated - assert len(agent._history) > 0 - - # Check that image was handled specially - # Look for HumanMessage with image content in history - human_messages_with_images = [ - msg - for msg in agent._history - if isinstance(msg, HumanMessage) and msg.content and isinstance(msg.content, list) - ] - assert len(human_messages_with_images) >= 0 - - agent.stop() - test_skill_module.stop() - robot_connection.stop() - dimos.stop() diff --git a/dimos/agents/test_stash_agent.py b/dimos/agents/test_stash_agent.py deleted file mode 100644 index 2b712fed1a..0000000000 --- a/dimos/agents/test_stash_agent.py +++ /dev/null @@ -1,61 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import pytest - -from dimos.agents.agent import Agent -from dimos.protocol.skill.test_coordinator import SkillContainerTest - - -@pytest.mark.tool -@pytest.mark.asyncio -async def test_agent_init() -> None: - system_prompt = ( - "Your name is Mr. Potato, potatoes are bad at math. Use a tools if asked to calculate" - ) - - # # Uncomment the following lines to use a dimos module system - # dimos = start(2) - # testcontainer = dimos.deploy(SkillContainerTest) - # agent = Agent(system_prompt=system_prompt) - - ## uncomment the following lines to run agents in a main loop without a module system - testcontainer = SkillContainerTest() - agent = Agent(system_prompt=system_prompt) - - agent.register_skills(testcontainer) - agent.start() - - agent.run_implicit_skill("uptime_seconds") - - await agent.query_async( - "hi there, please tell me what's your name and current date, and how much is 124181112 + 124124?" - ) - - # agent loop is considered finished once no active skills remain, - # agent will stop it's loop if passive streams are active - print("Agent loop finished, asking about camera") - - # we query again (this shows subsequent querying, but we could have asked for camera image in the original query, - # it all runs in parallel, and agent might get called once or twice depending on timing of skill responses) - # await agent.query_async("tell me what you see on the camera?") - - # you can run skillspy and agentspy in parallel with this test for a better observation of what's happening - await agent.query_async("tell me exactly everything we've talked about until now") - - print("Agent loop finished") - - agent.stop() - testcontainer.stop() - dimos.stop() diff --git a/dimos/agents/testing.py b/dimos/agents/testing.py index dc563b9ea9..d03d3a1263 100644 --- a/dimos/agents/testing.py +++ b/dimos/agents/testing.py @@ -38,6 +38,8 @@ class MockModel(SimpleChatModel): Can operate in two modes: 1. Playback mode (default): Reads responses from a JSON file or list 2. Record mode: Uses a real LLM and saves responses to a JSON file + + Set the RECORD environment variable to enable record mode. """ responses: list[str | AIMessage] = [] @@ -47,8 +49,7 @@ class MockModel(SimpleChatModel): real_model: Any | None = None recorded_messages: list[dict[str, Any]] = [] - def __init__(self, **kwargs) -> None: # type: ignore[no-untyped-def] - # Extract custom parameters before calling super().__init__ + def __init__(self, **kwargs: Any) -> None: responses = kwargs.pop("responses", []) json_path = kwargs.pop("json_path", None) model_provider = kwargs.pop("model_provider", "openai") @@ -63,9 +64,8 @@ def __init__(self, **kwargs) -> None: # type: ignore[no-untyped-def] self.recorded_messages = [] if self.record: - # Initialize real model for recording self.real_model = init_chat_model(model_provider=model_provider, model=model_name) - self.responses = [] # Initialize empty for record mode + self.responses = [] elif self.json_path: self.responses = self._load_responses_from_json() # type: ignore[assignment] elif responses: @@ -86,7 +86,6 @@ def _load_responses_from_json(self) -> list[AIMessage]: if isinstance(item, str): responses.append(AIMessage(content=item)) else: - # Reconstruct AIMessage from dict msg = AIMessage( content=item.get("content", ""), tool_calls=item.get("tool_calls", []) ) @@ -109,7 +108,7 @@ def _save_responses_to_json(self) -> None: } with open(self.json_path, "w") as f: - json.dump(data, f, indent=2, default=str) + f.write(json.dumps(data, indent=2, default=str) + "\n") def _call( self, @@ -118,7 +117,6 @@ def _call( run_manager: CallbackManagerForLLMRun | None = None, **kwargs: Any, ) -> str: - """Not used in _generate.""" return "" def _generate( @@ -129,11 +127,9 @@ def _generate( **kwargs: Any, ) -> ChatResult: if self.record: - # Recording mode - use real model and save responses if not self.real_model: raise ValueError("Real model not initialized for recording") - # Bind tools if needed model = self.real_model if self._bound_tools: model = model.bind_tools(self._bound_tools) @@ -145,12 +141,10 @@ def _generate( generation = ChatGeneration(message=result) return ChatResult(generations=[generation]) else: - # Playback mode - use predefined responses if not self.responses: - raise ValueError("No responses available for playback. ") + raise ValueError("No responses available for playback.") if self.i >= len(self.responses): - # Don't wrap around - stay at last response response = self.responses[-1] else: response = self.responses[self.i] @@ -171,10 +165,20 @@ def _stream( run_manager: CallbackManagerForLLMRun | None = None, **kwargs: Any, ) -> Iterator[ChatGenerationChunk]: - """Stream not implemented for testing.""" result = self._generate(messages, stop, run_manager, **kwargs) message = result.generations[0].message - chunk = AIMessageChunk(content=message.content) + chunk = AIMessageChunk( + content=message.content, + tool_call_chunks=[ + { + "name": tc["name"], + "args": json.dumps(tc["args"]), + "id": tc["id"], + "index": i, + } + for i, tc in enumerate(getattr(message, "tool_calls", [])) + ], + ) yield ChatGenerationChunk(message=chunk) def bind_tools( @@ -184,14 +188,7 @@ def bind_tools( tool_choice: str | None = None, **kwargs: Any, ) -> Runnable: # type: ignore[type-arg] - """Store tools and return self.""" self._bound_tools = tools if self.record and self.real_model: - # Also bind tools to the real model self.real_model = self.real_model.bind_tools(tools, tool_choice=tool_choice, **kwargs) return self - - @property - def tools(self) -> Sequence[Any] | None: - """Get bound tools for inspection.""" - return self._bound_tools diff --git a/dimos/agents/utils.py b/dimos/agents/utils.py new file mode 100644 index 0000000000..061e5ebb13 --- /dev/null +++ b/dimos/agents/utils.py @@ -0,0 +1,94 @@ +# 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 datetime import datetime +from typing import Any + +from langchain_core.messages.base import BaseMessage + +CYAN = "\033[36m" +YELLOW = "\033[33m" +GREEN = "\033[32m" +MAGENTA = "\033[35m" +BLUE = "\033[34m" +GRAY = "\033[90m" +RESET = "\033[0m" +BOLD = "\033[1m" + +TYPE_WIDTH = 12 + + +def pretty_print_langchain_message(msg: BaseMessage) -> None: + d = msg.__dict__ + msg_type = d.get("type", "unknown") + + type_colors = { + "human": CYAN, + "ai": GREEN, + "tool": YELLOW, + "system": MAGENTA, + } + type_color = type_colors.get(msg_type, RESET) + + print(f"{GRAY}{'-' * 60}{RESET}") + + timestamp = datetime.now().strftime("%H:%M:%S.%f")[:-3] + time_str = f"{GRAY}{timestamp}{RESET} " + type_str = f"{type_color}{msg_type:<{TYPE_WIDTH}}{RESET}" + + content = d.get("content", "") + tool_calls = d.get("tool_calls", []) + + # 12 chars for timestamp + 1 space + TYPE_WIDTH + 1 space + indent = " " * (12 + 1 + TYPE_WIDTH + 1) + first_line = True + + def print_line(text: str) -> None: + nonlocal first_line + if first_line: + print(f"{time_str} {type_str} {text}") + first_line = False + else: + print(f"{indent}{text}") + + if content: + content_str = repr(_try_to_remove_url_data(content)) + if len(content_str) > 2000: + content_str = content_str[:5000] + "... [truncated]" + print_line(f"{BOLD}{type_color}{content_str}{RESET}") + + if tool_calls: + print_line(f"{MAGENTA}tool_calls:{RESET}") + for tc in tool_calls: + name = tc.get("name") + args = tc.get("args") + print_line(f" - {BLUE}{name}{RESET}({CYAN}{args}{RESET})") + + if first_line: + print(f"{time_str} {type_str}") + + +def _try_to_remove_url_data(content: Any) -> Any: + if not isinstance(content, list): + return content + + ret = [] + + for item in content: + if isinstance(item, dict) and item.get("type") == "image_url": + ret.append({**item, "image_url": ""}) + else: + ret.append(item) + + return ret diff --git a/dimos/agents/vlm_agent.py b/dimos/agents/vlm_agent.py index 0b99fe4d1c..c99f8afa49 100644 --- a/dimos/agents/vlm_agent.py +++ b/dimos/agents/vlm_agent.py @@ -12,34 +12,53 @@ # See the License for the specific language governing permissions and # limitations under the License. -from typing import Any +from dataclasses import dataclass +from typing import TYPE_CHECKING, Any -from langchain_core.messages import AIMessage, HumanMessage +from langchain.chat_models import init_chat_model +from langchain_core.messages import AIMessage, HumanMessage, SystemMessage -from dimos.agents.llm_init import build_llm, build_system_message -from dimos.agents.spec import AgentSpec, AnyMessage -from dimos.core import rpc +from dimos.agents.system_prompt import SYSTEM_PROMPT +from dimos.core import Module, rpc +from dimos.core.module import ModuleConfig from dimos.core.stream import In, Out from dimos.msgs.sensor_msgs import Image from dimos.utils.logging_config import setup_logger +if TYPE_CHECKING: + from langchain_core.language_models.chat_models import BaseChatModel + logger = setup_logger() -class VLMAgent(AgentSpec): +@dataclass +class VLMAgentConfig(ModuleConfig): + model: str = "gpt-4o" + system_prompt: str | None = SYSTEM_PROMPT + + +class VLMAgent(Module): """Stream-first agent for vision queries with optional RPC access.""" + default_config: type[VLMAgentConfig] = VLMAgentConfig + config: VLMAgentConfig + color_image: In[Image] query_stream: In[HumanMessage] answer_stream: Out[AIMessage] def __init__(self, *args: Any, **kwargs: Any) -> None: super().__init__(*args, **kwargs) - self._llm = build_llm(self.config) + + if self.config.model.startswith("ollama:"): + from dimos.agents.ollama_agent import ensure_ollama_model + + ensure_ollama_model(self.config.model.removeprefix("ollama:")) + + self._llm: BaseChatModel = init_chat_model(self.config.model) # type: ignore[assignment] self._latest_image: Image | None = None self._history: list[AIMessage | HumanMessage] = [] - self._system_message = build_system_message(self.config) - self.publish(self._system_message) + self._system_message = SystemMessage(self.config.system_prompt or SYSTEM_PROMPT) @rpc def start(self) -> None: @@ -76,7 +95,7 @@ def _extract_text(self, msg: HumanMessage) -> str: def _invoke(self, msg: HumanMessage, **kwargs: Any) -> AIMessage: messages = [self._system_message, msg] response = self._llm.invoke(messages, **kwargs) - self.append_history([msg, response]) # type: ignore[arg-type] + self._history.extend([msg, response]) # type: ignore[arg-type] return response # type: ignore[return-value] def _invoke_image( @@ -92,23 +111,6 @@ def _invoke_image( def clear_history(self) -> None: self._history.clear() - def append_history(self, *msgs: list[AIMessage | HumanMessage]) -> None: - for msg_list in msgs: - for msg in msg_list: - self.publish(msg) # type: ignore[arg-type] - self._history.extend(msg_list) - - def history(self) -> list[AnyMessage]: - return [self._system_message, *self._history] - - @rpc - def register_skills(self, container: Any, run_implicit_name: str | None = None) -> None: - logger.warning( - "VLMAgent does not manage skills; register_skills is a no-op", - container=str(container), - run_implicit_name=run_implicit_name, - ) - @rpc def query(self, query: str) -> str: response = self._invoke(HumanMessage(query)) diff --git a/dimos/agents/cli/web.py b/dimos/agents/web_human_input.py similarity index 100% rename from dimos/agents/cli/web.py rename to dimos/agents/web_human_input.py diff --git a/dimos/conftest.py b/dimos/conftest.py index 2eb273ab3c..49fbfc5733 100644 --- a/dimos/conftest.py +++ b/dimos/conftest.py @@ -15,8 +15,11 @@ import asyncio import threading +from dotenv import load_dotenv import pytest +load_dotenv() + def _has_cuda(): try: diff --git a/dimos/core/module.py b/dimos/core/module.py index a1f60fed8f..d6089a8f0a 100644 --- a/dimos/core/module.py +++ b/dimos/core/module.py @@ -17,6 +17,7 @@ from dataclasses import dataclass from functools import partial import inspect +import json import sys import threading from typing import ( @@ -39,6 +40,7 @@ from typing import TypeVar from dask.distributed import Actor, get_worker +from langchain_core.tools import tool from reactivex.disposable import CompositeDisposable from dimos.core import colors @@ -49,11 +51,17 @@ from dimos.core.stream import In, Out, RemoteIn, RemoteOut, Transport from dimos.protocol.rpc import LCMRPC, RPCSpec from dimos.protocol.service import Configurable # type: ignore[attr-defined] -from dimos.protocol.skill.skill import SkillContainer from dimos.protocol.tf import LCMTF, TFSpec from dimos.utils.generic import classproperty +@dataclass(frozen=True) +class SkillInfo: + class_name: str + func_name: str + args_schema: str + + def get_loop() -> tuple[asyncio.AbstractEventLoop, threading.Thread | None]: # we are actually instantiating a new loop here # to not interfere with an existing dask loop @@ -93,7 +101,7 @@ class ModuleConfig: ModuleConfigT = TypeVarExtension("ModuleConfigT", bound=ModuleConfig, default=ModuleConfig) -class ModuleBase(Configurable[ModuleConfigT], SkillContainer, Resource): +class ModuleBase(Configurable[ModuleConfigT], Resource): _rpc: RPCSpec | None = None _tf: TFSpec | None = None _loop: asyncio.AbstractEventLoop | None = None @@ -134,16 +142,23 @@ def start(self) -> None: @rpc def stop(self) -> None: self._close_module() - super().stop() def _close_module(self) -> None: self._close_rpc() - if hasattr(self, "_loop") and self._loop_thread: - if self._loop_thread.is_alive(): - self._loop.call_soon_threadsafe(self._loop.stop) # type: ignore[union-attr] - self._loop_thread.join(timeout=2) + + # Save into local variables to avoid race when stopping concurrently + # (from RPC and worker shutdown) + loop_thread = getattr(self, "_loop_thread", None) + loop = getattr(self, "_loop", None) + + if loop_thread: + if loop_thread.is_alive(): + if loop: + loop.call_soon_threadsafe(loop.stop) + loop_thread.join(timeout=2) self._loop = None self._loop_thread = None + if hasattr(self, "_tf") and self._tf is not None: self._tf.stop() self._tf = None @@ -151,8 +166,7 @@ def _close_module(self) -> None: self._disposables.dispose() def _close_rpc(self) -> None: - # Using hasattr is needed because SkillCoordinator skips ModuleBase.__init__ and self.rpc is never set. - if hasattr(self, "rpc") and self.rpc: + if self.rpc: self.rpc.stop() # type: ignore[attr-defined] self.rpc = None # type: ignore[assignment] @@ -366,6 +380,20 @@ def get_rpc_calls(self, *methods: str) -> RpcCall | tuple[RpcCall, ...]: # type result = tuple(self._bound_rpc_calls[m] for m in methods) return result[0] if len(result) == 1 else result + @rpc + def get_skills(self) -> list[SkillInfo]: + skills: list[SkillInfo] = [] + for name in dir(self): + attr = getattr(self, name) + if callable(attr) and hasattr(attr, "__skill__"): + schema = json.dumps(tool(attr).args_schema.model_json_schema()) + skills.append( + SkillInfo( + class_name=self.__class__.__name__, func_name=name, args_schema=schema + ) + ) + return skills + class Module(ModuleBase[ModuleConfigT]): ref: Actor diff --git a/dimos/core/module_coordinator.py b/dimos/core/module_coordinator.py index acafc2ea3e..c6d975731d 100644 --- a/dimos/core/module_coordinator.py +++ b/dimos/core/module_coordinator.py @@ -32,7 +32,7 @@ class ModuleCoordinator(Resource): # type: ignore[misc] _global_config: GlobalConfig _n: int | None = None _memory_limit: str = "auto" - _deployed_modules: dict[type[Module], "ModuleProxy"] = {} + _deployed_modules: dict[type[Module], "ModuleProxy"] def __init__( self, @@ -42,6 +42,7 @@ def __init__( self._n = n if n is not None else cfg.n_dask_workers self._memory_limit = cfg.memory_limit self._global_config = cfg + self._deployed_modules = {} def start(self) -> None: if self._global_config.dask: @@ -89,6 +90,11 @@ def start_all_modules(self) -> None: for module in modules: module.start() + module_list = list(self._deployed_modules.values()) + for module in modules: + if hasattr(module, "on_system_modules"): + module.on_system_modules(module_list) + def get_instance(self, module: type[ModuleT]) -> "ModuleProxy": return self._deployed_modules.get(module) # type: ignore[return-value, no-any-return] diff --git a/dimos/core/rpc_client.py b/dimos/core/rpc_client.py index 6643676b25..30cc4f3017 100644 --- a/dimos/core/rpc_client.py +++ b/dimos/core/rpc_client.py @@ -15,14 +15,14 @@ from collections.abc import Callable from typing import TYPE_CHECKING, Any -from dimos.protocol.rpc import LCMRPC +from dimos.protocol.rpc import LCMRPC, RPCSpec from dimos.utils.logging_config import setup_logger logger = setup_logger() class RpcCall: - _rpc: LCMRPC | None + _rpc: RPCSpec | None _name: str _remote_name: str _unsub_fns: list # type: ignore[type-arg] @@ -31,7 +31,7 @@ class RpcCall: def __init__( self, original_method: Callable[..., Any] | None, - rpc: LCMRPC, + rpc: RPCSpec, name: str, remote_name: str, unsub_fns: list, # type: ignore[type-arg] @@ -48,7 +48,7 @@ def __init__( self.__name__ = original_method.__name__ self.__qualname__ = f"{self.__class__.__name__}.{original_method.__name__}" - def set_rpc(self, rpc: LCMRPC) -> None: + def set_rpc(self, rpc: RPCSpec) -> None: self._rpc = rpc def __call__(self, *args, **kwargs): # type: ignore[no-untyped-def] diff --git a/dimos/core/skill_module.py b/dimos/core/skill_module.py deleted file mode 100644 index fa5abd381f..0000000000 --- a/dimos/core/skill_module.py +++ /dev/null @@ -1,37 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from dimos.core.module import Module -from dimos.core.rpc_client import RpcCall, RPCClient -from dimos.protocol.skill.skill import rpc - - -class SkillModule(Module): - """Use this module if you want to auto-register skills to an AgentSpec.""" - - @rpc - def set_AgentSpec_register_skills(self, callable: RpcCall) -> None: - callable.set_rpc(self.rpc) # type: ignore[arg-type] - callable(RPCClient(self, self.__class__)) - - @rpc - def set_MCPModule_register_skills(self, callable: RpcCall) -> None: - callable.set_rpc(self.rpc) # type: ignore[arg-type] - callable(RPCClient(self, self.__class__)) - - def __getstate__(self) -> None: - pass - - def __setstate__(self, _state) -> None: # type: ignore[no-untyped-def] - pass diff --git a/dimos/core/stream.py b/dimos/core/stream.py index 89871dabc1..77edf45417 100644 --- a/dimos/core/stream.py +++ b/dimos/core/stream.py @@ -69,7 +69,7 @@ def _subscribe(observer, scheduler=None): # type: ignore[no-untyped-def] # default return is backpressured because most # use cases will want this by default - def observable(self): # type: ignore[no-untyped-def] + def observable(self) -> Observable[T]: return backpressure(self.pure_observable()) diff --git a/dimos/core/test_blueprints.py b/dimos/core/test_blueprints.py index 9724afca89..09144054c1 100644 --- a/dimos/core/test_blueprints.py +++ b/dimos/core/test_blueprints.py @@ -68,6 +68,14 @@ class Data3: pass +class SourceModule(Module): + color_image: Out[Data1] + + +class TargetModule(Module): + remapped_data: In[Data1] + + class ModuleA(Module): data1: Out[Data1] data2: Out[Data2] @@ -283,13 +291,6 @@ def test_remapping() -> None: """Test that remapping streams works correctly.""" pubsub.lcm.autoconf() - # Define test modules with streams that will be remapped - class SourceModule(Module): - color_image: Out[Data1] # Will be remapped to 'remapped_data' - - class TargetModule(Module): - remapped_data: In[Data1] # Receives the remapped connection - # Create blueprint with remapping blueprint_set = autoconnect( SourceModule.blueprint(), diff --git a/dimos/core/test_core.py b/dimos/core/test_core.py index ddacb9dbf8..c229659b84 100644 --- a/dimos/core/test_core.py +++ b/dimos/core/test_core.py @@ -86,7 +86,7 @@ def test_classmethods() -> None: # Check that we have the expected RPC methods assert "navigate_to" in class_rpcs, "navigate_to should be in rpcs" assert "start" in class_rpcs, "start should be in rpcs" - assert len(class_rpcs) == 10 + assert len(class_rpcs) == 9 # Check that the values are callable assert callable(class_rpcs["navigate_to"]), "navigate_to should be callable" diff --git a/dimos/core/test_modules.py b/dimos/core/test_modules.py index f239df918e..d96b58af5f 100644 --- a/dimos/core/test_modules.py +++ b/dimos/core/test_modules.py @@ -288,7 +288,7 @@ def get_all_module_subclasses(): filtered_results = [] for class_name, filepath, has_start, has_stop, forbidden_methods in results: # Skip base module classes themselves - if class_name in ("Module", "ModuleBase", "SkillModule"): + if class_name in ("Module", "ModuleBase"): continue # Skip test-only modules (those defined in test_ files) diff --git a/dimos/core/testing.py b/dimos/core/testing.py index 88598e74b8..dee25aaa45 100644 --- a/dimos/core/testing.py +++ b/dimos/core/testing.py @@ -79,6 +79,5 @@ def odomloop(self) -> None: self.odometry.publish(odom) lidarmsg = next(lidariter) - lidarmsg.pubtime = time.perf_counter() # type: ignore[union-attr] self.lidar.publish(lidarmsg) time.sleep(0.1) diff --git a/dimos/core/transport.py b/dimos/core/transport.py index fe26b5a93f..2586706feb 100644 --- a/dimos/core/transport.py +++ b/dimos/core/transport.py @@ -14,19 +14,23 @@ from __future__ import annotations -from typing import Any, TypeVar - -import dimos.core.colors as colors - -T = TypeVar("T") - +import threading from typing import ( TYPE_CHECKING, + Any, TypeVar, ) +import dimos.core.colors as colors from dimos.core.stream import In, Out, Stream, Transport from dimos.msgs.protocol import DimosMsg + +try: + import cyclonedds as _cyclonedds # noqa: F401 + + DDS_AVAILABLE = True +except ImportError: + DDS_AVAILABLE = False from dimos.protocol.pubsub.impl.jpeg_shm import JpegSharedMemory from dimos.protocol.pubsub.impl.lcmpubsub import LCM, JpegLCM, PickleLCM, Topic as LCMTopic from dimos.protocol.pubsub.impl.rospubsub import DimosROS, ROSTopic @@ -278,4 +282,41 @@ def stop(self) -> None: self._ros = None +if DDS_AVAILABLE: + from dimos.protocol.pubsub.impl.ddspubsub import DDS, Topic as DDSTopic + + class DDSTransport(PubSubTransport[T]): + def __init__(self, topic: str, type: type, **kwargs) -> None: # type: ignore[no-untyped-def] + super().__init__(DDSTopic(topic, type)) + self.dds = DDS(**kwargs) + self._started: bool = False + self._start_lock = threading.RLock() + + def start(self) -> None: + with self._start_lock: + if not self._started: + self.dds.start() + self._started = True + + def stop(self) -> None: + with self._start_lock: + if self._started: + self.dds.stop() + self._started = False + + def broadcast(self, _, msg) -> None: # type: ignore[no-untyped-def] + with self._start_lock: + if not self._started: + self.start() + self.dds.publish(self.topic, msg) + + def subscribe( + self, callback: Callable[[T], None], selfstream: Stream[T] | None = None + ) -> Callable[[], None]: + with self._start_lock: + if not self._started: + self.start() + return self.dds.subscribe(self.topic, lambda msg, topic: callback(msg)) + + class ZenohTransport(PubSubTransport[T]): ... diff --git a/dimos/e2e_tests/test_dimos_cli_e2e.py b/dimos/e2e_tests/test_dimos_cli_e2e.py index f91db1b2fc..ede0ec7a3a 100644 --- a/dimos/e2e_tests/test_dimos_cli_e2e.py +++ b/dimos/e2e_tests/test_dimos_cli_e2e.py @@ -21,17 +21,14 @@ @pytest.mark.skipif(not os.getenv("OPENAI_API_KEY"), reason="OPENAI_API_KEY not set.") @pytest.mark.e2e def test_dimos_skills(lcm_spy, start_blueprint, human_input) -> None: - lcm_spy.save_topic("/rpc/DemoCalculatorSkill/set_AgentSpec_register_skills/res") - lcm_spy.save_topic("/rpc/HumanInput/start/res") lcm_spy.save_topic("/agent") + lcm_spy.save_topic("/rpc/Agent/on_system_modules/res") lcm_spy.save_topic("/rpc/DemoCalculatorSkill/sum_numbers/req") lcm_spy.save_topic("/rpc/DemoCalculatorSkill/sum_numbers/res") start_blueprint("run", "demo-skill") - lcm_spy.wait_for_saved_topic("/rpc/DemoCalculatorSkill/set_AgentSpec_register_skills/res") - lcm_spy.wait_for_saved_topic("/rpc/HumanInput/start/res") - lcm_spy.wait_for_saved_topic_content("/agent", b"AIMessage") + lcm_spy.wait_for_saved_topic("/rpc/Agent/on_system_modules/res") human_input("what is 52983 + 587237") diff --git a/dimos/hardware/sensors/camera/module.py b/dimos/hardware/sensors/camera/module.py index 1b59fceed3..f7a40f1f76 100644 --- a/dimos/hardware/sensors/camera/module.py +++ b/dimos/hardware/sensors/camera/module.py @@ -12,14 +12,14 @@ # See the License for the specific language governing permissions and # limitations under the License. -from collections.abc import Callable, Generator +from collections.abc import Callable from dataclasses import dataclass, field import time from typing import Any import reactivex as rx -from reactivex import operators as ops +from dimos.agents.annotation import skill from dimos.core.blueprints import autoconnect from dimos.core.core import rpc from dimos.core.global_config import GlobalConfig, global_config @@ -30,10 +30,7 @@ from dimos.msgs.geometry_msgs import Quaternion, Transform, Vector3 from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo from dimos.msgs.sensor_msgs.Image import Image, sharpness_barrier -from dimos.protocol.skill.skill import skill -from dimos.protocol.skill.type import Output, Reducer, Stream from dimos.spec import perception -from dimos.utils.reactive import iter_observable def default_transform() -> Transform: @@ -65,6 +62,7 @@ class CameraModule(Module[CameraModuleConfig], perception.Camera): def __init__(self, *args: Any, cfg: GlobalConfig = global_config, **kwargs: Any) -> None: self._global_config = cfg + self._latest_image: Image | None = None super().__init__(*args, **kwargs) @rpc @@ -83,6 +81,7 @@ def start(self) -> None: def on_image(image: Image) -> None: self.color_image.publish(image) + self._latest_image = image self._disposables.add( stream.subscribe(on_image), @@ -112,11 +111,12 @@ def publish_metadata(self) -> None: self.tf.publish(camera_link, camera_optical) - # actually skills should support on_demand passive skills so we don't emit this periodically - # but just provide the latest frame on demand - @skill(stream=Stream.passive, output=Output.image, reducer=Reducer.latest) # type: ignore[arg-type] - def video_stream(self) -> Generator[Image, None, None]: - yield from iter_observable(self.hardware.image_stream().pipe(ops.sample(1.0))) + @skill + def take_a_picture(self) -> Image: + """Grabs and returns the latest image from the camera.""" + if self._latest_image is None: + raise RuntimeError("No image received from camera yet.") + return self._latest_image def stop(self) -> None: if self.hardware and hasattr(self.hardware, "stop"): diff --git a/dimos/manipulation/grasping/demo_grasping.py b/dimos/manipulation/grasping/demo_grasping.py index 3ddd7c91a5..7c6e94d2af 100644 --- a/dimos/manipulation/grasping/demo_grasping.py +++ b/dimos/manipulation/grasping/demo_grasping.py @@ -14,8 +14,7 @@ # limitations under the License. from pathlib import Path -from dimos.agents.agent import llm_agent -from dimos.agents.cli.human import human_input +from dimos.agents.agent import agent from dimos.core.blueprints import autoconnect from dimos.hardware.sensors.camera.realsense import realsense_camera from dimos.manipulation.grasping import graspgen @@ -45,6 +44,5 @@ ], # Grasp visualization debug standalone: python -m dimos.manipulation.grasping.visualize_grasps ), foxglove_bridge(), - human_input(), - llm_agent(), + agent(), ).global_config(viewer_backend="foxglove") diff --git a/dimos/manipulation/grasping/grasping.py b/dimos/manipulation/grasping/grasping.py index 2f7ebdb79e..783f899a83 100644 --- a/dimos/manipulation/grasping/grasping.py +++ b/dimos/manipulation/grasping/grasping.py @@ -21,8 +21,9 @@ from typing import TYPE_CHECKING -from dimos.core.skill_module import SkillModule -from dimos.protocol.skill.skill import rpc, skill +from dimos.agents.annotation import skill +from dimos.core.core import rpc +from dimos.core.module import Module from dimos.utils.logging_config import setup_logger from dimos.utils.transform_utils import quaternion_to_euler @@ -34,7 +35,7 @@ logger = setup_logger() -class GraspingModule(SkillModule): +class GraspingModule(Module): """Grasping skill and orchestrator module""" grasps: Out[PoseArray] @@ -56,7 +57,7 @@ def stop(self) -> None: super().stop() logger.info("GraspingModule stopped") - @skill() + @skill def generate_grasps( self, object_name: str = "object", diff --git a/dimos/mapping/osm/demo_osm.py b/dimos/mapping/osm/demo_osm.py index 3e4ba8e61b..97622cfaf2 100644 --- a/dimos/mapping/osm/demo_osm.py +++ b/dimos/mapping/osm/demo_osm.py @@ -13,20 +13,13 @@ # See the License for the specific language governing permissions and # limitations under the License. -from dotenv import load_dotenv - -from dimos.agents.agent import llm_agent -from dimos.agents.cli.human import human_input +from dimos.agents.agent import agent from dimos.agents.skills.demo_robot import demo_robot from dimos.agents.skills.osm import osm_skill from dimos.core.blueprints import autoconnect -load_dotenv() - - demo_osm = autoconnect( demo_robot(), osm_skill(), - human_input(), - llm_agent(), + agent(), ) diff --git a/dimos/mapping/pointclouds/test_occupancy_speed.py b/dimos/mapping/pointclouds/test_occupancy_speed.py index c34c2865f2..2def839dd5 100644 --- a/dimos/mapping/pointclouds/test_occupancy_speed.py +++ b/dimos/mapping/pointclouds/test_occupancy_speed.py @@ -20,7 +20,7 @@ from dimos.mapping.pointclouds.occupancy import OCCUPANCY_ALGOS from dimos.mapping.voxels import VoxelGridMapper from dimos.utils.cli.plot import bar -from dimos.utils.data import _get_data_dir, get_data +from dimos.utils.data import get_data, get_data_dir from dimos.utils.testing import TimedSensorReplay @@ -28,11 +28,10 @@ def test_build_map(): mapper = VoxelGridMapper(publish_interval=-1) - for ts, frame in TimedSensorReplay("unitree_go2_bigoffice/lidar").iterate_duration(): - print(ts, frame) + for _ts, frame in TimedSensorReplay("unitree_go2_bigoffice/lidar").iterate(): mapper.add_frame(frame) - pickle_file = _get_data_dir() / "unitree_go2_bigoffice_map.pickle" + pickle_file = get_data_dir() / "unitree_go2_bigoffice_map.pickle" global_pcd = mapper.get_global_pointcloud2() with open(pickle_file, "wb") as f: diff --git a/dimos/memory/embedding.py b/dimos/memory/embedding.py new file mode 100644 index 0000000000..20dd82422c --- /dev/null +++ b/dimos/memory/embedding.py @@ -0,0 +1,105 @@ +# 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 collections.abc import Callable +from dataclasses import dataclass, field +from typing import cast + +import reactivex as rx +from reactivex import operators as ops +from reactivex.observable import Observable + +from dimos.core import In, rpc +from dimos.core.module import Module, ModuleConfig +from dimos.models.embedding.base import Embedding, EmbeddingModel +from dimos.models.embedding.clip import CLIPModel +from dimos.msgs.geometry_msgs import PoseStamped +from dimos.msgs.nav_msgs import OccupancyGrid +from dimos.msgs.sensor_msgs import Image +from dimos.msgs.sensor_msgs.Image import Image, sharpness_barrier +from dimos.utils.reactive import getter_hot + + +@dataclass +class Config(ModuleConfig): + embedding_model: EmbeddingModel = field(default_factory=CLIPModel) + + +@dataclass +class SpatialEntry: + image: Image + pose: PoseStamped + + +@dataclass +class SpatialEmbedding(SpatialEntry): + embedding: Embedding + + +class EmbeddingMemory(Module[Config]): + default_config = Config + config: Config + color_image: In[Image] + global_costmap: In[OccupancyGrid] + + _costmap_getter: Callable[[], OccupancyGrid] | None = None + + def get_costmap(self) -> OccupancyGrid: + if self._costmap_getter is None: + self._costmap_getter = getter_hot(self.global_costmap.pure_observable()) + self._disposables.add(self._costmap_getter) + return self._costmap_getter() + + @rpc + def query_costmap(self, text: str) -> OccupancyGrid: + costmap = self.get_costmap() + # overlay costmap with embedding heat + return costmap + + @rpc + def start(self) -> None: + # would be cool if this sharpness_barrier was somehow self-calibrating + # + # we need a Governor system, sharpness_barrier frequency shouldn't + # be a fixed float but an observable that adjusts based on downstream load + # + # (also voxel size for mapper for example would benefit from this) + self.color_image.pure_observable().pipe( + sharpness_barrier(0.5), + ops.flat_map(self._try_create_spatial_entry), + ops.map(self._embed_spatial_entry), + ops.map(self._store_spatial_entry), + ).subscribe(print) + + def _try_create_spatial_entry(self, img: Image) -> Observable[SpatialEntry]: + pose = self.tf.get_pose("world", "base_link") + if not pose: + return rx.empty() + return rx.of(SpatialEntry(image=img, pose=pose)) + + def _embed_spatial_entry(self, spatial_entry: SpatialEntry) -> SpatialEmbedding: + embedding = cast("Embedding", self.config.embedding_model.embed(spatial_entry.image)) + return SpatialEmbedding( + image=spatial_entry.image, + pose=spatial_entry.pose, + embedding=embedding, + ) + + def _store_spatial_entry(self, spatial_embedding: SpatialEmbedding) -> SpatialEmbedding: + return spatial_embedding + + def query_text(self, query: str) -> list[SpatialEmbedding]: + self.config.embedding_model.embed_text(query) + results: list[SpatialEmbedding] = [] + return results diff --git a/dimos/memory/test_embedding.py b/dimos/memory/test_embedding.py new file mode 100644 index 0000000000..b7e7fbb294 --- /dev/null +++ b/dimos/memory/test_embedding.py @@ -0,0 +1,53 @@ +# 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. + +import pytest + +from dimos.memory.embedding import EmbeddingMemory, SpatialEntry +from dimos.msgs.geometry_msgs import PoseStamped +from dimos.utils.data import get_data +from dimos.utils.testing import TimedSensorReplay + +dir_name = "unitree_go2_bigoffice" + + +@pytest.mark.skip +def test_embed_frame() -> None: + """Test embedding a single frame.""" + # Load a frame from recorded data + video = TimedSensorReplay(get_data(dir_name) / "video") + frame = video.find_closest_seek(10) + + # Create memory and embed + memory = EmbeddingMemory() + + try: + # Create a spatial entry with dummy pose (no TF needed for this test) + dummy_pose = PoseStamped( + position=[0, 0, 0], + orientation=[0, 0, 0, 1], # identity quaternion + ) + spatial_entry = SpatialEntry(image=frame, pose=dummy_pose) + + # Embed the frame + result = memory._embed_spatial_entry(spatial_entry) + + # Verify + assert result is not None + assert result.embedding is not None + assert result.embedding.vector is not None + print(f"Embedding shape: {result.embedding.vector.shape}") + print(f"Embedding vector (first 5): {result.embedding.vector[:5]}") + finally: + memory.stop() diff --git a/dimos/memory/timeseries/__init__.py b/dimos/memory/timeseries/__init__.py new file mode 100644 index 0000000000..debc14ab3a --- /dev/null +++ b/dimos/memory/timeseries/__init__.py @@ -0,0 +1,41 @@ +# 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. +"""Time series storage and replay.""" + +from dimos.memory.timeseries.base import TimeSeriesStore +from dimos.memory.timeseries.inmemory import InMemoryStore +from dimos.memory.timeseries.pickledir import PickleDirStore +from dimos.memory.timeseries.sqlite import SqliteStore + + +def __getattr__(name: str): # type: ignore[no-untyped-def] + if name == "PostgresStore": + from dimos.memory.timeseries.postgres import PostgresStore + + return PostgresStore + if name == "reset_db": + from dimos.memory.timeseries.postgres import reset_db + + return reset_db + raise AttributeError(f"module {__name__!r} has no attribute {name!r}") + + +__all__ = [ + "InMemoryStore", + "PickleDirStore", + "PostgresStore", + "SqliteStore", + "TimeSeriesStore", + "reset_db", +] diff --git a/dimos/memory/timeseries/base.py b/dimos/memory/timeseries/base.py new file mode 100644 index 0000000000..0d88355b5b --- /dev/null +++ b/dimos/memory/timeseries/base.py @@ -0,0 +1,367 @@ +# 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. +"""Unified time series storage and replay.""" + +from __future__ import annotations + +from abc import ABC, abstractmethod +import time +from typing import TYPE_CHECKING, Generic, TypeVar + +import reactivex as rx +from reactivex import operators as ops +from reactivex.disposable import CompositeDisposable, Disposable +from reactivex.scheduler import TimeoutScheduler + +if TYPE_CHECKING: + from collections.abc import Iterator + + from reactivex.observable import Observable + + from dimos.types.timestamped import Timestamped + +T = TypeVar("T", bound="Timestamped") + + +class TimeSeriesStore(Generic[T], ABC): + """Unified storage + replay for sensor data. + + Implement abstract methods for your backend (in-memory, pickle, sqlite, etc.). + All iteration, streaming, and seek logic comes free from the base class. + + T must be a Timestamped subclass — timestamps are taken from .ts attribute. + """ + + @abstractmethod + def _save(self, timestamp: float, data: T) -> None: + """Save data at timestamp.""" + ... + + @abstractmethod + def _load(self, timestamp: float) -> T | None: + """Load data at exact timestamp. Returns None if not found.""" + ... + + @abstractmethod + def _delete(self, timestamp: float) -> T | None: + """Delete data at exact timestamp. Returns the deleted item or None.""" + ... + + @abstractmethod + def _iter_items( + self, start: float | None = None, end: float | None = None + ) -> Iterator[tuple[float, T]]: + """Lazy iteration of (timestamp, data) in range.""" + ... + + @abstractmethod + def _find_closest_timestamp( + self, timestamp: float, tolerance: float | None = None + ) -> float | None: + """Find closest timestamp. Backend can optimize (binary search, db index, etc.).""" + ... + + @abstractmethod + def _count(self) -> int: + """Return number of stored items.""" + ... + + @abstractmethod + def _last_timestamp(self) -> float | None: + """Return the last (largest) timestamp, or None if empty.""" + ... + + @abstractmethod + def _find_before(self, timestamp: float) -> tuple[float, T] | None: + """Find the last (ts, data) strictly before the given timestamp.""" + ... + + @abstractmethod + def _find_after(self, timestamp: float) -> tuple[float, T] | None: + """Find the first (ts, data) strictly after the given timestamp.""" + ... + + # --- Collection API (built on abstract methods) --- + + def __len__(self) -> int: + return self._count() + + def __iter__(self) -> Iterator[T]: + """Iterate over data items in timestamp order.""" + for _, data in self._iter_items(): + yield data + + def last_timestamp(self) -> float | None: + """Get the last timestamp in the store.""" + return self._last_timestamp() + + def last(self) -> T | None: + """Get the last data item in the store.""" + ts = self._last_timestamp() + if ts is None: + return None + return self._load(ts) + + @property + def start_ts(self) -> float | None: + """Get the start timestamp of the store.""" + return self.first_timestamp() + + @property + def end_ts(self) -> float | None: + """Get the end timestamp of the store.""" + return self._last_timestamp() + + def time_range(self) -> tuple[float, float] | None: + """Get the time range (start, end) of the store.""" + s = self.first_timestamp() + e = self._last_timestamp() + if s is None or e is None: + return None + return (s, e) + + def duration(self) -> float: + """Get the duration of the store in seconds.""" + r = self.time_range() + return (r[1] - r[0]) if r else 0.0 + + def find_before(self, timestamp: float) -> T | None: + """Find the last item strictly before the given timestamp.""" + result = self._find_before(timestamp) + return result[1] if result else None + + def find_after(self, timestamp: float) -> T | None: + """Find the first item strictly after the given timestamp.""" + result = self._find_after(timestamp) + return result[1] if result else None + + def slice_by_time(self, start: float, end: float) -> list[T]: + """Return items in [start, end) range.""" + return [data for _, data in self._iter_items(start=start, end=end)] + + def save(self, *data: T) -> None: + """Save one or more Timestamped items.""" + for item in data: + self._save(item.ts, item) + + def pipe_save(self, source: Observable[T]) -> Observable[T]: + """Operator for Observable.pipe() — saves items using .ts. + + Usage: + observable.pipe(store.pipe_save).subscribe(...) + """ + + def _save_and_return(data: T) -> T: + self._save(data.ts, data) + return data + + return source.pipe(ops.map(_save_and_return)) + + def consume_stream(self, observable: Observable[T]) -> rx.abc.DisposableBase: + """Subscribe to an observable and save items using .ts. + + Usage: + disposable = store.consume_stream(observable) + """ + return observable.subscribe(on_next=lambda data: self._save(data.ts, data)) + + def load(self, timestamp: float) -> T | None: + """Load data at exact timestamp.""" + return self._load(timestamp) + + def prune_old(self, cutoff: float) -> None: + """Prune items older than cutoff timestamp.""" + to_delete = [ts for ts, _ in self._iter_items(end=cutoff)] + for ts in to_delete: + self._delete(ts) + + def find_closest( + self, + timestamp: float, + tolerance: float | None = None, + ) -> T | None: + """Find data closest to the given absolute timestamp.""" + closest_ts = self._find_closest_timestamp(timestamp, tolerance) + if closest_ts is None: + return None + return self._load(closest_ts) + + def find_closest_seek( + self, + relative_seconds: float, + tolerance: float | None = None, + ) -> T | None: + """Find data closest to a time relative to the start.""" + first = self.first_timestamp() + if first is None: + return None + return self.find_closest(first + relative_seconds, tolerance) + + def first_timestamp(self) -> float | None: + """Get the first timestamp in the store.""" + for ts, _ in self._iter_items(): + return ts + return None + + def first(self) -> T | None: + """Get the first data item in the store.""" + for _, data in self._iter_items(): + return data + return None + + def iterate_items( + self, + seek: float | None = None, + duration: float | None = None, + from_timestamp: float | None = None, + loop: bool = False, + ) -> Iterator[tuple[float, T]]: + """Iterate over (timestamp, data) tuples with optional seek/duration.""" + first = self.first_timestamp() + if first is None: + return + + if from_timestamp is not None: + start = from_timestamp + elif seek is not None: + start = first + seek + else: + start = None + + end = None + if duration is not None: + start_ts = start if start is not None else first + end = start_ts + duration + + while True: + yield from self._iter_items(start=start, end=end) + if not loop: + break + + def iterate( + self, + seek: float | None = None, + duration: float | None = None, + from_timestamp: float | None = None, + loop: bool = False, + ) -> Iterator[T]: + """Iterate over data items with optional seek/duration.""" + for _, data in self.iterate_items( + seek=seek, duration=duration, from_timestamp=from_timestamp, loop=loop + ): + yield data + + def iterate_realtime( + self, + speed: float = 1.0, + seek: float | None = None, + duration: float | None = None, + from_timestamp: float | None = None, + loop: bool = False, + ) -> Iterator[T]: + """Iterate data, sleeping to match original timing.""" + prev_ts: float | None = None + for ts, data in self.iterate_items( + seek=seek, duration=duration, from_timestamp=from_timestamp, loop=loop + ): + if prev_ts is not None: + delay = (ts - prev_ts) / speed + if delay > 0: + time.sleep(delay) + prev_ts = ts + yield data + + def stream( + self, + speed: float = 1.0, + seek: float | None = None, + duration: float | None = None, + from_timestamp: float | None = None, + loop: bool = False, + ) -> Observable[T]: + """Stream data as Observable with timing control. + + Uses scheduler-based timing with absolute time reference to prevent drift. + """ + + def subscribe( + observer: rx.abc.ObserverBase[T], + scheduler: rx.abc.SchedulerBase | None = None, + ) -> rx.abc.DisposableBase: + sched = scheduler or TimeoutScheduler() + disp = CompositeDisposable() + is_disposed = False + + iterator = self.iterate_items( + seek=seek, duration=duration, from_timestamp=from_timestamp, loop=loop + ) + + try: + first_ts, first_data = next(iterator) + except StopIteration: + observer.on_completed() + return Disposable() + + start_local_time = time.time() + start_replay_time = first_ts + + observer.on_next(first_data) + + try: + next_message: tuple[float, T] | None = next(iterator) + except StopIteration: + observer.on_completed() + return disp + + def schedule_emission(message: tuple[float, T]) -> None: + nonlocal next_message, is_disposed + + if is_disposed: + return + + msg_ts, msg_data = message + + try: + next_message = next(iterator) + except StopIteration: + next_message = None + + target_time = start_local_time + (msg_ts - start_replay_time) / speed + delay = max(0.0, target_time - time.time()) + + def emit( + _scheduler: rx.abc.SchedulerBase, _state: object + ) -> rx.abc.DisposableBase | None: + if is_disposed: + return None + observer.on_next(msg_data) + if next_message is not None: + schedule_emission(next_message) + else: + observer.on_completed() + return None + + sched.schedule_relative(delay, emit) + + if next_message is not None: + schedule_emission(next_message) + + def dispose() -> None: + nonlocal is_disposed + is_disposed = True + disp.dispose() + + return Disposable(dispose) + + return rx.create(subscribe) diff --git a/dimos/memory/timeseries/inmemory.py b/dimos/memory/timeseries/inmemory.py new file mode 100644 index 0000000000..b67faca644 --- /dev/null +++ b/dimos/memory/timeseries/inmemory.py @@ -0,0 +1,119 @@ +# 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. +"""In-memory backend for TimeSeriesStore.""" + +from collections.abc import Iterator + +from sortedcontainers import SortedKeyList # type: ignore[import-untyped] + +from dimos.memory.timeseries.base import T, TimeSeriesStore + + +class InMemoryStore(TimeSeriesStore[T]): + """In-memory storage using SortedKeyList. O(log n) insert, lookup, and range queries.""" + + def __init__(self) -> None: + self._entries: SortedKeyList = SortedKeyList(key=lambda e: e.ts) + + def _bisect_exact(self, timestamp: float) -> int | None: + """Return index of entry with exact timestamp, or None.""" + pos = self._entries.bisect_key_left(timestamp) + if pos < len(self._entries) and self._entries[pos].ts == timestamp: + return pos # type: ignore[no-any-return] + return None + + def _save(self, timestamp: float, data: T) -> None: + self._entries.add(data) + + def _load(self, timestamp: float) -> T | None: + idx = self._bisect_exact(timestamp) + if idx is not None: + return self._entries[idx] # type: ignore[no-any-return] + return None + + def _delete(self, timestamp: float) -> T | None: + idx = self._bisect_exact(timestamp) + if idx is not None: + data = self._entries[idx] + del self._entries[idx] + return data # type: ignore[no-any-return] + return None + + def __iter__(self) -> Iterator[T]: + yield from self._entries + + def _iter_items( + self, start: float | None = None, end: float | None = None + ) -> Iterator[tuple[float, T]]: + if start is not None and end is not None: + it = self._entries.irange_key(start, end, (True, False)) + elif start is not None: + it = self._entries.irange_key(min_key=start) + elif end is not None: + it = self._entries.irange_key(max_key=end, inclusive=(True, False)) + else: + it = iter(self._entries) + for e in it: + yield (e.ts, e) + + def _find_closest_timestamp( + self, timestamp: float, tolerance: float | None = None + ) -> float | None: + if not self._entries: + return None + + pos = self._entries.bisect_key_left(timestamp) + + candidates: list[float] = [] + if pos > 0: + candidates.append(self._entries[pos - 1].ts) + if pos < len(self._entries): + candidates.append(self._entries[pos].ts) + + if not candidates: + return None + + # On ties, prefer the later timestamp (more recent data) + closest = max(candidates, key=lambda ts: (-abs(ts - timestamp), ts)) + + if tolerance is not None and abs(closest - timestamp) > tolerance: + return None + + return closest + + def _count(self) -> int: + return len(self._entries) + + def _last_timestamp(self) -> float | None: + if not self._entries: + return None + return self._entries[-1].ts # type: ignore[no-any-return] + + def _find_before(self, timestamp: float) -> tuple[float, T] | None: + if not self._entries: + return None + pos = self._entries.bisect_key_left(timestamp) + if pos > 0: + e = self._entries[pos - 1] + return (e.ts, e) + return None + + def _find_after(self, timestamp: float) -> tuple[float, T] | None: + if not self._entries: + return None + pos = self._entries.bisect_key_right(timestamp) + if pos < len(self._entries): + e = self._entries[pos] + return (e.ts, e) + return None diff --git a/dimos/memory/timeseries/legacy.py b/dimos/memory/timeseries/legacy.py new file mode 100644 index 0000000000..821d306d2d --- /dev/null +++ b/dimos/memory/timeseries/legacy.py @@ -0,0 +1,398 @@ +# 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. +"""Legacy pickle directory backend for TimeSeriesStore. + +Compatible with TimedSensorReplay/TimedSensorStorage file format. +""" + +from collections.abc import Callable, Iterator +import glob +import os +from pathlib import Path +import pickle +import re +import time +from typing import Any, cast + +import reactivex as rx +from reactivex.disposable import CompositeDisposable, Disposable +from reactivex.observable import Observable +from reactivex.scheduler import TimeoutScheduler + +from dimos.memory.timeseries.base import T, TimeSeriesStore +from dimos.utils.data import get_data, get_data_dir + + +class LegacyPickleStore(TimeSeriesStore[T]): + """Legacy pickle backend compatible with TimedSensorReplay/TimedSensorStorage. + + File format: + {name}/ + 000.pickle # contains (timestamp, data) tuple + 001.pickle + ... + + Files are assumed to be in chronological order (timestamps increase with file number). + No index is built - iteration is lazy and memory-efficient for large datasets. + + Usage: + # Load existing recording (auto-downloads from LFS if needed) + store = LegacyPickleStore("unitree_go2_bigoffice/lidar") + data = store.find_closest_seek(10.0) + + # Create new recording (directory created on first save) + store = LegacyPickleStore("my_recording/images") + store.save_ts(image) # uses image.ts for timestamp + + Backward compatibility: + This class also supports the old TimedSensorReplay/SensorReplay API: + - iterate_ts() - iterate returning (timestamp, data) tuples + - files - property returning list of file paths + - load_one() - load a single pickle file + """ + + def __init__(self, name: str | Path, autocast: Callable[[Any], T] | None = None) -> None: + """ + Args: + name: Data directory name (e.g. "unitree_go2_bigoffice/lidar") or absolute path. + autocast: Optional function to transform data after loading (for replay) or + before saving (for storage). E.g., `Odometry.from_msg`. + """ + self._name = str(name) + self._root_dir: Path | None = None + self._counter: int = 0 + self._autocast = autocast + + def _get_root_dir(self, for_write: bool = False) -> Path: + """Get root directory, creating on first write if needed.""" + if self._root_dir is not None: + # Ensure directory exists if writing + if for_write: + self._root_dir.mkdir(parents=True, exist_ok=True) + return self._root_dir + + # If absolute path, use directly + if Path(self._name).is_absolute(): + self._root_dir = Path(self._name) + if for_write: + self._root_dir.mkdir(parents=True, exist_ok=True) + elif for_write: + # For writing: use get_data_dir and create if needed + self._root_dir = get_data_dir(self._name) + self._root_dir.mkdir(parents=True, exist_ok=True) + else: + # For reading: use get_data (handles LFS download) + self._root_dir = get_data(self._name) + + return self._root_dir + + def _iter_files(self) -> Iterator[Path]: + """Iterate pickle files in sorted order (by number in filename).""" + + def extract_number(filepath: str) -> int: + basename = os.path.basename(filepath) + match = re.search(r"(\d+)\.pickle$", basename) + return int(match.group(1)) if match else 0 + + root_dir = self._get_root_dir() + files = sorted( + glob.glob(os.path.join(root_dir, "*.pickle")), + key=extract_number, + ) + for f in files: + yield Path(f) + + def _save(self, timestamp: float, data: T) -> None: + root_dir = self._get_root_dir(for_write=True) + + # Initialize counter from existing files if needed + if self._counter == 0: + existing = list(root_dir.glob("*.pickle")) + if existing: + # Find highest existing counter + max_num = 0 + for filepath in existing: + match = re.search(r"(\d+)\.pickle$", filepath.name) + if match: + max_num = max(max_num, int(match.group(1))) + self._counter = max_num + 1 + + full_path = root_dir / f"{self._counter:03d}.pickle" + + if full_path.exists(): + raise RuntimeError(f"File {full_path} already exists") + + # Save as (timestamp, data) tuple for legacy compatibility + with open(full_path, "wb") as f: + pickle.dump((timestamp, data), f) + + self._counter += 1 + + def _load(self, timestamp: float) -> T | None: + """Load data at exact timestamp (linear scan).""" + for ts, data in self._iter_items(): + if ts == timestamp: + return data + return None + + def _delete(self, timestamp: float) -> T | None: + """Delete not supported for legacy pickle format.""" + raise NotImplementedError("LegacyPickleStore does not support deletion") + + def _iter_items( + self, start: float | None = None, end: float | None = None + ) -> Iterator[tuple[float, T]]: + """Lazy iteration - loads one file at a time. + + Handles both timed format (timestamp, data) and non-timed format (just data). + For non-timed data, uses file index as synthetic timestamp. + """ + for idx, filepath in enumerate(self._iter_files()): + try: + with open(filepath, "rb") as f: + raw = pickle.load(f) + + # Handle both timed (timestamp, data) and non-timed (just data) formats + if isinstance(raw, tuple) and len(raw) == 2: + ts, data = raw + ts = float(ts) + else: + # Non-timed format: use index as synthetic timestamp + ts = float(idx) + data = raw + except Exception: + continue + + if start is not None and ts < start: + continue + if end is not None and ts >= end: + break + + if self._autocast is not None: + data = self._autocast(data) + yield (ts, cast("T", data)) + + def _find_closest_timestamp( + self, timestamp: float, tolerance: float | None = None + ) -> float | None: + """Linear scan with early exit (assumes timestamps are monotonically increasing).""" + closest_ts: float | None = None + closest_diff = float("inf") + + for ts, _ in self._iter_items(): + diff = abs(ts - timestamp) + + if diff < closest_diff: + closest_diff = diff + closest_ts = ts + elif diff > closest_diff: + # Moving away from target, can stop + break + + if closest_ts is None: + return None + + if tolerance is not None and closest_diff > tolerance: + return None + + return closest_ts + + def _count(self) -> int: + return sum(1 for _ in self._iter_files()) + + def _last_timestamp(self) -> float | None: + last_ts: float | None = None + for ts, _ in self._iter_items(): + last_ts = ts + return last_ts + + def _find_before(self, timestamp: float) -> tuple[float, T] | None: + result: tuple[float, T] | None = None + for ts, data in self._iter_items(): + if ts < timestamp: + result = (ts, data) + else: + break + return result + + def _find_after(self, timestamp: float) -> tuple[float, T] | None: + for ts, data in self._iter_items(): + if ts > timestamp: + return (ts, data) + return None + + # === Backward-compatible API (TimedSensorReplay/SensorReplay) === + + @property + def files(self) -> list[Path]: + """Return list of pickle files (backward compatibility with SensorReplay).""" + return list(self._iter_files()) + + def load_one(self, name: int | str | Path) -> T | Any: + """Load a single pickle file (backward compatibility with SensorReplay). + + Args: + name: File index (int), filename without extension (str), or full path (Path) + + Returns: + For TimedSensorReplay: (timestamp, data) tuple + For SensorReplay: just the data + """ + root_dir = self._get_root_dir() + + if isinstance(name, int): + full_path = root_dir / f"{name:03d}.pickle" + elif isinstance(name, Path): + full_path = name + else: + full_path = root_dir / Path(f"{name}.pickle") + + with open(full_path, "rb") as f: + data = pickle.load(f) + + # Legacy format: (timestamp, data) tuple + if isinstance(data, tuple) and len(data) == 2: + ts, payload = data + if self._autocast is not None: + payload = self._autocast(payload) + return (ts, payload) + + # Non-timed format: just data + if self._autocast is not None: + data = self._autocast(data) + return data + + def iterate_ts( + self, + seek: float | None = None, + duration: float | None = None, + from_timestamp: float | None = None, + loop: bool = False, + ) -> Iterator[tuple[float, T]]: + """Iterate with timestamps (backward compatibility with TimedSensorReplay). + + Args: + seek: Relative seconds from start + duration: Duration window in seconds + from_timestamp: Absolute timestamp to start from + loop: Whether to loop the data + + Yields: + (timestamp, data) tuples + """ + first = self.first_timestamp() + if first is None: + return + + # Calculate start timestamp + start: float | None = None + if from_timestamp is not None: + start = from_timestamp + elif seek is not None: + start = first + seek + + # Calculate end timestamp + end: float | None = None + if duration is not None: + start_ts = start if start is not None else first + end = start_ts + duration + + while True: + yield from self._iter_items(start=start, end=end) + if not loop: + break + + def stream( + self, + speed: float = 1.0, + seek: float | None = None, + duration: float | None = None, + from_timestamp: float | None = None, + loop: bool = False, + ) -> Observable[T]: + """Stream data as Observable with timing control. + + Uses stored timestamps from pickle files for timing (not data.ts). + """ + + def subscribe( + observer: rx.abc.ObserverBase[T], + scheduler: rx.abc.SchedulerBase | None = None, + ) -> rx.abc.DisposableBase: + sched = scheduler or TimeoutScheduler() + disp = CompositeDisposable() + is_disposed = False + + iterator = self.iterate_ts( + seek=seek, duration=duration, from_timestamp=from_timestamp, loop=loop + ) + + try: + first_ts, first_data = next(iterator) + except StopIteration: + observer.on_completed() + return Disposable() + + start_local_time = time.time() + start_replay_time = first_ts + + observer.on_next(first_data) + + try: + next_message: tuple[float, T] | None = next(iterator) + except StopIteration: + observer.on_completed() + return disp + + def schedule_emission(message: tuple[float, T]) -> None: + nonlocal next_message, is_disposed + + if is_disposed: + return + + ts, data = message + + try: + next_message = next(iterator) + except StopIteration: + next_message = None + + target_time = start_local_time + (ts - start_replay_time) / speed + delay = max(0.0, target_time - time.time()) + + def emit( + _scheduler: rx.abc.SchedulerBase, _state: object + ) -> rx.abc.DisposableBase | None: + if is_disposed: + return None + observer.on_next(data) + if next_message is not None: + schedule_emission(next_message) + else: + observer.on_completed() + return None + + sched.schedule_relative(delay, emit) + + if next_message is not None: + schedule_emission(next_message) + + def dispose() -> None: + nonlocal is_disposed + is_disposed = True + disp.dispose() + + return Disposable(dispose) + + return rx.create(subscribe) diff --git a/dimos/memory/timeseries/pickledir.py b/dimos/memory/timeseries/pickledir.py new file mode 100644 index 0000000000..9e8cd5a249 --- /dev/null +++ b/dimos/memory/timeseries/pickledir.py @@ -0,0 +1,198 @@ +# 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. +"""Pickle directory backend for TimeSeriesStore.""" + +import bisect +from collections.abc import Iterator +import glob +import os +from pathlib import Path +import pickle + +from dimos.memory.timeseries.base import T, TimeSeriesStore +from dimos.utils.data import get_data, get_data_dir + + +class PickleDirStore(TimeSeriesStore[T]): + """Pickle directory backend. Files named by timestamp. + + Directory structure: + {name}/ + 1704067200.123.pickle + 1704067200.456.pickle + ... + + Usage: + # Load existing recording (auto-downloads from LFS if needed) + store = PickleDirStore("unitree_go2_bigoffice/lidar") + data = store.find_closest_seek(10.0) + + # Create new recording (directory created on first save) + store = PickleDirStore("my_recording/images") + store.save(image) # saves using image.ts + """ + + def __init__(self, name: str) -> None: + """ + Args: + name: Data directory name (e.g. "unitree_go2_bigoffice/lidar") + """ + self._name = name + self._root_dir: Path | None = None + + # Cached sorted timestamps for find_closest + self._timestamps: list[float] | None = None + + def _get_root_dir(self, for_write: bool = False) -> Path: + """Get root directory, creating on first write if needed.""" + if self._root_dir is not None: + return self._root_dir + + # If absolute path, use directly + if Path(self._name).is_absolute(): + self._root_dir = Path(self._name) + if for_write: + self._root_dir.mkdir(parents=True, exist_ok=True) + elif for_write: + # For writing: use get_data_dir and create if needed + self._root_dir = get_data_dir(self._name) + self._root_dir.mkdir(parents=True, exist_ok=True) + else: + # For reading: use get_data (handles LFS download) + self._root_dir = get_data(self._name) + + return self._root_dir + + def _save(self, timestamp: float, data: T) -> None: + root_dir = self._get_root_dir(for_write=True) + full_path = root_dir / f"{timestamp}.pickle" + + if full_path.exists(): + raise RuntimeError(f"File {full_path} already exists") + + with open(full_path, "wb") as f: + pickle.dump(data, f) + + self._timestamps = None # Invalidate cache + + def _load(self, timestamp: float) -> T | None: + filepath = self._get_root_dir() / f"{timestamp}.pickle" + if filepath.exists(): + return self._load_file(filepath) + return None + + def _delete(self, timestamp: float) -> T | None: + filepath = self._get_root_dir() / f"{timestamp}.pickle" + if filepath.exists(): + data = self._load_file(filepath) + filepath.unlink() + self._timestamps = None # Invalidate cache + return data + return None + + def _iter_items( + self, start: float | None = None, end: float | None = None + ) -> Iterator[tuple[float, T]]: + for ts in self._get_timestamps(): + if start is not None and ts < start: + continue + if end is not None and ts >= end: + break + data = self._load(ts) + if data is not None: + yield (ts, data) + + def _find_closest_timestamp( + self, timestamp: float, tolerance: float | None = None + ) -> float | None: + timestamps = self._get_timestamps() + if not timestamps: + return None + + pos = bisect.bisect_left(timestamps, timestamp) + + # Check neighbors + candidates = [] + if pos > 0: + candidates.append(timestamps[pos - 1]) + if pos < len(timestamps): + candidates.append(timestamps[pos]) + + if not candidates: + return None + + closest = min(candidates, key=lambda ts: abs(ts - timestamp)) + + if tolerance is not None and abs(closest - timestamp) > tolerance: + return None + + return closest + + def _get_timestamps(self) -> list[float]: + """Get sorted list of all timestamps.""" + if self._timestamps is not None: + return self._timestamps + + timestamps: list[float] = [] + root_dir = self._get_root_dir() + for filepath in glob.glob(os.path.join(root_dir, "*.pickle")): + try: + ts = float(Path(filepath).stem) + timestamps.append(ts) + except ValueError: + continue + + timestamps.sort() + self._timestamps = timestamps + return timestamps + + def _count(self) -> int: + return len(self._get_timestamps()) + + def _last_timestamp(self) -> float | None: + timestamps = self._get_timestamps() + return timestamps[-1] if timestamps else None + + def _find_before(self, timestamp: float) -> tuple[float, T] | None: + timestamps = self._get_timestamps() + if not timestamps: + return None + pos = bisect.bisect_left(timestamps, timestamp) + if pos > 0: + ts = timestamps[pos - 1] + data = self._load(ts) + if data is not None: + return (ts, data) + return None + + def _find_after(self, timestamp: float) -> tuple[float, T] | None: + timestamps = self._get_timestamps() + if not timestamps: + return None + pos = bisect.bisect_right(timestamps, timestamp) + if pos < len(timestamps): + ts = timestamps[pos] + data = self._load(ts) + if data is not None: + return (ts, data) + return None + + def _load_file(self, filepath: Path) -> T | None: + """Load data from a pickle file (LRU cached).""" + try: + with open(filepath, "rb") as f: + data: T = pickle.load(f) + return data + except Exception: + return None diff --git a/dimos/memory/timeseries/postgres.py b/dimos/memory/timeseries/postgres.py new file mode 100644 index 0000000000..0daae44adb --- /dev/null +++ b/dimos/memory/timeseries/postgres.py @@ -0,0 +1,312 @@ +# 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. +"""PostgreSQL backend for TimeSeriesStore.""" + +from collections.abc import Iterator +import pickle +import re + +import psycopg2 # type: ignore[import-untyped] +import psycopg2.extensions # type: ignore[import-untyped] + +from dimos.core.resource import Resource +from dimos.memory.timeseries.base import T, TimeSeriesStore + +# Valid SQL identifier: alphanumeric and underscores, not starting with digit +_VALID_IDENTIFIER = re.compile(r"^[a-zA-Z_][a-zA-Z0-9_]*$") + + +def _validate_identifier(name: str) -> str: + """Validate SQL identifier to prevent injection.""" + if not _VALID_IDENTIFIER.match(name): + raise ValueError( + f"Invalid identifier '{name}': must be alphanumeric/underscore, not start with digit" + ) + if len(name) > 128: + raise ValueError(f"Identifier too long: {len(name)} > 128") + return name + + +class PostgresStore(TimeSeriesStore[T], Resource): + """PostgreSQL backend for sensor data. + + Multiple stores can share the same database with different tables. + Implements Resource for lifecycle management (start/stop/dispose). + + Usage: + # Create store + store = PostgresStore("lidar") + store.start() # open connection + + # Use store + store.save(data) # saves using data.ts + data = store.find_closest_seek(10.0) + + # Cleanup + store.stop() # close connection + + # Multiple sensors in same db + lidar = PostgresStore("lidar") + images = PostgresStore("images") + + # Manual run management via table naming + run1_lidar = PostgresStore("run1_lidar") + """ + + def __init__( + self, + table: str, + db: str = "dimensional", + host: str = "localhost", + port: int = 5432, + user: str | None = None, + ) -> None: + """ + Args: + table: Table name for this sensor's data (alphanumeric/underscore only). + db: Database name (alphanumeric/underscore only). + host: PostgreSQL host. + port: PostgreSQL port. + user: PostgreSQL user. Defaults to current system user. + """ + self._table = _validate_identifier(table) + self._db = _validate_identifier(db) + self._host = host + self._port = port + self._user = user + self._conn: psycopg2.extensions.connection | None = None + self._table_created = False + + def start(self) -> None: + """Open database connection.""" + if self._conn is not None: + return + self._conn = psycopg2.connect( + dbname=self._db, + host=self._host, + port=self._port, + user=self._user, + ) + + def stop(self) -> None: + """Close database connection.""" + if self._conn is not None: + self._conn.close() + self._conn = None + + def _get_conn(self) -> psycopg2.extensions.connection: + """Get connection, starting if needed.""" + if self._conn is None: + self.start() + assert self._conn is not None + return self._conn + + def _ensure_table(self) -> None: + """Create table if it doesn't exist.""" + if self._table_created: + return + conn = self._get_conn() + with conn.cursor() as cur: + cur.execute(f""" + CREATE TABLE IF NOT EXISTS {self._table} ( + timestamp DOUBLE PRECISION PRIMARY KEY, + data BYTEA NOT NULL + ) + """) + cur.execute(f""" + CREATE INDEX IF NOT EXISTS idx_{self._table}_ts + ON {self._table}(timestamp) + """) + conn.commit() + self._table_created = True + + def _save(self, timestamp: float, data: T) -> None: + self._ensure_table() + conn = self._get_conn() + blob = pickle.dumps(data) + with conn.cursor() as cur: + cur.execute( + f""" + INSERT INTO {self._table} (timestamp, data) VALUES (%s, %s) + ON CONFLICT (timestamp) DO UPDATE SET data = EXCLUDED.data + """, + (timestamp, psycopg2.Binary(blob)), + ) + conn.commit() + + def _load(self, timestamp: float) -> T | None: + self._ensure_table() + conn = self._get_conn() + with conn.cursor() as cur: + cur.execute(f"SELECT data FROM {self._table} WHERE timestamp = %s", (timestamp,)) + row = cur.fetchone() + if row is None: + return None + data: T = pickle.loads(row[0]) + return data + + def _delete(self, timestamp: float) -> T | None: + data = self._load(timestamp) + if data is not None: + conn = self._get_conn() + with conn.cursor() as cur: + cur.execute(f"DELETE FROM {self._table} WHERE timestamp = %s", (timestamp,)) + conn.commit() + return data + + def _iter_items( + self, start: float | None = None, end: float | None = None + ) -> Iterator[tuple[float, T]]: + self._ensure_table() + conn = self._get_conn() + + query = f"SELECT timestamp, data FROM {self._table}" + params: list[float] = [] + conditions = [] + + if start is not None: + conditions.append("timestamp >= %s") + params.append(start) + if end is not None: + conditions.append("timestamp < %s") + params.append(end) + + if conditions: + query += " WHERE " + " AND ".join(conditions) + query += " ORDER BY timestamp" + + with conn.cursor() as cur: + cur.execute(query, params) + for row in cur: + ts: float = row[0] + data: T = pickle.loads(row[1]) + yield (ts, data) + + def _find_closest_timestamp( + self, timestamp: float, tolerance: float | None = None + ) -> float | None: + self._ensure_table() + conn = self._get_conn() + + with conn.cursor() as cur: + # Get closest timestamp <= target + cur.execute( + f""" + SELECT timestamp FROM {self._table} + WHERE timestamp <= %s + ORDER BY timestamp DESC LIMIT 1 + """, + (timestamp,), + ) + before = cur.fetchone() + + # Get closest timestamp >= target + cur.execute( + f""" + SELECT timestamp FROM {self._table} + WHERE timestamp >= %s + ORDER BY timestamp ASC LIMIT 1 + """, + (timestamp,), + ) + after = cur.fetchone() + + candidates: list[float] = [] + if before: + candidates.append(before[0]) + if after: + candidates.append(after[0]) + + if not candidates: + return None + + closest = min(candidates, key=lambda ts: abs(ts - timestamp)) + + if tolerance is not None and abs(closest - timestamp) > tolerance: + return None + + return closest + + def _count(self) -> int: + self._ensure_table() + conn = self._get_conn() + with conn.cursor() as cur: + cur.execute(f"SELECT COUNT(*) FROM {self._table}") + row = cur.fetchone() + return row[0] if row else 0 # type: ignore[no-any-return] + + def _last_timestamp(self) -> float | None: + self._ensure_table() + conn = self._get_conn() + with conn.cursor() as cur: + cur.execute(f"SELECT MAX(timestamp) FROM {self._table}") + row = cur.fetchone() + if row is None or row[0] is None: + return None + return row[0] # type: ignore[no-any-return] + + def _find_before(self, timestamp: float) -> tuple[float, T] | None: + self._ensure_table() + conn = self._get_conn() + with conn.cursor() as cur: + cur.execute( + f"SELECT timestamp, data FROM {self._table} WHERE timestamp < %s ORDER BY timestamp DESC LIMIT 1", + (timestamp,), + ) + row = cur.fetchone() + if row is None: + return None + return (row[0], pickle.loads(row[1])) + + def _find_after(self, timestamp: float) -> tuple[float, T] | None: + self._ensure_table() + conn = self._get_conn() + with conn.cursor() as cur: + cur.execute( + f"SELECT timestamp, data FROM {self._table} WHERE timestamp > %s ORDER BY timestamp ASC LIMIT 1", + (timestamp,), + ) + row = cur.fetchone() + if row is None: + return None + return (row[0], pickle.loads(row[1])) + + +def reset_db(db: str = "dimensional", host: str = "localhost", port: int = 5432) -> None: + """Drop and recreate database. Simple migration strategy. + + WARNING: This deletes all data in the database! + + Args: + db: Database name to reset (alphanumeric/underscore only). + host: PostgreSQL host. + port: PostgreSQL port. + """ + db = _validate_identifier(db) + # Connect to 'postgres' database to drop/create + conn = psycopg2.connect(dbname="postgres", host=host, port=port) + conn.autocommit = True + with conn.cursor() as cur: + # Terminate existing connections + cur.execute( + """ + SELECT pg_terminate_backend(pid) + FROM pg_stat_activity + WHERE datname = %s AND pid <> pg_backend_pid() + """, + (db,), + ) + cur.execute(f"DROP DATABASE IF EXISTS {db}") + cur.execute(f"CREATE DATABASE {db}") + conn.close() diff --git a/dimos/memory/timeseries/sqlite.py b/dimos/memory/timeseries/sqlite.py new file mode 100644 index 0000000000..6e2ac7a7f5 --- /dev/null +++ b/dimos/memory/timeseries/sqlite.py @@ -0,0 +1,268 @@ +# 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. +"""SQLite backend for TimeSeriesStore.""" + +from collections.abc import Iterator +from pathlib import Path +import pickle +import re +import sqlite3 + +from dimos.memory.timeseries.base import T, TimeSeriesStore +from dimos.utils.data import get_data, get_data_dir + +# Valid SQL identifier: alphanumeric and underscores, not starting with digit +_VALID_IDENTIFIER = re.compile(r"^[a-zA-Z_][a-zA-Z0-9_]*$") + + +def _validate_identifier(name: str) -> str: + """Validate SQL identifier to prevent injection.""" + if not _VALID_IDENTIFIER.match(name): + raise ValueError( + f"Invalid identifier '{name}': must be alphanumeric/underscore, not start with digit" + ) + if len(name) > 128: + raise ValueError(f"Identifier too long: {len(name)} > 128") + return name + + +class SqliteStore(TimeSeriesStore[T]): + """SQLite backend for sensor data. Good for indexed queries and single-file storage. + + Data is stored as pickled BLOBs with timestamp as indexed column. + + Usage: + # Named store (uses data/ directory, auto-downloads from LFS if needed) + store = SqliteStore("recordings/lidar") # -> data/recordings/lidar.db + store.save(data) # saves using data.ts + + # Absolute path + store = SqliteStore("/path/to/sensors.db") + + # In-memory (for testing) + store = SqliteStore(":memory:") + + # Multiple tables in one DB + store = SqliteStore("recordings/sensors", table="lidar") + """ + + def __init__(self, name: str | Path, table: str = "sensor_data") -> None: + """ + Args: + name: Data name (e.g. "recordings/lidar") resolved via get_data, + absolute path, or ":memory:" for in-memory. + table: Table name for this sensor's data (alphanumeric/underscore only). + """ + self._name = str(name) + self._table = _validate_identifier(table) + self._db_path: str | None = None + self._conn: sqlite3.Connection | None = None + + def _get_db_path(self, for_write: bool = False) -> str: + """Get database path, resolving via get_data if needed.""" + if self._db_path is not None: + return self._db_path + + # Special case for in-memory + if self._name == ":memory:": + self._db_path = ":memory:" + return self._db_path + + # If absolute path, use directly + if Path(self._name).is_absolute(): + self._db_path = self._name + elif for_write: + # For writing: use get_data_dir + db_file = get_data_dir(self._name + ".db") + db_file.parent.mkdir(parents=True, exist_ok=True) + self._db_path = str(db_file) + else: + # For reading: use get_data (handles LFS download) + # Try with .db extension first + try: + db_file = get_data(self._name + ".db") + self._db_path = str(db_file) + except FileNotFoundError: + # Fall back to get_data_dir for new databases + db_file = get_data_dir(self._name + ".db") + db_file.parent.mkdir(parents=True, exist_ok=True) + self._db_path = str(db_file) + + return self._db_path + + def _get_conn(self) -> sqlite3.Connection: + """Get or create database connection.""" + if self._conn is None: + db_path = self._get_db_path(for_write=True) + self._conn = sqlite3.connect(db_path, check_same_thread=False) + self._create_table() + return self._conn + + def _create_table(self) -> None: + """Create table if it doesn't exist.""" + conn = self._conn + assert conn is not None + conn.execute(f""" + CREATE TABLE IF NOT EXISTS {self._table} ( + timestamp REAL PRIMARY KEY, + data BLOB NOT NULL + ) + """) + conn.execute(f""" + CREATE INDEX IF NOT EXISTS idx_{self._table}_timestamp + ON {self._table}(timestamp) + """) + conn.commit() + + def _save(self, timestamp: float, data: T) -> None: + conn = self._get_conn() + blob = pickle.dumps(data) + conn.execute( + f"INSERT OR REPLACE INTO {self._table} (timestamp, data) VALUES (?, ?)", + (timestamp, blob), + ) + conn.commit() + + def _load(self, timestamp: float) -> T | None: + conn = self._get_conn() + cursor = conn.execute(f"SELECT data FROM {self._table} WHERE timestamp = ?", (timestamp,)) + row = cursor.fetchone() + if row is None: + return None + data: T = pickle.loads(row[0]) + return data + + def _delete(self, timestamp: float) -> T | None: + data = self._load(timestamp) + if data is not None: + conn = self._get_conn() + conn.execute(f"DELETE FROM {self._table} WHERE timestamp = ?", (timestamp,)) + conn.commit() + return data + + def _iter_items( + self, start: float | None = None, end: float | None = None + ) -> Iterator[tuple[float, T]]: + conn = self._get_conn() + + # Build query with optional range filters + query = f"SELECT timestamp, data FROM {self._table}" + params: list[float] = [] + conditions = [] + + if start is not None: + conditions.append("timestamp >= ?") + params.append(start) + if end is not None: + conditions.append("timestamp < ?") + params.append(end) + + if conditions: + query += " WHERE " + " AND ".join(conditions) + query += " ORDER BY timestamp" + + cursor = conn.execute(query, params) + for row in cursor: + ts: float = row[0] + data: T = pickle.loads(row[1]) + yield (ts, data) + + def _find_closest_timestamp( + self, timestamp: float, tolerance: float | None = None + ) -> float | None: + conn = self._get_conn() + + # Find closest timestamp using SQL + # Get the closest timestamp <= target + cursor = conn.execute( + f""" + SELECT timestamp FROM {self._table} + WHERE timestamp <= ? + ORDER BY timestamp DESC LIMIT 1 + """, + (timestamp,), + ) + before = cursor.fetchone() + + # Get the closest timestamp >= target + cursor = conn.execute( + f""" + SELECT timestamp FROM {self._table} + WHERE timestamp >= ? + ORDER BY timestamp ASC LIMIT 1 + """, + (timestamp,), + ) + after = cursor.fetchone() + + # Find the closest of the two + candidates: list[float] = [] + if before: + candidates.append(before[0]) + if after: + candidates.append(after[0]) + + if not candidates: + return None + + closest = min(candidates, key=lambda ts: abs(ts - timestamp)) + + if tolerance is not None and abs(closest - timestamp) > tolerance: + return None + + return closest + + def _count(self) -> int: + conn = self._get_conn() + cursor = conn.execute(f"SELECT COUNT(*) FROM {self._table}") + return cursor.fetchone()[0] # type: ignore[no-any-return] + + def _last_timestamp(self) -> float | None: + conn = self._get_conn() + cursor = conn.execute(f"SELECT MAX(timestamp) FROM {self._table}") + row = cursor.fetchone() + if row is None or row[0] is None: + return None + return row[0] # type: ignore[no-any-return] + + def _find_before(self, timestamp: float) -> tuple[float, T] | None: + conn = self._get_conn() + cursor = conn.execute( + f"SELECT timestamp, data FROM {self._table} WHERE timestamp < ? ORDER BY timestamp DESC LIMIT 1", + (timestamp,), + ) + row = cursor.fetchone() + if row is None: + return None + return (row[0], pickle.loads(row[1])) + + def _find_after(self, timestamp: float) -> tuple[float, T] | None: + conn = self._get_conn() + cursor = conn.execute( + f"SELECT timestamp, data FROM {self._table} WHERE timestamp > ? ORDER BY timestamp ASC LIMIT 1", + (timestamp,), + ) + row = cursor.fetchone() + if row is None: + return None + return (row[0], pickle.loads(row[1])) + + def close(self) -> None: + """Close the database connection.""" + if self._conn is not None: + self._conn.close() + self._conn = None + + def __del__(self) -> None: + self.close() diff --git a/dimos/memory/timeseries/test_base.py b/dimos/memory/timeseries/test_base.py new file mode 100644 index 0000000000..9491d2c93c --- /dev/null +++ b/dimos/memory/timeseries/test_base.py @@ -0,0 +1,468 @@ +# 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. +"""Tests for TimeSeriesStore implementations.""" + +from dataclasses import dataclass +from pathlib import Path +import tempfile +import uuid + +import pytest + +from dimos.memory.timeseries.base import TimeSeriesStore +from dimos.memory.timeseries.inmemory import InMemoryStore +from dimos.memory.timeseries.legacy import LegacyPickleStore +from dimos.memory.timeseries.pickledir import PickleDirStore +from dimos.memory.timeseries.sqlite import SqliteStore +from dimos.types.timestamped import Timestamped + + +@dataclass +class SampleData(Timestamped): + """Simple timestamped data for testing.""" + + value: str + + def __init__(self, value: str, ts: float) -> None: + super().__init__(ts) + self.value = value + + def __eq__(self, other: object) -> bool: + if isinstance(other, SampleData): + return self.value == other.value and self.ts == other.ts + return False + + +@pytest.fixture +def temp_dir(): + """Create a temporary directory for file-based store tests.""" + with tempfile.TemporaryDirectory() as tmpdir: + yield tmpdir + + +def make_in_memory_store() -> TimeSeriesStore[SampleData]: + return InMemoryStore[SampleData]() + + +def make_pickle_dir_store(tmpdir: str) -> TimeSeriesStore[SampleData]: + return PickleDirStore[SampleData](tmpdir) + + +def make_sqlite_store(tmpdir: str) -> TimeSeriesStore[SampleData]: + return SqliteStore[SampleData](Path(tmpdir) / "test.db") + + +def make_legacy_pickle_store(tmpdir: str) -> TimeSeriesStore[SampleData]: + return LegacyPickleStore[SampleData](Path(tmpdir) / "legacy") + + +# Base test data (always available) +testdata: list[tuple[object, str]] = [ + (lambda _: make_in_memory_store(), "InMemoryStore"), + (lambda tmpdir: make_pickle_dir_store(tmpdir), "PickleDirStore"), + (lambda tmpdir: make_sqlite_store(tmpdir), "SqliteStore"), + (lambda tmpdir: make_legacy_pickle_store(tmpdir), "LegacyPickleStore"), +] + +# Track postgres tables to clean up +_postgres_tables: list[str] = [] + +try: + import psycopg2 + + from dimos.memory.timeseries.postgres import PostgresStore + + # Test connection + _test_conn = psycopg2.connect(dbname="dimensional") + _test_conn.close() + + def make_postgres_store(_tmpdir: str) -> TimeSeriesStore[SampleData]: + """Create PostgresStore with unique table name.""" + table = f"test_{uuid.uuid4().hex[:8]}" + _postgres_tables.append(table) + store = PostgresStore[SampleData](table) + store.start() + return store + + testdata.append((lambda tmpdir: make_postgres_store(tmpdir), "PostgresStore")) + + @pytest.fixture(autouse=True) + def cleanup_postgres_tables(): + """Clean up postgres test tables after each test.""" + yield + if _postgres_tables: + try: + conn = psycopg2.connect(dbname="dimensional") + conn.autocommit = True + with conn.cursor() as cur: + for table in _postgres_tables: + cur.execute(f"DROP TABLE IF EXISTS {table}") + conn.close() + except Exception: + pass # Ignore cleanup errors + _postgres_tables.clear() + +except Exception: + print("PostgreSQL not available") + + +@pytest.mark.parametrize("store_factory,store_name", testdata) +class TestTimeSeriesStore: + """Parametrized tests for all TimeSeriesStore implementations.""" + + def test_save_and_load(self, store_factory, store_name, temp_dir): + store = store_factory(temp_dir) + store.save(SampleData("data_at_1", 1.0)) + store.save(SampleData("data_at_2", 2.0)) + + assert store.load(1.0) == SampleData("data_at_1", 1.0) + assert store.load(2.0) == SampleData("data_at_2", 2.0) + assert store.load(3.0) is None + + def test_find_closest_timestamp(self, store_factory, store_name, temp_dir): + store = store_factory(temp_dir) + store.save(SampleData("a", 1.0), SampleData("b", 2.0), SampleData("c", 3.0)) + + # Exact match + assert store._find_closest_timestamp(2.0) == 2.0 + + # Closest to 1.4 is 1.0 + assert store._find_closest_timestamp(1.4) == 1.0 + + # Closest to 1.6 is 2.0 + assert store._find_closest_timestamp(1.6) == 2.0 + + # With tolerance + assert store._find_closest_timestamp(1.4, tolerance=0.5) == 1.0 + assert store._find_closest_timestamp(1.4, tolerance=0.3) is None + + def test_iter_items(self, store_factory, store_name, temp_dir): + store = store_factory(temp_dir) + store.save(SampleData("a", 1.0), SampleData("b", 2.0), SampleData("c", 3.0)) + + # Should iterate in timestamp order + items = list(store._iter_items()) + assert items == [ + (1.0, SampleData("a", 1.0)), + (2.0, SampleData("b", 2.0)), + (3.0, SampleData("c", 3.0)), + ] + + def test_iter_items_with_range(self, store_factory, store_name, temp_dir): + store = store_factory(temp_dir) + store.save( + SampleData("a", 1.0), + SampleData("b", 2.0), + SampleData("c", 3.0), + SampleData("d", 4.0), + ) + + # Start only + items = list(store._iter_items(start=2.0)) + assert items == [ + (2.0, SampleData("b", 2.0)), + (3.0, SampleData("c", 3.0)), + (4.0, SampleData("d", 4.0)), + ] + + # End only + items = list(store._iter_items(end=3.0)) + assert items == [(1.0, SampleData("a", 1.0)), (2.0, SampleData("b", 2.0))] + + # Both + items = list(store._iter_items(start=2.0, end=4.0)) + assert items == [(2.0, SampleData("b", 2.0)), (3.0, SampleData("c", 3.0))] + + def test_empty_store(self, store_factory, store_name, temp_dir): + store = store_factory(temp_dir) + + assert store.load(1.0) is None + assert store._find_closest_timestamp(1.0) is None + assert list(store._iter_items()) == [] + + def test_first_and_first_timestamp(self, store_factory, store_name, temp_dir): + store = store_factory(temp_dir) + + # Empty store + assert store.first() is None + assert store.first_timestamp() is None + + # Add data (in chronological order) + store.save(SampleData("a", 1.0), SampleData("b", 2.0), SampleData("c", 3.0)) + + # Should return first by timestamp + assert store.first_timestamp() == 1.0 + assert store.first() == SampleData("a", 1.0) + + def test_find_closest(self, store_factory, store_name, temp_dir): + store = store_factory(temp_dir) + store.save(SampleData("a", 1.0), SampleData("b", 2.0), SampleData("c", 3.0)) + + # Exact match + assert store.find_closest(2.0) == SampleData("b", 2.0) + + # Closest to 1.4 is 1.0 + assert store.find_closest(1.4) == SampleData("a", 1.0) + + # Closest to 1.6 is 2.0 + assert store.find_closest(1.6) == SampleData("b", 2.0) + + # With tolerance + assert store.find_closest(1.4, tolerance=0.5) == SampleData("a", 1.0) + assert store.find_closest(1.4, tolerance=0.3) is None + + def test_find_closest_seek(self, store_factory, store_name, temp_dir): + store = store_factory(temp_dir) + store.save(SampleData("a", 10.0), SampleData("b", 11.0), SampleData("c", 12.0)) + + # Seek 0 = first item (10.0) + assert store.find_closest_seek(0.0) == SampleData("a", 10.0) + + # Seek 1.0 = 11.0 + assert store.find_closest_seek(1.0) == SampleData("b", 11.0) + + # Seek 1.4 -> closest to 11.4 is 11.0 + assert store.find_closest_seek(1.4) == SampleData("b", 11.0) + + # Seek 1.6 -> closest to 11.6 is 12.0 + assert store.find_closest_seek(1.6) == SampleData("c", 12.0) + + # With tolerance + assert store.find_closest_seek(1.4, tolerance=0.5) == SampleData("b", 11.0) + assert store.find_closest_seek(1.4, tolerance=0.3) is None + + def test_iterate(self, store_factory, store_name, temp_dir): + store = store_factory(temp_dir) + store.save(SampleData("a", 1.0), SampleData("b", 2.0), SampleData("c", 3.0)) + + # Should iterate in timestamp order, returning data only (not tuples) + items = list(store.iterate()) + assert items == [ + SampleData("a", 1.0), + SampleData("b", 2.0), + SampleData("c", 3.0), + ] + + def test_iterate_with_seek_and_duration(self, store_factory, store_name, temp_dir): + store = store_factory(temp_dir) + store.save( + SampleData("a", 10.0), + SampleData("b", 11.0), + SampleData("c", 12.0), + SampleData("d", 13.0), + ) + + # Seek from start + items = list(store.iterate(seek=1.0)) + assert items == [ + SampleData("b", 11.0), + SampleData("c", 12.0), + SampleData("d", 13.0), + ] + + # Duration + items = list(store.iterate(duration=2.0)) + assert items == [SampleData("a", 10.0), SampleData("b", 11.0)] + + # Seek + duration + items = list(store.iterate(seek=1.0, duration=2.0)) + assert items == [SampleData("b", 11.0), SampleData("c", 12.0)] + + # from_timestamp + items = list(store.iterate(from_timestamp=12.0)) + assert items == [SampleData("c", 12.0), SampleData("d", 13.0)] + + def test_variadic_save(self, store_factory, store_name, temp_dir): + store = store_factory(temp_dir) + + # Save multiple items at once + store.save( + SampleData("a", 1.0), + SampleData("b", 2.0), + SampleData("c", 3.0), + ) + + assert store.load(1.0) == SampleData("a", 1.0) + assert store.load(2.0) == SampleData("b", 2.0) + assert store.load(3.0) == SampleData("c", 3.0) + + def test_pipe_save(self, store_factory, store_name, temp_dir): + import reactivex as rx + + store = store_factory(temp_dir) + + # Create observable with test data + source = rx.of( + SampleData("a", 1.0), + SampleData("b", 2.0), + SampleData("c", 3.0), + ) + + # Pipe through store.pipe_save — should save and pass through + results: list[SampleData] = [] + source.pipe(store.pipe_save).subscribe(results.append) + + # Data should be saved + assert store.load(1.0) == SampleData("a", 1.0) + assert store.load(2.0) == SampleData("b", 2.0) + assert store.load(3.0) == SampleData("c", 3.0) + + # Data should also pass through + assert results == [ + SampleData("a", 1.0), + SampleData("b", 2.0), + SampleData("c", 3.0), + ] + + def test_consume_stream(self, store_factory, store_name, temp_dir): + import reactivex as rx + + store = store_factory(temp_dir) + + # Create observable with test data + source = rx.of( + SampleData("a", 1.0), + SampleData("b", 2.0), + SampleData("c", 3.0), + ) + + # Consume stream — should save all items + disposable = store.consume_stream(source) + + # Data should be saved + assert store.load(1.0) == SampleData("a", 1.0) + assert store.load(2.0) == SampleData("b", 2.0) + assert store.load(3.0) == SampleData("c", 3.0) + + disposable.dispose() + + def test_iterate_items(self, store_factory, store_name, temp_dir): + store = store_factory(temp_dir) + store.save(SampleData("a", 1.0), SampleData("b", 2.0), SampleData("c", 3.0)) + + items = list(store.iterate_items()) + assert items == [ + (1.0, SampleData("a", 1.0)), + (2.0, SampleData("b", 2.0)), + (3.0, SampleData("c", 3.0)), + ] + + # With seek + items = list(store.iterate_items(seek=1.0)) + assert len(items) == 2 + assert items[0] == (2.0, SampleData("b", 2.0)) + + def test_stream_basic(self, store_factory, store_name, temp_dir): + store = store_factory(temp_dir) + store.save(SampleData("a", 1.0), SampleData("b", 2.0), SampleData("c", 3.0)) + + # Stream at high speed (essentially instant) + results: list[SampleData] = [] + store.stream(speed=1000.0).subscribe( + on_next=results.append, + on_completed=lambda: None, + ) + + # Give it a moment to complete + import time + + time.sleep(0.1) + + assert results == [ + SampleData("a", 1.0), + SampleData("b", 2.0), + SampleData("c", 3.0), + ] + + +@pytest.mark.parametrize("store_factory,store_name", testdata) +class TestCollectionAPI: + """Test new collection API methods on all backends.""" + + def test_len(self, store_factory, store_name, temp_dir): + store = store_factory(temp_dir) + assert len(store) == 0 + store.save(SampleData("a", 1.0), SampleData("b", 2.0), SampleData("c", 3.0)) + assert len(store) == 3 + + def test_iter(self, store_factory, store_name, temp_dir): + store = store_factory(temp_dir) + store.save(SampleData("a", 1.0), SampleData("b", 2.0)) + items = list(store) + assert items == [SampleData("a", 1.0), SampleData("b", 2.0)] + + def test_last_timestamp(self, store_factory, store_name, temp_dir): + store = store_factory(temp_dir) + assert store.last_timestamp() is None + store.save(SampleData("a", 1.0), SampleData("b", 2.0), SampleData("c", 3.0)) + assert store.last_timestamp() == 3.0 + + def test_last(self, store_factory, store_name, temp_dir): + store = store_factory(temp_dir) + assert store.last() is None + store.save(SampleData("a", 1.0), SampleData("b", 2.0), SampleData("c", 3.0)) + assert store.last() == SampleData("c", 3.0) + + def test_start_end_ts(self, store_factory, store_name, temp_dir): + store = store_factory(temp_dir) + assert store.start_ts is None + assert store.end_ts is None + store.save(SampleData("a", 1.0), SampleData("b", 2.0), SampleData("c", 3.0)) + assert store.start_ts == 1.0 + assert store.end_ts == 3.0 + + def test_time_range(self, store_factory, store_name, temp_dir): + store = store_factory(temp_dir) + assert store.time_range() is None + store.save(SampleData("a", 1.0), SampleData("b", 5.0)) + assert store.time_range() == (1.0, 5.0) + + def test_duration(self, store_factory, store_name, temp_dir): + store = store_factory(temp_dir) + assert store.duration() == 0.0 + store.save(SampleData("a", 1.0), SampleData("b", 5.0)) + assert store.duration() == 4.0 + + def test_find_before(self, store_factory, store_name, temp_dir): + store = store_factory(temp_dir) + store.save(SampleData("a", 1.0), SampleData("b", 2.0), SampleData("c", 3.0)) + + assert store.find_before(0.5) is None + assert store.find_before(1.0) is None # strictly before + assert store.find_before(1.5) == SampleData("a", 1.0) + assert store.find_before(2.5) == SampleData("b", 2.0) + assert store.find_before(10.0) == SampleData("c", 3.0) + + def test_find_after(self, store_factory, store_name, temp_dir): + store = store_factory(temp_dir) + store.save(SampleData("a", 1.0), SampleData("b", 2.0), SampleData("c", 3.0)) + + assert store.find_after(0.5) == SampleData("a", 1.0) + assert store.find_after(1.0) == SampleData("b", 2.0) # strictly after + assert store.find_after(2.5) == SampleData("c", 3.0) + assert store.find_after(3.0) is None # strictly after + assert store.find_after(10.0) is None + + def test_slice_by_time(self, store_factory, store_name, temp_dir): + store = store_factory(temp_dir) + store.save( + SampleData("a", 1.0), + SampleData("b", 2.0), + SampleData("c", 3.0), + SampleData("d", 4.0), + ) + + # [2.0, 4.0) should include b, c + result = store.slice_by_time(2.0, 4.0) + assert result == [SampleData("b", 2.0), SampleData("c", 3.0)] diff --git a/dimos/memory/timeseries/test_legacy.py b/dimos/memory/timeseries/test_legacy.py new file mode 100644 index 0000000000..aaad962a95 --- /dev/null +++ b/dimos/memory/timeseries/test_legacy.py @@ -0,0 +1,48 @@ +# 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. +"""Tests specific to LegacyPickleStore.""" + +from dimos.memory.timeseries.legacy import LegacyPickleStore + + +class TestLegacyPickleStoreRealData: + """Test LegacyPickleStore with real recorded data.""" + + def test_read_lidar_recording(self) -> None: + """Test reading from unitree_go2_bigoffice/lidar recording.""" + store = LegacyPickleStore("unitree_go2_bigoffice/lidar") + + # Check first timestamp exists + first_ts = store.first_timestamp() + assert first_ts is not None + assert first_ts > 0 + + # Check first data + first = store.first() + assert first is not None + assert hasattr(first, "ts") + + # Check find_closest_seek works + data_at_10s = store.find_closest_seek(10.0) + assert data_at_10s is not None + + # Check iteration returns monotonically increasing timestamps + prev_ts = None + for i, item in enumerate(store.iterate()): + assert item.ts is not None + if prev_ts is not None: + assert item.ts >= prev_ts, "Timestamps should be monotonically increasing" + prev_ts = item.ts + if i >= 10: # Only check first 10 items + break diff --git a/dimos/models/embedding/__init__.py b/dimos/models/embedding/__init__.py index a780f92246..050d35467e 100644 --- a/dimos/models/embedding/__init__.py +++ b/dimos/models/embedding/__init__.py @@ -7,24 +7,24 @@ # Optional: CLIP support try: - from dimos.models.embedding.clip import CLIPEmbedding, CLIPModel + from dimos.models.embedding.clip import CLIPModel + + __all__.append("CLIPModel") except ImportError: pass -else: - __all__ += ["CLIPEmbedding", "CLIPModel"] # Optional: MobileCLIP support try: - from dimos.models.embedding.mobileclip import MobileCLIPEmbedding, MobileCLIPModel + from dimos.models.embedding.mobileclip import MobileCLIPModel + + __all__.append("MobileCLIPModel") except ImportError: pass -else: - __all__ += ["MobileCLIPEmbedding", "MobileCLIPModel"] # Optional: TorchReID support try: - from dimos.models.embedding.treid import TorchReIDEmbedding, TorchReIDModel + from dimos.models.embedding.treid import TorchReIDModel + + __all__.append("TorchReIDModel") except ImportError: pass -else: - __all__ += ["TorchReIDEmbedding", "TorchReIDModel"] diff --git a/dimos/models/embedding/base.py b/dimos/models/embedding/base.py index eba5e45894..c6b78fcf2c 100644 --- a/dimos/models/embedding/base.py +++ b/dimos/models/embedding/base.py @@ -17,7 +17,7 @@ from abc import ABC, abstractmethod from dataclasses import dataclass import time -from typing import TYPE_CHECKING, Generic, TypeVar +from typing import TYPE_CHECKING import numpy as np import torch @@ -90,16 +90,13 @@ def to_cpu(self) -> Embedding: return self -E = TypeVar("E", bound="Embedding") - - -class EmbeddingModel(ABC, Generic[E]): +class EmbeddingModel(ABC): """Abstract base class for embedding models supporting vision and language.""" device: str @abstractmethod - def embed(self, *images: Image) -> E | list[E]: + def embed(self, *images: Image) -> Embedding | list[Embedding]: """ Embed one or more images. Returns single Embedding if one image, list if multiple. @@ -107,14 +104,14 @@ def embed(self, *images: Image) -> E | list[E]: pass @abstractmethod - def embed_text(self, *texts: str) -> E | list[E]: + def embed_text(self, *texts: str) -> Embedding | list[Embedding]: """ Embed one or more text strings. Returns single Embedding if one text, list if multiple. """ pass - def compare_one_to_many(self, query: E, candidates: list[E]) -> torch.Tensor: + def compare_one_to_many(self, query: Embedding, candidates: list[Embedding]) -> torch.Tensor: """ Efficiently compare one query against many candidates on GPU. @@ -129,7 +126,9 @@ def compare_one_to_many(self, query: E, candidates: list[E]) -> torch.Tensor: candidate_tensors = torch.stack([c.to_torch(self.device) for c in candidates]) return query_tensor @ candidate_tensors.T - def compare_many_to_many(self, queries: list[E], candidates: list[E]) -> torch.Tensor: + def compare_many_to_many( + self, queries: list[Embedding], candidates: list[Embedding] + ) -> torch.Tensor: """ Efficiently compare all queries against all candidates on GPU. @@ -144,7 +143,9 @@ def compare_many_to_many(self, queries: list[E], candidates: list[E]) -> torch.T candidate_tensors = torch.stack([c.to_torch(self.device) for c in candidates]) return query_tensors @ candidate_tensors.T - def query(self, query_emb: E, candidates: list[E], top_k: int = 5) -> list[tuple[int, float]]: + def query( + self, query_emb: Embedding, candidates: list[Embedding], top_k: int = 5 + ) -> list[tuple[int, float]]: """ Find top-k most similar candidates to query (GPU accelerated). @@ -160,6 +161,5 @@ def query(self, query_emb: E, candidates: list[E], top_k: int = 5) -> list[tuple top_values, top_indices = similarities.topk(k=min(top_k, len(candidates))) return [(idx.item(), val.item()) for idx, val in zip(top_indices, top_values, strict=False)] - def warmup(self) -> None: - """Optional warmup method to pre-load model.""" - pass + + ... diff --git a/dimos/models/embedding/clip.py b/dimos/models/embedding/clip.py index a6512568d2..1b8d3e68bb 100644 --- a/dimos/models/embedding/clip.py +++ b/dimos/models/embedding/clip.py @@ -25,16 +25,13 @@ from dimos.msgs.sensor_msgs import Image -class CLIPEmbedding(Embedding): ... - - @dataclass class CLIPModelConfig(HuggingFaceEmbeddingModelConfig): model_name: str = "openai/clip-vit-base-patch32" dtype: torch.dtype = torch.float32 -class CLIPModel(EmbeddingModel[CLIPEmbedding], HuggingFaceModel): +class CLIPModel(EmbeddingModel, HuggingFaceModel): """CLIP embedding model for vision-language re-identification.""" default_config = CLIPModelConfig @@ -50,7 +47,7 @@ def _model(self) -> HFCLIPModel: def _processor(self) -> CLIPProcessor: return CLIPProcessor.from_pretrained(self.config.model_name) - def embed(self, *images: Image) -> CLIPEmbedding | list[CLIPEmbedding]: + def embed(self, *images: Image) -> Embedding | list[Embedding]: """Embed one or more images. Returns embeddings as torch.Tensor on device for efficient GPU comparisons. @@ -67,14 +64,14 @@ def embed(self, *images: Image) -> CLIPEmbedding | list[CLIPEmbedding]: image_features = functional.normalize(image_features, dim=-1) # Create embeddings (keep as torch.Tensor on device) - embeddings = [] + embeddings: list[Embedding] = [] for i, feat in enumerate(image_features): timestamp = images[i].ts - embeddings.append(CLIPEmbedding(vector=feat, timestamp=timestamp)) + embeddings.append(Embedding(vector=feat, timestamp=timestamp)) return embeddings[0] if len(images) == 1 else embeddings - def embed_text(self, *texts: str) -> CLIPEmbedding | list[CLIPEmbedding]: + def embed_text(self, *texts: str) -> Embedding | list[Embedding]: """Embed one or more text strings. Returns embeddings as torch.Tensor on device for efficient GPU comparisons. @@ -89,9 +86,9 @@ def embed_text(self, *texts: str) -> CLIPEmbedding | list[CLIPEmbedding]: text_features = functional.normalize(text_features, dim=-1) # Create embeddings (keep as torch.Tensor on device) - embeddings = [] + embeddings: list[Embedding] = [] for feat in text_features: - embeddings.append(CLIPEmbedding(vector=feat)) + embeddings.append(Embedding(vector=feat)) return embeddings[0] if len(texts) == 1 else embeddings diff --git a/dimos/models/embedding/mobileclip.py b/dimos/models/embedding/mobileclip.py index 7c3d7adc69..c02361b367 100644 --- a/dimos/models/embedding/mobileclip.py +++ b/dimos/models/embedding/mobileclip.py @@ -27,15 +27,12 @@ from dimos.utils.data import get_data -class MobileCLIPEmbedding(Embedding): ... - - @dataclass class MobileCLIPModelConfig(EmbeddingModelConfig): model_name: str = "MobileCLIP2-S4" -class MobileCLIPModel(EmbeddingModel[MobileCLIPEmbedding], LocalModel): +class MobileCLIPModel(EmbeddingModel, LocalModel): """MobileCLIP embedding model for vision-language re-identification.""" default_config = MobileCLIPModelConfig @@ -62,7 +59,7 @@ def _preprocess(self) -> Any: def _tokenizer(self) -> Any: return open_clip.get_tokenizer(self.config.model_name) - def embed(self, *images: Image) -> MobileCLIPEmbedding | list[MobileCLIPEmbedding]: + def embed(self, *images: Image) -> Embedding | list[Embedding]: """Embed one or more images. Returns embeddings as torch.Tensor on device for efficient GPU comparisons. @@ -83,11 +80,11 @@ def embed(self, *images: Image) -> MobileCLIPEmbedding | list[MobileCLIPEmbeddin embeddings = [] for i, feat in enumerate(feats): timestamp = images[i].ts - embeddings.append(MobileCLIPEmbedding(vector=feat, timestamp=timestamp)) + embeddings.append(Embedding(vector=feat, timestamp=timestamp)) return embeddings[0] if len(images) == 1 else embeddings - def embed_text(self, *texts: str) -> MobileCLIPEmbedding | list[MobileCLIPEmbedding]: + def embed_text(self, *texts: str) -> Embedding | list[Embedding]: """Embed one or more text strings. Returns embeddings as torch.Tensor on device for efficient GPU comparisons. @@ -101,7 +98,7 @@ def embed_text(self, *texts: str) -> MobileCLIPEmbedding | list[MobileCLIPEmbedd # Create embeddings (keep as torch.Tensor on device) embeddings = [] for feat in feats: - embeddings.append(MobileCLIPEmbedding(vector=feat)) + embeddings.append(Embedding(vector=feat)) return embeddings[0] if len(texts) == 1 else embeddings diff --git a/dimos/models/embedding/treid.py b/dimos/models/embedding/treid.py index 23da1effe2..85e32cd39b 100644 --- a/dimos/models/embedding/treid.py +++ b/dimos/models/embedding/treid.py @@ -13,13 +13,14 @@ # limitations under the License. import warnings + +warnings.filterwarnings("ignore", message="Cython evaluation.*unavailable", category=UserWarning) + from dataclasses import dataclass from functools import cached_property import torch import torch.nn.functional as functional - -warnings.filterwarnings("ignore", message="Cython evaluation.*unavailable", category=UserWarning) from torchreid import utils as torchreid_utils from dimos.models.base import LocalModel @@ -28,9 +29,6 @@ from dimos.utils.data import get_data -class TorchReIDEmbedding(Embedding): ... - - # osnet models downloaded from https://kaiyangzhou.github.io/deep-person-reid/MODEL_ZOO.html # into dimos/data/models_torchreid/ # feel free to add more @@ -39,7 +37,7 @@ class TorchReIDModelConfig(EmbeddingModelConfig): model_name: str = "osnet_x1_0" -class TorchReIDModel(EmbeddingModel[TorchReIDEmbedding], LocalModel): +class TorchReIDModel(EmbeddingModel, LocalModel): """TorchReID embedding model for person re-identification.""" default_config = TorchReIDModelConfig @@ -54,7 +52,7 @@ def _model(self) -> torchreid_utils.FeatureExtractor: device=self.config.device, ) - def embed(self, *images: Image) -> TorchReIDEmbedding | list[TorchReIDEmbedding]: + def embed(self, *images: Image) -> Embedding | list[Embedding]: """Embed one or more images. Returns embeddings as torch.Tensor on device for efficient GPU comparisons. @@ -79,11 +77,11 @@ def embed(self, *images: Image) -> TorchReIDEmbedding | list[TorchReIDEmbedding] embeddings = [] for i, feat in enumerate(features_tensor): timestamp = images[i].ts - embeddings.append(TorchReIDEmbedding(vector=feat, timestamp=timestamp)) + embeddings.append(Embedding(vector=feat, timestamp=timestamp)) return embeddings[0] if len(images) == 1 else embeddings - def embed_text(self, *texts: str) -> TorchReIDEmbedding | list[TorchReIDEmbedding]: + def embed_text(self, *texts: str) -> Embedding | list[Embedding]: """Text embedding not supported for ReID models. TorchReID models are vision-only person re-identification models diff --git a/dimos/msgs/geometry_msgs/PoseWithCovariance.py b/dimos/msgs/geometry_msgs/PoseWithCovariance.py index 7e856da6dc..03ce7fd081 100644 --- a/dimos/msgs/geometry_msgs/PoseWithCovariance.py +++ b/dimos/msgs/geometry_msgs/PoseWithCovariance.py @@ -14,7 +14,7 @@ from __future__ import annotations -from typing import TYPE_CHECKING, TypeAlias +from typing import TYPE_CHECKING, Any, TypeAlias from dimos_lcm.geometry_msgs import ( PoseWithCovariance as LCMPoseWithCovariance, @@ -38,6 +38,7 @@ class PoseWithCovariance(LCMPoseWithCovariance): # type: ignore[misc] pose: Pose + covariance: np.ndarray[tuple[int], np.dtype[np.floating[Any]]] msg_name = "geometry_msgs.PoseWithCovariance" @dispatch diff --git a/dimos/msgs/geometry_msgs/Transform.py b/dimos/msgs/geometry_msgs/Transform.py index 58bab1ec4a..5f50f9b9d1 100644 --- a/dimos/msgs/geometry_msgs/Transform.py +++ b/dimos/msgs/geometry_msgs/Transform.py @@ -20,6 +20,8 @@ if TYPE_CHECKING: import rerun as rr + from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped + from dimos_lcm.geometry_msgs import ( Transform as LCMTransform, TransformStamped as LCMTransformStamped, @@ -191,7 +193,7 @@ def from_pose(cls, frame_id: str, pose: Pose | PoseStamped) -> Transform: # typ else: raise TypeError(f"Expected Pose or PoseStamped, got {type(pose).__name__}") - def to_pose(self, **kwargs) -> PoseStamped: # type: ignore[name-defined, no-untyped-def] + def to_pose(self, **kwargs: object) -> PoseStamped: """Create a Transform from a Pose or PoseStamped. Args: @@ -201,10 +203,10 @@ def to_pose(self, **kwargs) -> PoseStamped: # type: ignore[name-defined, no-unt A Transform with the same translation and rotation as the pose """ # Import locally to avoid circular imports - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped + from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped as _PoseStamped # Handle both Pose and PoseStamped - return PoseStamped( + result: PoseStamped = _PoseStamped( **{ "position": self.translation, "orientation": self.rotation, @@ -212,6 +214,7 @@ def to_pose(self, **kwargs) -> PoseStamped: # type: ignore[name-defined, no-unt }, **kwargs, ) + return result def to_matrix(self) -> np.ndarray: # type: ignore[name-defined] """Convert Transform to a 4x4 transformation matrix. diff --git a/dimos/msgs/geometry_msgs/TwistWithCovariance.py b/dimos/msgs/geometry_msgs/TwistWithCovariance.py index 09f234ea84..90ddb94d7c 100644 --- a/dimos/msgs/geometry_msgs/TwistWithCovariance.py +++ b/dimos/msgs/geometry_msgs/TwistWithCovariance.py @@ -14,7 +14,7 @@ from __future__ import annotations -from typing import TypeAlias +from typing import Any, TypeAlias from dimos_lcm.geometry_msgs import ( TwistWithCovariance as LCMTwistWithCovariance, @@ -35,6 +35,7 @@ class TwistWithCovariance(LCMTwistWithCovariance): # type: ignore[misc] twist: Twist + covariance: np.ndarray[tuple[int], np.dtype[np.floating[Any]]] msg_name = "geometry_msgs.TwistWithCovariance" @dispatch diff --git a/dimos/msgs/nav_msgs/Odometry.py b/dimos/msgs/nav_msgs/Odometry.py index 7bcad24e93..a958f8dba0 100644 --- a/dimos/msgs/nav_msgs/Odometry.py +++ b/dimos/msgs/nav_msgs/Odometry.py @@ -15,14 +15,13 @@ from __future__ import annotations import time -from typing import TYPE_CHECKING, TypeAlias +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 -from plum import dispatch from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.geometry_msgs.PoseWithCovariance import PoseWithCovariance @@ -31,23 +30,15 @@ from dimos.types.timestamped import Timestamped if TYPE_CHECKING: + from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Vector3 import Vector3 -# Types that can be converted to/from Odometry -OdometryConvertable: TypeAlias = ( - LCMOdometry | dict[str, float | str | PoseWithCovariance | TwistWithCovariance | Pose | Twist] -) +class Odometry(Timestamped): + """Odometry message with pose, twist, and frame information.""" -class Odometry(LCMOdometry, Timestamped): # type: ignore[misc] - pose: PoseWithCovariance - twist: TwistWithCovariance msg_name = "nav_msgs.Odometry" - ts: float - frame_id: str - child_frame_id: str - @dispatch def __init__( self, ts: float = 0.0, @@ -56,250 +47,113 @@ def __init__( pose: PoseWithCovariance | Pose | None = None, twist: TwistWithCovariance | Twist | None = None, ) -> None: - """Initialize with timestamp, frame IDs, pose and twist. - - Args: - ts: Timestamp in seconds (defaults to current time if 0) - frame_id: Reference frame ID (e.g., "odom", "map") - child_frame_id: Child frame ID (e.g., "base_link", "base_footprint") - pose: Pose with covariance (or just Pose, covariance will be zero) - twist: Twist with covariance (or just Twist, covariance will be zero) - """ self.ts = ts if ts != 0 else time.time() self.frame_id = frame_id self.child_frame_id = child_frame_id - # Handle pose if pose is None: self.pose = PoseWithCovariance() - elif isinstance(pose, PoseWithCovariance): - self.pose = pose elif isinstance(pose, Pose): self.pose = PoseWithCovariance(pose) else: - self.pose = PoseWithCovariance(Pose(pose)) - - # Handle twist - if twist is None: - self.twist = TwistWithCovariance() - elif isinstance(twist, TwistWithCovariance): - self.twist = twist - elif isinstance(twist, Twist): - self.twist = TwistWithCovariance(twist) - else: - self.twist = TwistWithCovariance(Twist(twist)) - - @dispatch # type: ignore[no-redef] - def __init__(self, odometry: Odometry) -> None: - """Initialize from another Odometry (copy constructor).""" - self.ts = odometry.ts - self.frame_id = odometry.frame_id - self.child_frame_id = odometry.child_frame_id - self.pose = PoseWithCovariance(odometry.pose) - self.twist = TwistWithCovariance(odometry.twist) - - @dispatch # type: ignore[no-redef] - def __init__(self, lcm_odometry: LCMOdometry) -> None: - """Initialize from an LCM Odometry.""" - self.ts = lcm_odometry.header.stamp.sec + (lcm_odometry.header.stamp.nsec / 1_000_000_000) - self.frame_id = lcm_odometry.header.frame_id - self.child_frame_id = lcm_odometry.child_frame_id - self.pose = PoseWithCovariance(lcm_odometry.pose) - self.twist = TwistWithCovariance(lcm_odometry.twist) - - @dispatch # type: ignore[no-redef] - def __init__( - self, - odometry_dict: dict[ - str, float | str | PoseWithCovariance | TwistWithCovariance | Pose | Twist - ], - ) -> None: - """Initialize from a dictionary.""" - self.ts = odometry_dict.get("ts", odometry_dict.get("timestamp", time.time())) - self.frame_id = odometry_dict.get("frame_id", "") - self.child_frame_id = odometry_dict.get("child_frame_id", "") - - # Handle pose - pose = odometry_dict.get("pose") - if pose is None: - self.pose = PoseWithCovariance() - elif isinstance(pose, PoseWithCovariance): self.pose = pose - elif isinstance(pose, Pose): - self.pose = PoseWithCovariance(pose) - else: - self.pose = PoseWithCovariance(Pose(pose)) - # Handle twist - twist = odometry_dict.get("twist") if twist is None: self.twist = TwistWithCovariance() - elif isinstance(twist, TwistWithCovariance): - self.twist = twist elif isinstance(twist, Twist): self.twist = TwistWithCovariance(twist) else: - self.twist = TwistWithCovariance(Twist(twist)) + self.twist = twist + + # -- Convenience properties -- @property def position(self) -> Vector3: - """Get position from pose.""" return self.pose.position @property - def orientation(self): # type: ignore[no-untyped-def] - """Get orientation from pose.""" + def orientation(self) -> Quaternion: return self.pose.orientation @property def linear_velocity(self) -> Vector3: - """Get linear velocity from twist.""" return self.twist.linear @property def angular_velocity(self) -> Vector3: - """Get angular velocity from twist.""" return self.twist.angular @property def x(self) -> float: - """X position.""" return self.pose.x @property def y(self) -> float: - """Y position.""" return self.pose.y @property def z(self) -> float: - """Z position.""" return self.pose.z @property def vx(self) -> float: - """Linear velocity in X.""" return self.twist.linear.x @property def vy(self) -> float: - """Linear velocity in Y.""" return self.twist.linear.y @property def vz(self) -> float: - """Linear velocity in Z.""" return self.twist.linear.z @property def wx(self) -> float: - """Angular velocity around X (roll rate).""" return self.twist.angular.x @property def wy(self) -> float: - """Angular velocity around Y (pitch rate).""" return self.twist.angular.y @property def wz(self) -> float: - """Angular velocity around Z (yaw rate).""" return self.twist.angular.z @property def roll(self) -> float: - """Roll angle in radians.""" return self.pose.roll @property def pitch(self) -> float: - """Pitch angle in radians.""" return self.pose.pitch @property def yaw(self) -> float: - """Yaw angle in radians.""" return self.pose.yaw - def __repr__(self) -> str: - return ( - f"Odometry(ts={self.ts:.6f}, frame_id='{self.frame_id}', " - f"child_frame_id='{self.child_frame_id}', pose={self.pose!r}, twist={self.twist!r})" - ) - - def __str__(self) -> str: - return ( - f"Odometry:\n" - f" Timestamp: {self.ts:.6f}\n" - f" Frame: {self.frame_id} -> {self.child_frame_id}\n" - f" Position: [{self.x:.3f}, {self.y:.3f}, {self.z:.3f}]\n" - f" Orientation: [roll={self.roll:.3f}, pitch={self.pitch:.3f}, yaw={self.yaw:.3f}]\n" - 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 __eq__(self, other) -> bool: # type: ignore[no-untyped-def] - """Check if two Odometry messages are equal.""" - if not isinstance(other, Odometry): - return False - return ( - abs(self.ts - other.ts) < 1e-6 - and self.frame_id == other.frame_id - and self.child_frame_id == other.child_frame_id - and self.pose == other.pose - and self.twist == other.twist - ) - - def to_rerun(self) -> Archetype: - """Convert to rerun Transform3D for visualizing the pose.""" - import rerun as rr - - return rr.Transform3D( - translation=[self.x, self.y, self.z], - rotation=rr.Quaternion( - xyzw=[ - self.orientation.x, - self.orientation.y, - self.orientation.z, - self.orientation.w, - ] - ), - ) + # -- Serialization -- def lcm_encode(self) -> bytes: - """Encode to LCM binary format.""" lcm_msg = LCMOdometry() - # Set header - [lcm_msg.header.stamp.sec, lcm_msg.header.stamp.nsec] = self.ros_timestamp() + lcm_msg.header.stamp.sec, lcm_msg.header.stamp.nsec = self.ros_timestamp() lcm_msg.header.frame_id = self.frame_id lcm_msg.child_frame_id = self.child_frame_id - # Set pose with covariance lcm_msg.pose.pose = self.pose.pose - if isinstance(self.pose.covariance, np.ndarray): # type: ignore[has-type] - lcm_msg.pose.covariance = self.pose.covariance.tolist() # type: ignore[has-type] - else: - lcm_msg.pose.covariance = list(self.pose.covariance) # type: ignore[has-type] + lcm_msg.pose.covariance = list(np.asarray(self.pose.covariance)) - # Set twist with covariance lcm_msg.twist.twist = self.twist.twist - if isinstance(self.twist.covariance, np.ndarray): # type: ignore[has-type] - lcm_msg.twist.covariance = self.twist.covariance.tolist() # type: ignore[has-type] - else: - lcm_msg.twist.covariance = list(self.twist.covariance) # type: ignore[has-type] + lcm_msg.twist.covariance = list(np.asarray(self.twist.covariance)) return lcm_msg.lcm_encode() # type: ignore[no-any-return] @classmethod def lcm_decode(cls, data: bytes) -> Odometry: - """Decode from LCM binary format.""" lcm_msg = LCMOdometry.lcm_decode(data) - # Extract timestamp ts = lcm_msg.header.stamp.sec + (lcm_msg.header.stamp.nsec / 1_000_000_000) - # Create pose with covariance pose = Pose( position=[ lcm_msg.pose.pose.position.x, @@ -313,9 +167,6 @@ def lcm_decode(cls, data: bytes) -> Odometry: lcm_msg.pose.pose.orientation.w, ], ) - pose_with_cov = PoseWithCovariance(pose, lcm_msg.pose.covariance) - - # Create twist with covariance twist = Twist( linear=[ lcm_msg.twist.twist.linear.x, @@ -328,12 +179,57 @@ def lcm_decode(cls, data: bytes) -> Odometry: lcm_msg.twist.twist.angular.z, ], ) - twist_with_cov = TwistWithCovariance(twist, lcm_msg.twist.covariance) return cls( ts=ts, frame_id=lcm_msg.header.frame_id, child_frame_id=lcm_msg.child_frame_id, - pose=pose_with_cov, - twist=twist_with_cov, + pose=PoseWithCovariance(pose, lcm_msg.pose.covariance), + twist=TwistWithCovariance(twist, lcm_msg.twist.covariance), + ) + + # -- Comparison / display -- + + def __eq__(self, other: object) -> bool: + if not isinstance(other, Odometry): + return False + return ( + abs(self.ts - other.ts) < 1e-6 + and self.frame_id == other.frame_id + and self.child_frame_id == other.child_frame_id + and self.pose == other.pose + and self.twist == other.twist + ) + + def __repr__(self) -> str: + return ( + f"Odometry(ts={self.ts:.6f}, frame_id='{self.frame_id}', " + f"child_frame_id='{self.child_frame_id}', pose={self.pose!r}, twist={self.twist!r})" + ) + + def __str__(self) -> str: + return ( + f"Odometry:\n" + f" Timestamp: {self.ts:.6f}\n" + f" Frame: {self.frame_id} -> {self.child_frame_id}\n" + f" Position: [{self.x:.3f}, {self.y:.3f}, {self.z:.3f}]\n" + f" Orientation: [roll={self.roll:.3f}, pitch={self.pitch:.3f}, yaw={self.yaw:.3f}]\n" + 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/nav_msgs/test_Odometry.py b/dimos/msgs/nav_msgs/test_Odometry.py index e6eadc51f8..c532aed1ca 100644 --- a/dimos/msgs/nav_msgs/test_Odometry.py +++ b/dimos/msgs/nav_msgs/test_Odometry.py @@ -25,48 +25,34 @@ def test_odometry_default_init() -> None: - """Test default initialization.""" odom = Odometry() - # Should have current timestamp assert odom.ts > 0 assert odom.frame_id == "" assert odom.child_frame_id == "" - # Pose should be at origin with identity orientation assert odom.pose.position.x == 0.0 assert odom.pose.position.y == 0.0 assert odom.pose.position.z == 0.0 assert odom.pose.orientation.w == 1.0 - # Twist should be zero assert odom.twist.linear.x == 0.0 - assert odom.twist.linear.y == 0.0 - assert odom.twist.linear.z == 0.0 assert odom.twist.angular.x == 0.0 - assert odom.twist.angular.y == 0.0 - assert odom.twist.angular.z == 0.0 - # Covariances should be zero assert np.all(odom.pose.covariance == 0.0) assert np.all(odom.twist.covariance == 0.0) def test_odometry_with_frames() -> None: - """Test initialization with frame IDs.""" ts = 1234567890.123456 - frame_id = "odom" - child_frame_id = "base_link" - - odom = Odometry(ts=ts, frame_id=frame_id, child_frame_id=child_frame_id) + odom = Odometry(ts=ts, frame_id="odom", child_frame_id="base_link") assert odom.ts == ts - assert odom.frame_id == frame_id - assert odom.child_frame_id == child_frame_id + assert odom.frame_id == "odom" + assert odom.child_frame_id == "base_link" def test_odometry_with_pose_and_twist() -> None: - """Test initialization with pose and twist.""" pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) twist = Twist(Vector3(0.5, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)) @@ -80,7 +66,6 @@ def test_odometry_with_pose_and_twist() -> None: def test_odometry_with_covariances() -> None: - """Test initialization with pose and twist with covariances.""" pose = Pose(1.0, 2.0, 3.0) pose_cov = np.arange(36, dtype=float) pose_with_cov = PoseWithCovariance(pose, pose_cov) @@ -103,88 +88,35 @@ def test_odometry_with_covariances() -> None: assert np.array_equal(odom.twist.covariance, twist_cov) -def test_odometry_copy_constructor() -> None: - """Test copy constructor.""" - original = Odometry( - ts=1000.0, - frame_id="odom", - child_frame_id="base_link", - pose=Pose(1.0, 2.0, 3.0), - twist=Twist(Vector3(0.5, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), - ) - - copy = Odometry(original) - - assert copy == original - assert copy is not original - assert copy.pose is not original.pose - assert copy.twist is not original.twist - - -def test_odometry_dict_init() -> None: - """Test initialization from dictionary.""" - odom_dict = { - "ts": 1000.0, - "frame_id": "odom", - "child_frame_id": "base_link", - "pose": Pose(1.0, 2.0, 3.0), - "twist": Twist(Vector3(0.5, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), - } - - odom = Odometry(odom_dict) - - assert odom.ts == 1000.0 - assert odom.frame_id == "odom" - assert odom.child_frame_id == "base_link" - assert odom.pose.position.x == 1.0 - assert odom.twist.linear.x == 0.5 - - def test_odometry_properties() -> None: - """Test convenience properties.""" pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) twist = Twist(Vector3(0.5, 0.6, 0.7), Vector3(0.1, 0.2, 0.3)) odom = Odometry(ts=1000.0, frame_id="odom", child_frame_id="base_link", pose=pose, twist=twist) - # Position properties assert odom.x == 1.0 assert odom.y == 2.0 assert odom.z == 3.0 assert odom.position.x == 1.0 - assert odom.position.y == 2.0 - assert odom.position.z == 3.0 - # Orientation properties assert odom.orientation.x == 0.1 - assert odom.orientation.y == 0.2 - assert odom.orientation.z == 0.3 - assert odom.orientation.w == 0.9 - # Velocity properties assert odom.vx == 0.5 assert odom.vy == 0.6 assert odom.vz == 0.7 assert odom.linear_velocity.x == 0.5 - assert odom.linear_velocity.y == 0.6 - assert odom.linear_velocity.z == 0.7 - # Angular velocity properties assert odom.wx == 0.1 assert odom.wy == 0.2 assert odom.wz == 0.3 assert odom.angular_velocity.x == 0.1 - assert odom.angular_velocity.y == 0.2 - assert odom.angular_velocity.z == 0.3 - # Euler angles assert odom.roll == pose.roll assert odom.pitch == pose.pitch assert odom.yaw == pose.yaw def test_odometry_str_repr() -> None: - """Test string representations.""" odom = Odometry( ts=1234567890.123456, frame_id="odom", @@ -193,22 +125,16 @@ def test_odometry_str_repr() -> None: twist=Twist(Vector3(0.5, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), ) - repr_str = repr(odom) - assert "Odometry" in repr_str - assert "1234567890.123456" in repr_str - assert "odom" in repr_str - assert "base_link" in repr_str + assert "Odometry" in repr(odom) + assert "1234567890.123456" in repr(odom) - str_repr = str(odom) - assert "Odometry" in str_repr - assert "odom -> base_link" in str_repr - assert "1.234" in str_repr - assert "0.500" in str_repr + s = str(odom) + assert "odom -> base_link" in s + assert "1.234" in s def test_odometry_equality() -> None: - """Test equality comparison.""" - odom1 = Odometry( + kwargs = dict( ts=1000.0, frame_id="odom", child_frame_id="base_link", @@ -216,29 +142,12 @@ def test_odometry_equality() -> None: twist=Twist(Vector3(0.5, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), ) - odom2 = Odometry( - ts=1000.0, - frame_id="odom", - child_frame_id="base_link", - pose=Pose(1.0, 2.0, 3.0), - twist=Twist(Vector3(0.5, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), - ) - - odom3 = Odometry( - ts=1000.0, - frame_id="odom", - child_frame_id="base_link", - pose=Pose(1.1, 2.0, 3.0), # Different position - twist=Twist(Vector3(0.5, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), - ) + assert Odometry(**kwargs) == Odometry(**kwargs) + assert Odometry(**kwargs) != Odometry(**{**kwargs, "pose": Pose(1.1, 2.0, 3.0)}) + assert Odometry(**kwargs) != "not an odometry" - assert odom1 == odom2 - assert odom1 != odom3 - assert odom1 != "not an odometry" - -def test_odometry_lcm_encode_decode() -> None: - """Test LCM encoding and decoding.""" +def test_odometry_lcm_roundtrip() -> None: pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) pose_cov = np.arange(36, dtype=float) twist = Twist(Vector3(0.5, 0.6, 0.7), Vector3(0.1, 0.2, 0.3)) @@ -252,11 +161,8 @@ def test_odometry_lcm_encode_decode() -> None: twist=TwistWithCovariance(twist, twist_cov), ) - # Encode and decode - binary_msg = source.lcm_encode() - decoded = Odometry.lcm_decode(binary_msg) + decoded = Odometry.lcm_decode(source.lcm_encode()) - # Check values (allowing for timestamp precision loss) assert abs(decoded.ts - source.ts) < 1e-6 assert decoded.frame_id == source.frame_id assert decoded.child_frame_id == source.child_frame_id @@ -265,56 +171,38 @@ def test_odometry_lcm_encode_decode() -> None: def test_odometry_zero_timestamp() -> None: - """Test that zero timestamp gets replaced with current time.""" odom = Odometry(ts=0.0) - - # Should have been replaced with current time assert odom.ts > 0 assert odom.ts <= time.time() def test_odometry_with_just_pose() -> None: - """Test initialization with just a Pose (no covariance).""" - pose = Pose(1.0, 2.0, 3.0) - - odom = Odometry(pose=pose) + odom = Odometry(pose=Pose(1.0, 2.0, 3.0)) assert odom.pose.position.x == 1.0 - assert odom.pose.position.y == 2.0 - assert odom.pose.position.z == 3.0 - assert np.all(odom.pose.covariance == 0.0) # Should have zero covariance - assert np.all(odom.twist.covariance == 0.0) # Twist should also be zero + assert np.all(odom.pose.covariance == 0.0) + assert np.all(odom.twist.covariance == 0.0) def test_odometry_with_just_twist() -> None: - """Test initialization with just a Twist (no covariance).""" - twist = Twist(Vector3(0.5, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)) - - odom = Odometry(twist=twist) + odom = Odometry(twist=Twist(Vector3(0.5, 0.0, 0.0), Vector3(0.0, 0.0, 0.1))) assert odom.twist.linear.x == 0.5 assert odom.twist.angular.z == 0.1 - assert np.all(odom.twist.covariance == 0.0) # Should have zero covariance - assert np.all(odom.pose.covariance == 0.0) # Pose should also be zero + assert np.all(odom.twist.covariance == 0.0) def test_odometry_typical_robot_scenario() -> None: - """Test a typical robot odometry scenario.""" - # Robot moving forward at 0.5 m/s with slight rotation odom = Odometry( ts=1000.0, frame_id="odom", child_frame_id="base_footprint", - pose=Pose(10.0, 5.0, 0.0, 0.0, 0.0, np.sin(0.1), np.cos(0.1)), # 0.2 rad yaw - twist=Twist( - Vector3(0.5, 0.0, 0.0), Vector3(0.0, 0.0, 0.05) - ), # Moving forward, turning slightly + pose=Pose(10.0, 5.0, 0.0, 0.0, 0.0, np.sin(0.1), np.cos(0.1)), + twist=Twist(Vector3(0.5, 0.0, 0.0), Vector3(0.0, 0.0, 0.05)), ) - # Check we can access all the typical properties assert odom.x == 10.0 assert odom.y == 5.0 - assert odom.z == 0.0 - assert abs(odom.yaw - 0.2) < 0.01 # Approximately 0.2 radians - assert odom.vx == 0.5 # Forward velocity - assert odom.wz == 0.05 # Yaw rate + assert abs(odom.yaw - 0.2) < 0.01 + assert odom.vx == 0.5 + assert odom.wz == 0.05 diff --git a/dimos/msgs/sensor_msgs/Image.py b/dimos/msgs/sensor_msgs/Image.py index eb12b64c0a..66c2876b62 100644 --- a/dimos/msgs/sensor_msgs/Image.py +++ b/dimos/msgs/sensor_msgs/Image.py @@ -33,6 +33,7 @@ from dimos.utils.reactive import quality_barrier if TYPE_CHECKING: + from collections.abc import Callable import os from reactivex.observable import Observable @@ -592,9 +593,9 @@ def sharpness_window(target_frequency: float, source: Observable[Image]) -> Obse thread_scheduler = ThreadPoolScheduler(max_workers=1) def find_best(*_args: Any) -> Image | None: - if not window._items: + if len(window) == 0: return None - return max(window._items, key=lambda img: img.sharpness) # type: ignore[no-any-return] + return max(window, key=lambda img: img.sharpness) # type: ignore[no-any-return] return rx.interval(1.0 / target_frequency).pipe( # type: ignore[misc] ops.observe_on(thread_scheduler), @@ -603,7 +604,7 @@ def find_best(*_args: Any) -> Image | None: ) -def sharpness_barrier(target_frequency: float) -> Any: +def sharpness_barrier(target_frequency: float) -> Callable[[Observable[Image]], Observable[Image]]: """Select the sharpest Image within each time window.""" if target_frequency <= 0: raise ValueError("target_frequency must be positive") diff --git a/dimos/navigation/rosnav.py b/dimos/navigation/rosnav.py index 9f40a60198..8efabaebee 100644 --- a/dimos/navigation/rosnav.py +++ b/dimos/navigation/rosnav.py @@ -18,7 +18,6 @@ Encapsulates ROS transport and topic remapping for Unitree robots. """ -from collections.abc import Generator from dataclasses import dataclass, field import logging import threading @@ -28,7 +27,7 @@ from reactivex.subject import Subject from dimos import spec -from dimos.agents import Reducer, Stream, skill # type: ignore[attr-defined] +from dimos.agents.annotation import skill from dimos.core import DimosCluster, In, LCMTransport, Module, Out, rpc from dimos.core._dask_exports import DimosCluster from dimos.core.core import rpc @@ -48,8 +47,6 @@ from dimos.msgs.std_msgs import Bool, Int8 from dimos.msgs.tf2_msgs.TFMessage import TFMessage from dimos.navigation.base import NavigationInterface, NavigationState -from dimos.protocol.skill.skill import skill -from dimos.protocol.skill.type import Reducer, Stream from dimos.utils.logging_config import setup_logger from dimos.utils.transform_utils import euler_to_quaternion @@ -211,21 +208,8 @@ def _set_autonomy_mode(self) -> None: self.ros_joy.publish(joy_msg) logger.info("Setting autonomy mode via Joy message") - @skill(stream=Stream.passive, reducer=Reducer.latest) # type: ignore[arg-type] - def current_position(self): # type: ignore[no-untyped-def] - """passively stream the current position of the robot every second""" - if self._current_position_running: - return "already running" - while True: - self._current_position_running = True - time.sleep(1.0) - tf = self.tf.get("map", "base_link") - if not tf: - continue - yield f"current position {tf.translation.x}, {tf.translation.y}" - - @skill(stream=Stream.call_agent, reducer=Reducer.string) # type: ignore[untyped-decorator] - def goto(self, x: float, y: float): # type: ignore[no-untyped-def] + @skill + def goto(self, x: float, y: float) -> str: """ move the robot in relative coordinates x is forward, y is left @@ -239,12 +223,11 @@ def goto(self, x: float, y: float): # type: ignore[no-untyped-def] ts=time.time(), ) - yield "moving, please wait..." self.navigate_to(pose_to) - yield "arrived" + return "arrived" - @skill(stream=Stream.call_agent, reducer=Reducer.string) # type: ignore[untyped-decorator] - def goto_global(self, x: float, y: float) -> Generator[str, None, None]: + @skill + def goto_global(self, x: float, y: float) -> str: """ go to coordinates x,y in the map frame 0,0 is your starting position @@ -256,13 +239,9 @@ def goto_global(self, x: float, y: float) -> Generator[str, None, None]: orientation=Quaternion(0.0, 0.0, 0.0, 0.0), ) - pos = self.tf.get("base_link", "map").translation - - yield f"moving from {pos.x:.2f}, {pos.y:.2f} to {x:.2f}, {y:.2f}, please wait..." - self.navigate_to(target) - yield "arrived to {x:.2f}, {y:.2f}" + return f"arrived to {x:.2f}, {y:.2f}" @rpc def navigate_to(self, pose: PoseStamped, timeout: float = 60.0) -> bool: diff --git a/dimos/perception/demo_object_scene_registration.py b/dimos/perception/demo_object_scene_registration.py index d1d879d0ab..c02f7d2984 100644 --- a/dimos/perception/demo_object_scene_registration.py +++ b/dimos/perception/demo_object_scene_registration.py @@ -13,8 +13,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from dimos.agents.agent import llm_agent -from dimos.agents.cli.human import human_input +from dimos.agents.agent import agent from dimos.core.blueprints import autoconnect from dimos.hardware.sensors.camera.realsense import realsense_camera from dimos.hardware.sensors.camera.zed import zed_camera @@ -35,6 +34,5 @@ camera_module, object_scene_registration_module(target_frame="world", prompt_mode=YoloePromptMode.LRPC), foxglove_bridge(), - human_input(), - llm_agent(), + agent(), ).global_config(viewer_backend="foxglove") diff --git a/dimos/perception/detection/module3D.py b/dimos/perception/detection/module3D.py index 9fe5bd9c6c..d275fbc85f 100644 --- a/dimos/perception/detection/module3D.py +++ b/dimos/perception/detection/module3D.py @@ -23,6 +23,7 @@ from reactivex.observable import Observable from dimos import spec +from dimos.agents.annotation import skill from dimos.core.core import rpc from dimos.core.stream import In, Out from dimos.msgs.geometry_msgs import PoseStamped, Quaternion, Transform, Vector3 @@ -32,7 +33,6 @@ from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D from dimos.perception.detection.type.detection3d import Detection3DPC from dimos.perception.detection.type.detection3d.imageDetections3DPC import ImageDetections3DPC -from dimos.protocol.skill.skill import skill from dimos.types.timestamped import align_timestamped from dimos.utils.reactive import backpressure @@ -111,7 +111,7 @@ def pixel_to_3d( # Camera optical frame: X right, Y down, Z forward return Vector3(x_norm * assumed_depth, y_norm * assumed_depth, assumed_depth) - @skill() + @skill def ask_vlm(self, question: str) -> str: """asks a visual model about the view of the robot, for example is the bannana in the trunk? diff --git a/dimos/perception/detection/reid/embedding_id_system.py b/dimos/perception/detection/reid/embedding_id_system.py index 9b57e1eb6c..15bb491f5c 100644 --- a/dimos/perception/detection/reid/embedding_id_system.py +++ b/dimos/perception/detection/reid/embedding_id_system.py @@ -33,7 +33,7 @@ class EmbeddingIDSystem(IDSystem): def __init__( self, - model: Callable[[], EmbeddingModel[Embedding]], + model: Callable[[], EmbeddingModel], padding: int = 0, similarity_threshold: float = 0.63, comparison_mode: Literal["max", "mean", "top_k_mean"] = "top_k_mean", diff --git a/dimos/perception/experimental/temporal_memory/temporal_memory.py b/dimos/perception/experimental/temporal_memory/temporal_memory.py index b884b0886f..66b6fce911 100644 --- a/dimos/perception/experimental/temporal_memory/temporal_memory.py +++ b/dimos/perception/experimental/temporal_memory/temporal_memory.py @@ -32,14 +32,13 @@ from reactivex import Subject, interval from reactivex.disposable import Disposable +from dimos.agents.annotation import skill from dimos.core.core import rpc -from dimos.core.module import ModuleConfig -from dimos.core.skill_module import SkillModule +from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In from dimos.models.vl.base import VlModel from dimos.msgs.sensor_msgs import Image from dimos.msgs.sensor_msgs.Image import sharpness_barrier -from dimos.protocol.skill.skill import skill from . import temporal_utils as tu from .clip_filter import ( @@ -101,7 +100,7 @@ class TemporalMemoryConfig(ModuleConfig): nearby_distance_meters: float = 5.0 # "Nearby" threshold -class TemporalMemory(SkillModule): +class TemporalMemory(Module): """ builds temporal understanding of video streams using vlms. @@ -450,7 +449,7 @@ def _update_rolling_summary(self, w_end: float) -> None: except Exception as e: logger.error(f"summary update failed: {e}", exc_info=True) - @skill() + @skill def query(self, question: str) -> str: """Answer a question about the video stream using temporal memory and graph knowledge. diff --git a/dimos/perception/object_scene_registration.py b/dimos/perception/object_scene_registration.py index 85cdcc4c8f..cfc4ab8d3e 100644 --- a/dimos/perception/object_scene_registration.py +++ b/dimos/perception/object_scene_registration.py @@ -20,8 +20,9 @@ from numpy.typing import NDArray import open3d as o3d # type: ignore[import-untyped] +from dimos.agents.annotation import skill from dimos.core import In, Out, rpc -from dimos.core.skill_module import SkillModule +from dimos.core.module import Module from dimos.msgs.foxglove_msgs import ImageAnnotations from dimos.msgs.sensor_msgs import CameraInfo, Image, PointCloud2 from dimos.msgs.sensor_msgs.Image import ImageFormat @@ -36,7 +37,6 @@ aggregate_pointclouds, to_detection3d_array, ) -from dimos.protocol.skill.skill import skill from dimos.types.timestamped import align_timestamped from dimos.utils.logging_config import setup_logger from dimos.utils.reactive import backpressure @@ -44,7 +44,7 @@ logger = setup_logger() -class ObjectSceneRegistrationModule(SkillModule): +class ObjectSceneRegistrationModule(Module): """Module for detecting objects in camera images using YOLO-E with 2D and 3D detection.""" color_image: In[Image] @@ -221,7 +221,7 @@ def get_full_scene_pointcloud( return pc - @skill() + @skill def detect(self, *prompts: str) -> str: """Detect objects matching the given text prompts. @@ -253,7 +253,7 @@ def detect(self, *prompts: str) -> str: obj_list = [f" - {obj['name']} (object_id='{obj['object_id']}')" for obj in detected] return f"Detected {len(detected)} object(s):\n" + "\n".join(obj_list) - @skill() + @skill def select(self, track_id: int) -> str: """Select an object by track_id and promote it to permanent. diff --git a/dimos/perception/spatial_perception.py b/dimos/perception/spatial_perception.py index 7bb83b67cd..d7f27c31dc 100644 --- a/dimos/perception/spatial_perception.py +++ b/dimos/perception/spatial_perception.py @@ -33,7 +33,7 @@ from dimos.agents_deprecated.memory.visual_memory import VisualMemory from dimos.constants import DIMOS_PROJECT_ROOT from dimos.core import DimosCluster, In, rpc -from dimos.core.skill_module import SkillModule +from dimos.core.module import Module from dimos.msgs.sensor_msgs import Image from dimos.types.robot_location import RobotLocation from dimos.utils.logging_config import setup_logger @@ -51,7 +51,7 @@ logger = setup_logger() -class SpatialMemory(SkillModule): +class SpatialMemory(Module): """ A Dask module for building and querying Robot spatial memory. diff --git a/dimos/protocol/mcp/README.md b/dimos/protocol/mcp/README.md index a034e3d631..233e852669 100644 --- a/dimos/protocol/mcp/README.md +++ b/dimos/protocol/mcp/README.md @@ -30,6 +30,6 @@ uv run dimos run unitree-go2-agentic-mcp ## How It Works -1. `llm_agent(mcp_port=9990)` in the blueprint starts a TCP server +1. `MCPModule` in the blueprint starts a TCP server on port 9990 2. Claude Code spawns the bridge (`--bridge`) which connects to `localhost:9990` 3. Skills are exposed as MCP tools (e.g., `relative_move`, `navigate_with_text`) diff --git a/dimos/protocol/mcp/__init__.py b/dimos/protocol/mcp/__init__.py index 51432ba0cf..e69de29bb2 100644 --- a/dimos/protocol/mcp/__init__.py +++ b/dimos/protocol/mcp/__init__.py @@ -1,17 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from dimos.protocol.mcp.mcp import MCPModule - -__all__ = ["MCPModule"] diff --git a/dimos/protocol/mcp/mcp.py b/dimos/protocol/mcp/mcp.py index f7427cd613..78d19c64db 100644 --- a/dimos/protocol/mcp/mcp.py +++ b/dimos/protocol/mcp/mcp.py @@ -16,26 +16,28 @@ import asyncio import json from typing import TYPE_CHECKING, Any -import uuid from dimos.core import Module, rpc -from dimos.protocol.skill.coordinator import SkillCoordinator, SkillStateEnum +from dimos.core.rpc_client import RpcCall, RPCClient if TYPE_CHECKING: - from dimos.protocol.skill.coordinator import SkillState + from dimos.core.module import SkillInfo class MCPModule(Module): - def __init__(self, *args, **kwargs) -> None: # type: ignore[no-untyped-def] + _skills: list[SkillInfo] + _rpc_calls: dict[str, RpcCall] + + def __init__(self, *args: Any, **kwargs: Any) -> None: super().__init__(*args, **kwargs) - self.coordinator = SkillCoordinator() + self._skills = [] + self._rpc_calls = {} self._server: asyncio.AbstractServer | None = None self._server_future: object | None = None @rpc def start(self) -> None: super().start() - self.coordinator.start() self._start_server() @rpc @@ -48,12 +50,16 @@ def stop(self) -> None: self._server = None if self._server_future and hasattr(self._server_future, "cancel"): self._server_future.cancel() - self.coordinator.stop() super().stop() @rpc - def register_skills(self, container) -> None: # type: ignore[no-untyped-def] - self.coordinator.register_skills(container) + def on_system_modules(self, modules: list[RPCClient]) -> None: + assert self.rpc is not None + self._skills = [skill for module in modules for skill in (module.get_skills() or [])] + self._rpc_calls = { + skill.func_name: RpcCall(None, self.rpc, skill.func_name, skill.class_name, []) + for skill in self._skills + } def _start_server(self, port: int = 9990) -> None: async def handle_client(reader, writer) -> None: # type: ignore[no-untyped-def] @@ -85,15 +91,16 @@ async def _handle_request(self, request: dict[str, Any]) -> dict[str, Any]: } return {"jsonrpc": "2.0", "id": req_id, "result": init_result} if method == "tools/list": - tools = [ - { - "name": c.name, - "description": c.schema.get("function", {}).get("description", ""), - "inputSchema": c.schema.get("function", {}).get("parameters", {}), - } - for c in self.coordinator.skills().values() - if not c.hide_skill - ] + tools = [] + for skill in self._skills: + schema = json.loads(skill.args_schema) + tools.append( + { + "name": skill.func_name, + "description": schema.get("description", ""), + "inputSchema": schema, + } + ) return {"jsonrpc": "2.0", "id": req_id, "result": {"tools": tools}} if method == "tools/call": name = params.get("name") @@ -106,21 +113,20 @@ async def _handle_request(self, request: dict[str, Any]) -> dict[str, Any]: } if not isinstance(args, dict): args = {} - call_id = str(uuid.uuid4()) - self.coordinator.call_skill(call_id, name, args) - result: SkillState | None = self.coordinator._skill_state.get(call_id) + rpc_call = self._rpc_calls.get(name) + if rpc_call is None: + return { + "jsonrpc": "2.0", + "id": req_id, + "result": {"content": [{"type": "text", "text": "Skill not found"}]}, + } try: - await asyncio.wait_for(self.coordinator.wait_for_updates(), timeout=5.0) - except asyncio.TimeoutError: - pass - if result is None: - text = "Skill not found" - elif result.state == SkillStateEnum.completed: - text = str(result.content()) if result.content() else "Completed" - elif result.state == SkillStateEnum.error: - text = f"Error: {result.content()}" - else: - text = f"Started ({result.state.name})" + result = await asyncio.get_event_loop().run_in_executor( + None, lambda: rpc_call(**args) + ) + text = str(result) if result is not None else "Completed" + except Exception as e: + text = f"Error: {e}" return { "jsonrpc": "2.0", "id": req_id, diff --git a/dimos/protocol/mcp/test_mcp_module.py b/dimos/protocol/mcp/test_mcp_module.py index 15ed512845..050e24f13b 100644 --- a/dimos/protocol/mcp/test_mcp_module.py +++ b/dimos/protocol/mcp/test_mcp_module.py @@ -16,17 +16,26 @@ import asyncio import json -import os from pathlib import Path -import socket -import subprocess -import sys - -import pytest +from unittest.mock import MagicMock +from dimos.core.module import SkillInfo from dimos.protocol.mcp.mcp import MCPModule -from dimos.protocol.skill.coordinator import SkillStateEnum -from dimos.protocol.skill.skill import SkillContainer, skill + + +def _make_mcp(skills: list[SkillInfo], call_results: dict[str, object]) -> MCPModule: + """Create an MCPModule with pre-populated skills and mock RPC calls.""" + mcp = MCPModule.__new__(MCPModule) + mcp._skills = skills + mcp._rpc_calls = {} + for skill in skills: + mock_call = MagicMock() + if skill.func_name in call_results: + mock_call.return_value = call_results[skill.func_name] + else: + mock_call.return_value = None + mcp._rpc_calls[skill.func_name] = mock_call + return mcp def test_unitree_blueprint_has_mcp() -> None: @@ -38,38 +47,21 @@ def test_unitree_blueprint_has_mcp() -> None: def test_mcp_module_request_flow() -> None: - class DummySkill: - def __init__(self) -> None: - self.name = "add" - self.hide_skill = False - self.schema = {"function": {"description": "", "parameters": {"type": "object"}}} - - class DummyState: - def __init__(self, content: int) -> None: - self.state = SkillStateEnum.completed - self._content = content - - def content(self) -> int: - return self._content - - class DummyCoordinator: - def __init__(self) -> None: - self._skill_state: dict[str, DummyState] = {} - - def skills(self) -> dict[str, DummySkill]: - return {"add": DummySkill()} - - def call_skill(self, call_id: str, _name: str, args: dict[str, int]) -> None: - self._skill_state[call_id] = DummyState(args["x"] + args["y"]) - - async def wait_for_updates(self) -> bool: - return True + schema = json.dumps( + { + "type": "object", + "description": "Add two numbers", + "properties": {"x": {"type": "integer"}, "y": {"type": "integer"}}, + "required": ["x", "y"], + } + ) + skills = [SkillInfo(class_name="TestSkills", func_name="add", args_schema=schema)] - mcp = MCPModule.__new__(MCPModule) - mcp.coordinator = DummyCoordinator() + mcp = _make_mcp(skills, {"add": 5}) response = asyncio.run(mcp._handle_request({"method": "tools/list", "id": 1})) assert response["result"]["tools"][0]["name"] == "add" + assert response["result"]["tools"][0]["description"] == "Add two numbers" response = asyncio.run( mcp._handle_request( @@ -83,128 +75,56 @@ async def wait_for_updates(self) -> bool: assert response["result"]["content"][0]["text"] == "5" -def test_mcp_module_handles_hidden_and_errors() -> None: - class DummySkill: - def __init__(self, name: str, hide_skill: bool) -> None: - self.name = name - self.hide_skill = hide_skill - self.schema = {"function": {"description": "", "parameters": {"type": "object"}}} - - class DummyState: - def __init__(self, state: SkillStateEnum, content: str | None) -> None: - self.state = state - self._content = content - - def content(self) -> str | None: - return self._content - - class DummyCoordinator: - def __init__(self) -> None: - self._skill_state: dict[str, DummyState] = {} - self._skills = { - "visible": DummySkill("visible", False), - "hidden": DummySkill("hidden", True), - "fail": DummySkill("fail", False), - } - - def skills(self) -> dict[str, DummySkill]: - return self._skills +def test_mcp_module_handles_errors() -> None: + schema = json.dumps({"type": "object", "properties": {}}) + skills = [ + SkillInfo(class_name="TestSkills", func_name="ok_skill", args_schema=schema), + SkillInfo(class_name="TestSkills", func_name="fail_skill", args_schema=schema), + ] - def call_skill(self, call_id: str, name: str, _args: dict[str, int]) -> None: - if name == "fail": - self._skill_state[call_id] = DummyState(SkillStateEnum.error, "boom") - elif name in self._skills: - self._skill_state[call_id] = DummyState(SkillStateEnum.running, None) - - async def wait_for_updates(self) -> bool: - return True - - mcp = MCPModule.__new__(MCPModule) - mcp.coordinator = DummyCoordinator() + mcp = _make_mcp(skills, {"ok_skill": "done"}) + mcp._rpc_calls["fail_skill"] = MagicMock(side_effect=RuntimeError("boom")) + # All skills listed response = asyncio.run(mcp._handle_request({"method": "tools/list", "id": 1})) tool_names = {tool["name"] for tool in response["result"]["tools"]} - assert "visible" in tool_names - assert "hidden" not in tool_names + assert "ok_skill" in tool_names + assert "fail_skill" in tool_names + # Error skill returns error text response = asyncio.run( mcp._handle_request( - {"method": "tools/call", "id": 2, "params": {"name": "fail", "arguments": {}}} + {"method": "tools/call", "id": 2, "params": {"name": "fail_skill", "arguments": {}}} ) ) assert "Error:" in response["result"]["content"][0]["text"] + assert "boom" in response["result"]["content"][0]["text"] - -@pytest.mark.integration -def test_mcp_end_to_end_lcm_bridge() -> None: - try: - import lcm # type: ignore[import-untyped] - - lcm.LCM() - except Exception as exc: - if os.environ.get("CI"): - pytest.fail(f"LCM unavailable for MCP end-to-end test: {exc}") - pytest.skip("LCM unavailable for MCP end-to-end test.") - - try: - socket.socket(socket.AF_INET, socket.SOCK_STREAM).close() - except PermissionError: - if os.environ.get("CI"): - pytest.fail("Socket creation not permitted in CI environment.") - pytest.skip("Socket creation not permitted in this environment.") - - class TestSkills(SkillContainer): - @skill() - def add(self, x: int, y: int) -> int: - return x + y - - mcp = MCPModule() - mcp.start() - - try: - mcp.register_skills(TestSkills()) - - env = {"MCP_HOST": "127.0.0.1", "MCP_PORT": "9990"} - proc = subprocess.Popen( - [sys.executable, "-m", "dimos.protocol.mcp"], - stdin=subprocess.PIPE, - stdout=subprocess.PIPE, - stderr=subprocess.PIPE, - env={**os.environ, **env}, - text=True, + # Unknown skill returns not found + response = asyncio.run( + mcp._handle_request( + {"method": "tools/call", "id": 3, "params": {"name": "no_such", "arguments": {}}} ) - try: - request = {"jsonrpc": "2.0", "id": 1, "method": "tools/list"} - proc.stdin.write(json.dumps(request) + "\n") - proc.stdin.flush() - stdout = proc.stdout.readline() - assert '"tools"' in stdout - assert '"add"' in stdout - finally: - proc.terminate() - proc.wait(timeout=5) - - proc = subprocess.Popen( - [sys.executable, "-m", "dimos.protocol.mcp"], - stdin=subprocess.PIPE, - stdout=subprocess.PIPE, - stderr=subprocess.PIPE, - env={**os.environ, **env}, - text=True, + ) + assert "not found" in response["result"]["content"][0]["text"].lower() + + +def test_mcp_module_initialize_and_unknown() -> None: + mcp = _make_mcp([], {}) + + response = asyncio.run(mcp._handle_request({"method": "initialize", "id": 1})) + assert response["result"]["serverInfo"]["name"] == "dimensional" + + response = asyncio.run(mcp._handle_request({"method": "unknown/method", "id": 2})) + assert response["error"]["code"] == -32601 + + +def test_mcp_module_invalid_tool_name() -> None: + mcp = _make_mcp([], {}) + + response = asyncio.run( + mcp._handle_request( + {"method": "tools/call", "id": 1, "params": {"name": 123, "arguments": {}}} ) - try: - request = { - "jsonrpc": "2.0", - "id": 2, - "method": "tools/call", - "params": {"name": "add", "arguments": {"x": 2, "y": 3}}, - } - proc.stdin.write(json.dumps(request) + "\n") - proc.stdin.flush() - stdout = proc.stdout.readline() - assert "5" in stdout - finally: - proc.terminate() - proc.wait(timeout=5) - finally: - mcp.stop() + ) + assert response["error"]["code"] == -32602 diff --git a/dimos/protocol/pubsub/benchmark/test_benchmark.py b/dimos/protocol/pubsub/benchmark/test_benchmark.py index 865c4ee324..39a4421c35 100644 --- a/dimos/protocol/pubsub/benchmark/test_benchmark.py +++ b/dimos/protocol/pubsub/benchmark/test_benchmark.py @@ -82,6 +82,7 @@ def benchmark_results() -> Generator[BenchmarkResults, None, None]: results.print_heatmap() results.print_bandwidth_heatmap() results.print_latency_heatmap() + results.print_loss_heatmap() @pytest.mark.tool diff --git a/dimos/protocol/pubsub/benchmark/testdata.py b/dimos/protocol/pubsub/benchmark/testdata.py index 244d09f105..ad604131e0 100644 --- a/dimos/protocol/pubsub/benchmark/testdata.py +++ b/dimos/protocol/pubsub/benchmark/testdata.py @@ -14,12 +14,20 @@ from collections.abc import Generator from contextlib import contextmanager +from dataclasses import dataclass from typing import TYPE_CHECKING, Any import numpy as np from dimos.msgs.sensor_msgs.Image import Image, ImageFormat from dimos.protocol.pubsub.benchmark.type import Case + +try: + import cyclonedds as _cyclonedds # noqa: F401 + + DDS_AVAILABLE = True +except ImportError: + DDS_AVAILABLE = False from dimos.protocol.pubsub.impl.lcmpubsub import LCM, LCMPubSubBase, Topic as LCMTopic from dimos.protocol.pubsub.impl.memory import Memory from dimos.protocol.pubsub.impl.shmpubsub import ( @@ -173,6 +181,67 @@ def shm_lcm_pubsub_channel() -> Generator[LCMSharedMemory, None, None]: ) ) +if DDS_AVAILABLE: + from cyclonedds.idl import IdlStruct + from cyclonedds.idl.types import sequence, uint8 + from cyclonedds.qos import Policy, Qos + + from dimos.protocol.pubsub.impl.ddspubsub import ( + DDS, + Topic as DDSTopic, + ) + + @dataclass + class DDSBenchmarkData(IdlStruct): # type: ignore[misc] + """DDS message type for benchmarking with variable-size byte payload.""" + + data: sequence[uint8] # type: ignore[type-arg] + + @contextmanager + def dds_high_throughput_pubsub_channel() -> Generator[DDS, None, None]: + """DDS with high-throughput QoS preset.""" + HIGH_THROUGHPUT_QOS = Qos( + Policy.Reliability.BestEffort, + Policy.History.KeepLast(depth=1), + Policy.Durability.Volatile, + ) + dds_pubsub = DDS(qos=HIGH_THROUGHPUT_QOS) + dds_pubsub.start() + yield dds_pubsub + dds_pubsub.stop() + + @contextmanager + def dds_reliable_pubsub_channel() -> Generator[DDS, None, None]: + """DDS with reliable QoS preset.""" + RELIABLE_QOS = Qos( + Policy.Reliability.Reliable(max_blocking_time=0), + Policy.History.KeepLast(depth=5000), + Policy.Durability.Volatile, + ) + dds_pubsub = DDS(qos=RELIABLE_QOS) + dds_pubsub.start() + yield dds_pubsub + dds_pubsub.stop() + + def dds_msggen(size: int) -> tuple[DDSTopic, DDSBenchmarkData]: + """Generate DDS message for benchmark.""" + topic = DDSTopic(name="benchmark/dds", data_type=DDSBenchmarkData) + return (topic, DDSBenchmarkData(data=list(make_data_bytes(size)))) # type: ignore[arg-type] + + testcases.append( + Case( + pubsub_context=dds_high_throughput_pubsub_channel, + msg_gen=dds_msggen, + ) + ) + + testcases.append( + Case( + pubsub_context=dds_reliable_pubsub_channel, + msg_gen=dds_msggen, + ) + ) + try: from dimos.protocol.pubsub.impl.redispubsub import Redis diff --git a/dimos/protocol/pubsub/benchmark/type.py b/dimos/protocol/pubsub/benchmark/type.py index 425f2a480a..a9ef80fe7a 100644 --- a/dimos/protocol/pubsub/benchmark/type.py +++ b/dimos/protocol/pubsub/benchmark/type.py @@ -263,3 +263,11 @@ def fmt(v: float) -> str: fmt, high_is_good=False, ) + + def print_loss_heatmap(self) -> None: + """Print message loss percentage heatmap.""" + + def fmt(v: float) -> str: + return f"{v:.1f}%" + + self._print_heatmap("Loss %", lambda r: r.loss_pct, fmt, high_is_good=False) diff --git a/dimos/protocol/pubsub/impl/ddspubsub.py b/dimos/protocol/pubsub/impl/ddspubsub.py new file mode 100644 index 0000000000..1e6dc36296 --- /dev/null +++ b/dimos/protocol/pubsub/impl/ddspubsub.py @@ -0,0 +1,161 @@ +# 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 + +from collections.abc import Callable +from dataclasses import dataclass +import threading +from typing import TYPE_CHECKING, Any, TypeAlias + +from cyclonedds.core import Listener +from cyclonedds.pub import DataWriter as DDSDataWriter +from cyclonedds.qos import Policy, Qos +from cyclonedds.sub import DataReader as DDSDataReader +from cyclonedds.topic import Topic as DDSTopic + +from dimos.protocol.pubsub.spec import PubSub +from dimos.protocol.service.ddsservice import DDSService +from dimos.utils.logging_config import setup_logger + +if TYPE_CHECKING: + from cyclonedds.idl import IdlStruct + +logger = setup_logger() + + +@dataclass(frozen=True) +class Topic: + """Represents a DDS topic.""" + + name: str + data_type: type[IdlStruct] + + def __str__(self) -> str: + return f"{self.name}#{self.data_type.__name__}" + + +MessageCallback: TypeAlias = Callable[[Any, Topic], None] + + +class _DDSMessageListener(Listener): # type: ignore[misc] + """Listener for DataReader that dispatches messages to callbacks.""" + + __slots__ = ("_callbacks", "_lock", "_topic") + + def __init__(self, topic: Topic) -> None: + super().__init__() # type: ignore[no-untyped-call] + self._topic = topic + self._callbacks: tuple[MessageCallback, ...] = () + self._lock = threading.Lock() + + def add_callback(self, callback: MessageCallback) -> None: + """Add a callback to the listener.""" + with self._lock: + self._callbacks = (*self._callbacks, callback) + + def remove_callback(self, callback: MessageCallback) -> None: + """Remove a callback from the listener.""" + with self._lock: + self._callbacks = tuple(cb for cb in self._callbacks if cb is not callback) + + def on_data_available(self, reader: DDSDataReader[Any]) -> None: + """Called when data is available on the reader.""" + try: + samples = reader.take() + except Exception as e: + logger.error(f"Error reading from topic {self._topic}: {e}", exc_info=True) + return + for sample in samples: + if sample is not None: + for callback in self._callbacks: + try: + callback(sample, self._topic) + except Exception as e: + logger.error(f"Callback error on topic {self._topic}: {e}", exc_info=True) + + +class DDS(DDSService, PubSub[Topic, Any]): + def __init__(self, qos: Qos | None = None, **kwargs: Any) -> None: + super().__init__(**kwargs) + self._qos = qos + self._writers: dict[Topic, DDSDataWriter[Any]] = {} + self._writer_lock = threading.Lock() + self._readers: dict[Topic, DDSDataReader[Any]] = {} + self._reader_lock = threading.Lock() + self._listeners: dict[Topic, _DDSMessageListener] = {} + + @property + def qos(self) -> Qos | None: + """Get the QoS settings.""" + return self._qos + + def _get_writer(self, topic: Topic) -> DDSDataWriter[Any]: + """Get or create a DataWriter for the given topic.""" + with self._writer_lock: + if topic not in self._writers: + dds_topic = DDSTopic(self.participant, topic.name, topic.data_type) + self._writers[topic] = DDSDataWriter(self.participant, dds_topic, qos=self._qos) + return self._writers[topic] + + def publish(self, topic: Topic, message: Any) -> None: + """Publish a message to a DDS topic.""" + writer = self._get_writer(topic) + try: + writer.write(message) + except Exception as e: + logger.error(f"Error publishing to topic {topic}: {e}", exc_info=True) + + def _get_listener(self, topic: Topic) -> _DDSMessageListener: + """Get or create a listener and reader for the given topic.""" + with self._reader_lock: + if topic not in self._readers: + dds_topic = DDSTopic(self.participant, topic.name, topic.data_type) + listener = _DDSMessageListener(topic) + self._readers[topic] = DDSDataReader( + self.participant, dds_topic, qos=self._qos, listener=listener + ) + self._listeners[topic] = listener + return self._listeners[topic] + + def subscribe(self, topic: Topic, callback: MessageCallback) -> Callable[[], None]: + """Subscribe to a DDS topic with a callback.""" + listener = self._get_listener(topic) + listener.add_callback(callback) + return lambda: self._unsubscribe_callback(topic, callback) + + def _unsubscribe_callback(self, topic: Topic, callback: MessageCallback) -> None: + """Unsubscribe a callback from a topic.""" + with self._reader_lock: + listener = self._listeners.get(topic) + if listener: + listener.remove_callback(callback) + + def stop(self) -> None: + """Stop the DDS service and clean up resources.""" + with self._reader_lock: + self._readers.clear() + self._listeners.clear() + with self._writer_lock: + self._writers.clear() + super().stop() + + +__all__ = [ + "DDS", + "MessageCallback", + "Policy", + "Qos", + "Topic", +] diff --git a/dimos/protocol/rpc/spec.py b/dimos/protocol/rpc/spec.py index 1c502abe24..47ad77e825 100644 --- a/dimos/protocol/rpc/spec.py +++ b/dimos/protocol/rpc/spec.py @@ -41,6 +41,8 @@ def call(self, name: str, arguments: Args, cb: Callable[[Any], None]) -> Callabl def call(self, name: str, arguments: Args, cb: Callable | None) -> Callable[[], Any] | None: ... # type: ignore[type-arg] + def call_nowait(self, name: str, arguments: Args) -> None: ... + # we expect to crash if we don't get a return value after 10 seconds # but callers can override this timeout for extra long functions def call_sync( diff --git a/dimos/protocol/service/__init__.py b/dimos/protocol/service/__init__.py index bcafa6af4a..fb9df08ca9 100644 --- a/dimos/protocol/service/__init__.py +++ b/dimos/protocol/service/__init__.py @@ -1,4 +1,4 @@ -from dimos.protocol.service.lcmservice import LCMService as LCMService +from dimos.protocol.service.lcmservice import LCMService from dimos.protocol.service.spec import Configurable as Configurable, Service as Service __all__ = [ diff --git a/dimos/protocol/service/ddsservice.py b/dimos/protocol/service/ddsservice.py new file mode 100644 index 0000000000..6ed04c07ad --- /dev/null +++ b/dimos/protocol/service/ddsservice.py @@ -0,0 +1,80 @@ +# 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 + +from dataclasses import dataclass +import threading +from typing import TYPE_CHECKING, Any + +try: + from cyclonedds.domain import DomainParticipant + + DDS_AVAILABLE = True +except ImportError: + DDS_AVAILABLE = False + DomainParticipant = None # type: ignore[assignment, misc] + +from dimos.protocol.service.spec import Service +from dimos.utils.logging_config import setup_logger + +if TYPE_CHECKING: + from cyclonedds.qos import Qos + +logger = setup_logger() + +_participants: dict[int, DomainParticipant] = {} +_participants_lock = threading.Lock() + + +@dataclass +class DDSConfig: + """Configuration for DDS service.""" + + domain_id: int = 0 + qos: Qos | None = None + + +class DDSService(Service[DDSConfig]): + default_config = DDSConfig + + def __init__(self, **kwargs: Any) -> None: + super().__init__(**kwargs) + + def start(self) -> None: + """Start the DDS service.""" + domain_id = self.config.domain_id + with _participants_lock: + if domain_id not in _participants: + _participants[domain_id] = DomainParticipant(domain_id) + logger.info(f"DDS service started with Cyclone DDS domain {domain_id}") + super().start() + + def stop(self) -> None: + """Stop the DDS service.""" + super().stop() + + @property + def participant(self) -> DomainParticipant: + """Get the DomainParticipant instance for this service's domain.""" + domain_id = self.config.domain_id + if domain_id not in _participants: + raise RuntimeError(f"DomainParticipant not initialized for domain {domain_id}") + return _participants[domain_id] + + +__all__ = [ + "DDSConfig", + "DDSService", +] diff --git a/dimos/protocol/skill/__init__.py b/dimos/protocol/skill/__init__.py deleted file mode 100644 index 09f78b6e76..0000000000 --- a/dimos/protocol/skill/__init__.py +++ /dev/null @@ -1,6 +0,0 @@ -from dimos.protocol.skill.skill import SkillContainer, skill - -__all__ = [ - "SkillContainer", - "skill", -] diff --git a/dimos/protocol/skill/comms.py b/dimos/protocol/skill/comms.py deleted file mode 100644 index 2f281f4078..0000000000 --- a/dimos/protocol/skill/comms.py +++ /dev/null @@ -1,99 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -from __future__ import annotations - -from abc import abstractmethod -from dataclasses import dataclass, field -from typing import TYPE_CHECKING, Generic, TypeVar - -from dimos.protocol.pubsub.impl.lcmpubsub import PickleLCM, Topic as LCMTopic -from dimos.protocol.service import Service -from dimos.protocol.skill.type import MsgType, SkillMsg - -if TYPE_CHECKING: - from collections.abc import Callable - - from dimos.protocol.pubsub.spec import PubSub - -# defines a protocol for communication between skills and agents -# it has simple requirements of pub/sub semantics capable of sending and receiving SkillMsg objects - - -class SkillCommsSpec: - @abstractmethod - def publish(self, msg: SkillMsg[MsgType]) -> None: ... - - @abstractmethod - def subscribe(self, cb: Callable[[SkillMsg[MsgType]], None]) -> None: ... - - @abstractmethod - def start(self) -> None: ... - - @abstractmethod - def stop(self) -> None: ... - - -MsgT = TypeVar("MsgT") -TopicT = TypeVar("TopicT") - - -@dataclass -class PubSubCommsConfig(Generic[TopicT, MsgT]): - topic: TopicT | None = None - pubsub: type[PubSub[TopicT, MsgT]] | PubSub[TopicT, MsgT] | None = None - autostart: bool = True - - -# implementation of the SkillComms using any standard PubSub mechanism -class PubSubComms(Service[PubSubCommsConfig[TopicT, MsgT]], SkillCommsSpec, Generic[TopicT, MsgT]): - default_config: type[PubSubCommsConfig[TopicT, MsgT]] = PubSubCommsConfig # type: ignore[assignment] - - def __init__(self, **kwargs: object) -> None: - super().__init__(**kwargs) - pubsub_config = getattr(self.config, "pubsub", None) - if pubsub_config is not None: - if callable(pubsub_config): - self.pubsub = pubsub_config() - else: - self.pubsub = pubsub_config - else: - raise ValueError("PubSub configuration is missing") - - if getattr(self.config, "autostart", True): - self.start() - - def start(self) -> None: - self.pubsub.start() - - def stop(self) -> None: - self.pubsub.stop() - - def publish(self, msg: SkillMsg[MsgType]) -> None: - self.pubsub.publish(self.config.topic, msg) - - def subscribe(self, cb: Callable[[SkillMsg[MsgType]], None]) -> None: - self.pubsub.subscribe(self.config.topic, lambda msg, topic: cb(msg)) - - -@dataclass -class LCMCommsConfig(PubSubCommsConfig[LCMTopic, SkillMsg[MsgType]]): - topic: LCMTopic = field(default_factory=lambda: LCMTopic("/skill")) - pubsub: type[PickleLCM] = PickleLCM - # lcm needs to be started only if receiving - # skill comms are broadcast only in modules so we don't autostart - autostart: bool = False - - -class LCMSkillComms(PubSubComms[LCMTopic, SkillMsg[MsgType]]): - default_config: type[LCMCommsConfig] = LCMCommsConfig diff --git a/dimos/protocol/skill/coordinator.py b/dimos/protocol/skill/coordinator.py deleted file mode 100644 index 95fc8844d4..0000000000 --- a/dimos/protocol/skill/coordinator.py +++ /dev/null @@ -1,637 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import asyncio -from copy import copy -from dataclasses import dataclass -from enum import Enum -import json -import time -from typing import Any, Literal - -from langchain_core.messages import ToolMessage -from langchain_core.tools import tool as langchain_tool -from rich.console import Console -from rich.table import Table -from rich.text import Text - -from dimos.core import rpc -from dimos.core.module import Module, ModuleConfig -from dimos.protocol.skill.comms import LCMSkillComms, SkillCommsSpec -from dimos.protocol.skill.skill import SkillConfig, SkillContainer # type: ignore[attr-defined] -from dimos.protocol.skill.type import MsgType, Output, Reducer, Return, SkillMsg, Stream -from dimos.protocol.skill.utils import interpret_tool_call_args -from dimos.utils.logging_config import setup_logger - -logger = setup_logger() - - -@dataclass -class SkillCoordinatorConfig(ModuleConfig): - skill_transport: type[SkillCommsSpec] = LCMSkillComms - - -class SkillStateEnum(Enum): - pending = 0 - running = 1 - completed = 2 - error = 3 - - def colored_name(self) -> Text: - """Return the state name as a rich Text object with color.""" - colors = { - SkillStateEnum.pending: "yellow", - SkillStateEnum.running: "blue", - SkillStateEnum.completed: "green", - SkillStateEnum.error: "red", - } - return Text(self.name, style=colors.get(self, "white")) - - -# This object maintains the state of a skill run on a caller end -class SkillState: - call_id: str - name: str - state: SkillStateEnum - skill_config: SkillConfig - - msg_count: int = 0 - sent_tool_msg: bool = False - - start_msg: SkillMsg[Literal[MsgType.start]] = None # type: ignore[assignment] - end_msg: SkillMsg[Literal[MsgType.ret]] = None # type: ignore[assignment] - error_msg: SkillMsg[Literal[MsgType.error]] = None # type: ignore[assignment] - ret_msg: SkillMsg[Literal[MsgType.ret]] = None # type: ignore[assignment] - reduced_stream_msg: list[SkillMsg[Literal[MsgType.reduced_stream]]] = None # type: ignore[assignment] - - def __init__(self, call_id: str, name: str, skill_config: SkillConfig | None = None) -> None: - super().__init__() - - self.skill_config = skill_config or SkillConfig( - name=name, - stream=Stream.none, - ret=Return.none, - reducer=Reducer.all, - output=Output.standard, - schema={}, - ) - - self.state = SkillStateEnum.pending - self.call_id = call_id - self.name = name - - def duration(self) -> float: - """Calculate the duration of the skill run.""" - if self.start_msg and self.end_msg: - return self.end_msg.ts - self.start_msg.ts - elif self.start_msg: - return time.time() - self.start_msg.ts - else: - return 0.0 - - def content(self) -> dict[str, Any] | str | int | float | None: # type: ignore[return] - if self.state == SkillStateEnum.running: - if self.reduced_stream_msg: - return self.reduced_stream_msg.content # type: ignore[attr-defined, no-any-return] - - if self.state == SkillStateEnum.completed: - if self.reduced_stream_msg: # are we a streaming skill? - return self.reduced_stream_msg.content # type: ignore[attr-defined, no-any-return] - return self.ret_msg.content # type: ignore[return-value] - - if self.state == SkillStateEnum.error: - print("Error msg:", self.error_msg.content) - if self.reduced_stream_msg: - (self.reduced_stream_msg.content + "\n" + self.error_msg.content) # type: ignore[attr-defined] - else: - return self.error_msg.content # type: ignore[return-value] - - def agent_encode(self) -> ToolMessage | str: - # tool call can emit a single ToolMessage - # subsequent messages are considered SituationalAwarenessMessages, - # those are collapsed into a HumanMessage, that's artificially prepended to history - - if not self.sent_tool_msg: - self.sent_tool_msg = True - return ToolMessage( - self.content() or "Querying, please wait, you will receive a response soon.", # type: ignore[arg-type] - name=self.name, - tool_call_id=self.call_id, - ) - else: - return json.dumps( - { - "name": self.name, - "call_id": self.call_id, - "state": self.state.name, - "data": self.content(), - "ran_for": self.duration(), - } - ) - - # returns True if the agent should be called for this message - def handle_msg(self, msg: SkillMsg) -> bool: # type: ignore[type-arg] - self.msg_count += 1 - if msg.type == MsgType.stream: - self.state = SkillStateEnum.running - self.reduced_stream_msg = self.skill_config.reducer(self.reduced_stream_msg, msg) # type: ignore[arg-type, assignment] - - if ( - self.skill_config.stream == Stream.none - or self.skill_config.stream == Stream.passive - ): - return False - - if self.skill_config.stream == Stream.call_agent: - return True - - if msg.type == MsgType.ret: - self.state = SkillStateEnum.completed - self.ret_msg = msg - if self.skill_config.ret == Return.call_agent: - return True - return False - - if msg.type == MsgType.error: - self.state = SkillStateEnum.error - self.error_msg = msg - return True - - if msg.type == MsgType.start: - self.state = SkillStateEnum.running - self.start_msg = msg - return False - - return False - - def __len__(self) -> int: - return self.msg_count - - def __str__(self) -> str: - # For standard string representation, we'll use rich's Console to render the colored text - console = Console(force_terminal=True, legacy_windows=False) - colored_state = self.state.colored_name() - - # Build the parts of the string - parts = [Text(f"SkillState({self.name} "), colored_state, Text(f", call_id={self.call_id}")] - - if self.state == SkillStateEnum.completed or self.state == SkillStateEnum.error: - parts.append(Text(", ran for=")) - else: - parts.append(Text(", running for=")) - - parts.append(Text(f"{self.duration():.2f}s")) - - if len(self): - parts.append(Text(f", msg_count={self.msg_count})")) - else: - parts.append(Text(", No Messages)")) - - # Combine all parts into a single Text object - combined = Text() - for part in parts: - combined.append(part) - - # Render to string with console - with console.capture() as capture: - console.print(combined, end="") - return capture.get() - - -# subclassed the dict just to have a better string representation -class SkillStateDict(dict[str, SkillState]): - """Custom dict for skill states with better string representation.""" - - def table(self) -> Table: - # Add skill states section - states_table = Table(show_header=True) - states_table.add_column("Call ID", style="dim", width=12) - states_table.add_column("Skill", style="white") - states_table.add_column("State", style="white") - states_table.add_column("Duration", style="yellow") - states_table.add_column("Messages", style="dim") - - for call_id, skill_state in self.items(): - # Get colored state name - state_text = skill_state.state.colored_name() - - # Duration formatting - if ( - skill_state.state == SkillStateEnum.completed - or skill_state.state == SkillStateEnum.error - ): - duration = f"{skill_state.duration():.2f}s" - else: - duration = f"{skill_state.duration():.2f}s..." - - # Messages info - msg_count = str(len(skill_state)) - - states_table.add_row( - call_id[:8] + "...", skill_state.name, state_text, duration, msg_count - ) - - if not self: - states_table.add_row("", "[dim]No active skills[/dim]", "", "", "") - return states_table - - def __str__(self) -> str: - console = Console(force_terminal=True, legacy_windows=False) - - # Render to string with title above - with console.capture() as capture: - console.print(Text(" SkillState", style="bold blue")) - console.print(self.table()) - return capture.get().strip() - - -# This class is responsible for managing the lifecycle of skills, -# handling skill calls, and coordinating communication between the agent and skills. -# -# It aggregates skills from static and dynamic containers, manages skill states, -# and decides when to notify the agent about updates. -class SkillCoordinator(Module): - default_config = SkillCoordinatorConfig - empty: bool = True - - _static_containers: list[SkillContainer] - _dynamic_containers: list[SkillContainer] - _skill_state: SkillStateDict # key is call_id, not skill_name - _skills: dict[str, SkillConfig] - _updates_available: asyncio.Event | None - _agent_loop: asyncio.AbstractEventLoop | None - - def __init__(self, **kwargs: Any) -> None: - super().__init__(**kwargs) - self._static_containers = [] - self._dynamic_containers = [] - self._skills = {} - self._skill_state = SkillStateDict() - - # Defer event creation until we're in the correct loop context - self._updates_available = None - self._agent_loop = None - self._pending_notifications = 0 # Count pending notifications - self._closed_coord = False - self._transport_unsub_fn = None - - def _ensure_updates_available(self) -> asyncio.Event: - """Lazily create the updates available event in the correct loop context.""" - if self._updates_available is None: - # Create the event in the current running loop, not the stored loop - try: - loop = asyncio.get_running_loop() - # print(f"[DEBUG] Creating _updates_available event in current loop {id(loop)}") - # Always use the current running loop for the event - # This ensures the event is created in the context where it will be used - self._updates_available = asyncio.Event() - # Store the loop where the event was created - this is the agent's loop - self._agent_loop = loop - # print( - # f"[DEBUG] Created _updates_available event {id(self._updates_available)} in agent loop {id(loop)}" - # ) - except RuntimeError: - # No running loop, defer event creation until we have the proper context - # print(f"[DEBUG] No running loop, deferring event creation") - # Don't create the event yet - wait for the proper loop context - pass - else: - ... - # print(f"[DEBUG] Reusing _updates_available event {id(self._updates_available)}") - return self._updates_available # type: ignore[return-value] - - @rpc - def start(self) -> None: - super().start() - self.skill_transport.start() - self._transport_unsub_fn = self.skill_transport.subscribe(self.handle_message) - - @rpc - def stop(self) -> None: - self._close_module() - self._closed_coord = True - self.skill_transport.stop() - if self._transport_unsub_fn: - self._transport_unsub_fn() - - # Stop all registered skill containers - for container in self._static_containers: - container.stop() - for container in self._dynamic_containers: - container.stop() - - super().stop() - - def len(self) -> int: - return len(self._skills) - - def __len__(self) -> int: - return self.len() - - # this can be converted to non-langchain json schema output - # and langchain takes this output as well - # just faster for now - def get_tools(self) -> list[dict]: # type: ignore[type-arg] - return [ - langchain_tool(skill_config.f) # type: ignore[arg-type, misc] - for skill_config in self.skills().values() - if not skill_config.hide_skill - ] - - # internal skill call - def call_skill( - self, call_id: str | Literal[False], skill_name: str, args: dict[str, Any] - ) -> None: - if not call_id: - call_id = str(time.time()) - skill_config = self.get_skill_config(skill_name) - if not skill_config: - logger.error( - f"Skill {skill_name} not found in registered skills, but agent tried to call it (did a dynamic skill expire?)" - ) - return - - self._skill_state[call_id] = SkillState( - call_id=call_id, name=skill_name, skill_config=skill_config - ) - - # TODO agent often calls the skill again if previous response is still loading. - # maybe create a new skill_state linked to a previous one? not sure - - arg_keywords = args.get("args") or {} - arg_list = [] - - if isinstance(arg_keywords, list): - arg_list = arg_keywords - arg_keywords = {} - - arg_list, arg_keywords = interpret_tool_call_args(args) - - return skill_config.call( # type: ignore[no-any-return] - call_id, - *arg_list, - **arg_keywords, - ) - - # Receives a message from active skill - # Updates local skill state (appends to streamed data if needed etc) - # - # Checks if agent needs to be notified (if ToolConfig has Return=call_agent or Stream=call_agent) - def handle_message(self, msg: SkillMsg) -> None: # type: ignore[type-arg] - if self._closed_coord: - import traceback - - traceback.print_stack() - return - # logger.info(f"SkillMsg from {msg.skill_name}, {msg.call_id} - {msg}") - - if self._skill_state.get(msg.call_id) is None: - logger.warn( - f"Skill state for {msg.skill_name} (call_id={msg.call_id}) not found, (skill not called by our agent?) initializing. (message received: {msg})" - ) - self._skill_state[msg.call_id] = SkillState(call_id=msg.call_id, name=msg.skill_name) - - should_notify = self._skill_state[msg.call_id].handle_msg(msg) - - if should_notify: - updates_available = self._ensure_updates_available() - if updates_available is None: - print("[DEBUG] Event not created yet, deferring notification") - return - - try: - current_loop = asyncio.get_running_loop() - agent_loop = getattr(self, "_agent_loop", self._loop) - # print( - # f"[DEBUG] handle_message: current_loop={id(current_loop)}, agent_loop={id(agent_loop) if agent_loop else 'None'}, event={id(updates_available)}" - # ) - if agent_loop and agent_loop != current_loop: - # print( - # f"[DEBUG] Calling set() via call_soon_threadsafe from loop {id(current_loop)} to agent loop {id(agent_loop)}" - # ) - agent_loop.call_soon_threadsafe(updates_available.set) - else: - # print(f"[DEBUG] Calling set() directly in current loop {id(current_loop)}") - updates_available.set() - except RuntimeError: - # No running loop, use call_soon_threadsafe if we have an agent loop - agent_loop = getattr(self, "_agent_loop", self._loop) - # print( - # f"[DEBUG] No current running loop, agent_loop={id(agent_loop) if agent_loop else 'None'}" - # ) - if agent_loop: - # print( - # f"[DEBUG] Calling set() via call_soon_threadsafe to agent loop {id(agent_loop)}" - # ) - agent_loop.call_soon_threadsafe(updates_available.set) - else: - # print(f"[DEBUG] Event creation was deferred, can't notify") - pass - - def has_active_skills(self) -> bool: - if not self.has_passive_skills(): - return False - for skill_run in self._skill_state.values(): - # check if this skill will notify agent - if skill_run.skill_config.ret == Return.call_agent: - return True - if skill_run.skill_config.stream == Stream.call_agent: - return True - return False - - def has_passive_skills(self) -> bool: - # check if dict is empty - if self._skill_state == {}: - return False - return True - - async def wait_for_updates(self, timeout: float | None = None) -> True: # type: ignore[valid-type] - """Wait for skill updates to become available. - - This method should be called by the agent when it's ready to receive updates. - It will block until updates are available or timeout is reached. - - Args: - timeout: Optional timeout in seconds - - Returns: - True if updates are available, False on timeout - """ - updates_available = self._ensure_updates_available() - if updates_available is None: - # Force event creation now that we're in the agent's loop context - # print(f"[DEBUG] wait_for_updates: Creating event in current loop context") - current_loop = asyncio.get_running_loop() - self._updates_available = asyncio.Event() - self._agent_loop = current_loop - updates_available = self._updates_available - # print( - # f"[DEBUG] wait_for_updates: Created event {id(updates_available)} in loop {id(current_loop)}" - # ) - - try: - current_loop = asyncio.get_running_loop() - - # Double-check the loop context before waiting - if self._agent_loop != current_loop: - # print(f"[DEBUG] Loop context changed! Recreating event for loop {id(current_loop)}") - self._updates_available = asyncio.Event() - self._agent_loop = current_loop - updates_available = self._updates_available - - # print( - # f"[DEBUG] wait_for_updates: current_loop={id(current_loop)}, event={id(updates_available)}, is_set={updates_available.is_set()}" - # ) - if timeout: - # print(f"[DEBUG] Waiting for event with timeout {timeout}") - await asyncio.wait_for(updates_available.wait(), timeout=timeout) - else: - print("[DEBUG] Waiting for event without timeout") - await updates_available.wait() - print("[DEBUG] Event was set! Returning True") - return True - except asyncio.TimeoutError: - print("[DEBUG] Timeout occurred while waiting for event") - return False - except RuntimeError as e: - if "bound to a different event loop" in str(e): - print( - "[DEBUG] Event loop binding error detected, recreating event and returning False to retry" - ) - # Recreate the event in the current loop - current_loop = asyncio.get_running_loop() - self._updates_available = asyncio.Event() - self._agent_loop = current_loop - return False - else: - raise - - def generate_snapshot(self, clear: bool = True) -> SkillStateDict: - """Generate a fresh snapshot of completed skills and optionally clear them.""" - ret = copy(self._skill_state) - - if clear: - updates_available = self._ensure_updates_available() - if updates_available is not None: - # print(f"[DEBUG] generate_snapshot: clearing event {id(updates_available)}") - updates_available.clear() - else: - ... - # rint(f"[DEBUG] generate_snapshot: event not created yet, nothing to clear") - to_delete = [] - # Since snapshot is being sent to agent, we can clear the finished skill runs - for call_id, skill_run in self._skill_state.items(): - if skill_run.state == SkillStateEnum.completed: - logger.info(f"Skill {skill_run.name} (call_id={call_id}) finished") - to_delete.append(call_id) - if skill_run.state == SkillStateEnum.error: - error_msg = skill_run.error_msg.content.get("msg", "Unknown error") # type: ignore[union-attr] - error_traceback = skill_run.error_msg.content.get( # type: ignore[union-attr] - "traceback", "No traceback available" - ) - - logger.error( - f"Skill error for {skill_run.name} (call_id={call_id}): {error_msg}" - ) - print(error_traceback) - to_delete.append(call_id) - - elif ( - skill_run.state == SkillStateEnum.running - and skill_run.reduced_stream_msg is not None - ): - # preserve ret as a copy - ret[call_id] = copy(skill_run) - logger.debug( - f"Resetting accumulator for skill {skill_run.name} (call_id={call_id})" - ) - skill_run.reduced_stream_msg = None # type: ignore[assignment] - - for call_id in to_delete: - logger.debug(f"Call {call_id} finished, removing from state") - del self._skill_state[call_id] - - return ret - - def __str__(self) -> str: - console = Console(force_terminal=True, legacy_windows=False) - - # Create main table without any header - table = Table(show_header=False) - - # Add containers section - containers_table = Table(show_header=True, show_edge=False, box=None) - containers_table.add_column("Type", style="cyan") - containers_table.add_column("Container", style="white") - - # Add static containers - for container in self._static_containers: - containers_table.add_row("Static", str(container)) - - # Add dynamic containers - for container in self._dynamic_containers: - containers_table.add_row("Dynamic", str(container)) - - if not self._static_containers and not self._dynamic_containers: - containers_table.add_row("", "[dim]No containers registered[/dim]") - - # Add skill states section - states_table = self._skill_state.table() - states_table.show_edge = False - states_table.box = None - - # Combine into main table - table.add_column("Section", style="bold") - table.add_column("Details", style="none") - table.add_row("Containers", containers_table) - table.add_row("Skills", states_table) - - # Render to string with title above - with console.capture() as capture: - console.print(Text(" SkillCoordinator", style="bold blue")) - console.print(table) - return capture.get().strip() - - # Given skillcontainers can run remotely, we are - # Caching available skills from static containers - # - # Dynamic containers will be queried at runtime via - # .skills() method - def register_skills(self, container: SkillContainer) -> None: - self.empty = False - if not container.dynamic_skills(): - logger.info(f"Registering static skill container, {container}") - self._static_containers.append(container) - for name, skill_config in container.skills().items(): - self._skills[name] = skill_config.bind(getattr(container, name)) - else: - logger.info(f"Registering dynamic skill container, {container}") - self._dynamic_containers.append(container) - - def get_skill_config(self, skill_name: str) -> SkillConfig | None: - skill_config = self._skills.get(skill_name) - if not skill_config: - skill_config = self.skills().get(skill_name) - return skill_config - - def skills(self) -> dict[str, SkillConfig]: - # Static container skilling is already cached - all_skills: dict[str, SkillConfig] = {**self._skills} - - # Then aggregate skills from dynamic containers - for container in self._dynamic_containers: - for skill_name, skill_config in container.skills().items(): - all_skills[skill_name] = skill_config.bind(getattr(container, skill_name)) - - return all_skills diff --git a/dimos/protocol/skill/schema.py b/dimos/protocol/skill/schema.py deleted file mode 100644 index 3b265f9c1b..0000000000 --- a/dimos/protocol/skill/schema.py +++ /dev/null @@ -1,103 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import inspect -from typing import Union, get_args, get_origin - - -def python_type_to_json_schema(python_type) -> dict: # type: ignore[no-untyped-def, type-arg] - """Convert Python type annotations to JSON Schema format.""" - # Handle None/NoneType - if python_type is type(None) or python_type is None: - return {"type": "null"} - - # Handle Union types (including Optional) - origin = get_origin(python_type) - if origin is Union: - args = get_args(python_type) - # Handle Optional[T] which is Union[T, None] - if len(args) == 2 and type(None) in args: - non_none_type = args[0] if args[1] is type(None) else args[1] - schema = python_type_to_json_schema(non_none_type) - # For OpenAI function calling, we don't use anyOf for optional params - return schema - else: - # For other Union types, use anyOf - return {"anyOf": [python_type_to_json_schema(arg) for arg in args]} - - # Handle List/list types - if origin in (list, list): - args = get_args(python_type) - if args: - return {"type": "array", "items": python_type_to_json_schema(args[0])} - return {"type": "array"} - - # Handle Dict/dict types - if origin in (dict, dict): - return {"type": "object"} - - # Handle basic types - type_map = { - str: {"type": "string"}, - int: {"type": "integer"}, - float: {"type": "number"}, - bool: {"type": "boolean"}, - list: {"type": "array"}, - dict: {"type": "object"}, - } - - return type_map.get(python_type, {"type": "string"}) - - -def function_to_schema(func) -> dict: # type: ignore[no-untyped-def, type-arg] - """Convert a function to OpenAI function schema format.""" - try: - signature = inspect.signature(func) - except ValueError as e: - raise ValueError(f"Failed to get signature for function {func.__name__}: {e!s}") - - properties = {} - required = [] - - for param_name, param in signature.parameters.items(): - # Skip 'self' parameter for methods - if param_name == "self": - continue - - # Get the type annotation - if param.annotation != inspect.Parameter.empty: - param_schema = python_type_to_json_schema(param.annotation) - else: - # Default to string if no type annotation - param_schema = {"type": "string"} - - # Add description from docstring if available (would need more sophisticated parsing) - properties[param_name] = param_schema - - # Add to required list if no default value - if param.default == inspect.Parameter.empty: - required.append(param_name) - - return { - "type": "function", - "function": { - "name": func.__name__, - "description": (func.__doc__ or "").strip(), - "parameters": { - "type": "object", - "properties": properties, - "required": required, - }, - }, - } diff --git a/dimos/protocol/skill/skill.py b/dimos/protocol/skill/skill.py deleted file mode 100644 index 33e71a590b..0000000000 --- a/dimos/protocol/skill/skill.py +++ /dev/null @@ -1,250 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import asyncio -from collections.abc import Callable -from concurrent.futures import ThreadPoolExecutor -from dataclasses import dataclass -from typing import Any, ParamSpec, TypeVar, cast - -# from dimos.core.core import rpc -from dimos.protocol.skill.comms import LCMSkillComms, SkillCommsSpec -from dimos.protocol.skill.schema import function_to_schema -from dimos.protocol.skill.type import ( - MsgType, - Output, - Reducer, - Return, - SkillConfig, - SkillMsg, - Stream, -) - -# skill is a decorator that allows us to specify a skill behaviour for a function. -# -# there are several parameters that can be specified: -# - ret: how to return the value from the skill, can be one of: -# -# Return.none: doesn't return anything to an agent -# Return.passive: doesn't schedule an agent call but -# returns the value to the agent when agent is called -# Return.call_agent: calls the agent with the value, scheduling an agent call -# -# - stream: if the skill streams values, it can behave in several ways: -# -# Stream.none: no streaming, skill doesn't emit any values -# Stream.passive: doesn't schedule an agent call upon emitting a value, -# returns the streamed value to the agent when agent is called -# Stream.call_agent: calls the agent with every value emitted, scheduling an agent call -# -# - reducer: defines an optional strategy for passive streams and how we collapse potential -# multiple values into something meaningful for the agent -# -# Reducer.none: no reduction, every emitted value is returned to the agent -# Reducer.latest: only the latest value is returned to the agent -# Reducer.average: assumes the skill emits a number, -# the average of all values is returned to the agent - - -P = ParamSpec("P") -R = TypeVar("R") - - -def rpc(fn: Callable[P, R]) -> Callable[P, R]: - fn.__rpc__ = True # type: ignore[attr-defined] - return fn - - -def skill( - reducer: Reducer = Reducer.latest, # type: ignore[assignment] - stream: Stream = Stream.none, - ret: Return = Return.call_agent, - output: Output = Output.standard, - hide_skill: bool = False, -) -> Callable[[Callable[P, R]], Callable[P, R]]: - def decorator(f: Callable[P, R]) -> Callable[P, R]: - def wrapper(self: Any, *args: P.args, **kwargs: P.kwargs) -> R: - skill = f"{f.__name__}" - - call_id = kwargs.get("call_id", None) - if call_id: - del kwargs["call_id"] - - return cast("R", self.call_skill(call_id, skill, args, kwargs)) # type: ignore[attr-defined] - # def run_function(): - # return self.call_skill(call_id, skill, args, kwargs) - # - # thread = threading.Thread(target=run_function) - # thread.start() - # return None - - return f(self, *args, **kwargs) - - # sig = inspect.signature(f) - # params = list(sig.parameters.values()) - # if params and params[0].name == "self": - # params = params[1:] # Remove first parameter 'self' - # wrapper.__signature__ = sig.replace(parameters=params) - - skill_config = SkillConfig( - name=f.__name__, - reducer=reducer, # type: ignore[arg-type] - stream=stream, - # if stream is passive, ret must be passive too - ret=ret.passive if stream == Stream.passive else ret, - output=output, - schema=function_to_schema(f), - hide_skill=hide_skill, - ) - - wrapper.__rpc__ = True # type: ignore[attr-defined] - wrapper._skill_config = skill_config # type: ignore[attr-defined] - wrapper.__name__ = f.__name__ # Preserve original function name - wrapper.__doc__ = f.__doc__ # Preserve original docstring - return cast("Callable[P, R]", wrapper) - - return decorator - - -@dataclass -class SkillContainerConfig: - skill_transport: type[SkillCommsSpec] = LCMSkillComms - - -def threaded(f: Callable[..., Any]) -> Callable[..., None]: - """Decorator to run a function in a thread pool.""" - - def wrapper(self, *args, **kwargs): # type: ignore[no-untyped-def] - if self._skill_thread_pool is None: - self._skill_thread_pool = ThreadPoolExecutor( - max_workers=50, thread_name_prefix="skill_worker" - ) - self._skill_thread_pool.submit(f, self, *args, **kwargs) - return None - - return wrapper - - -# Inherited by any class that wants to provide skills -# (This component works standalone but commonly used by DimOS modules) -# -# Hosts the function execution and handles correct publishing of skill messages -# according to the individual skill decorator configuration -# -# - It allows us to specify a communication layer for skills (LCM for now by default) -# - introspection of available skills via the `skills` RPC method -# - ability to provide dynamic context dependant skills with dynamic_skills flag -# for this you'll need to override the `skills` method to return a dynamic set of skills -# SkillCoordinator will call this method to get the skills available upon every request to -# the agent - - -class SkillContainer: - skill_transport_class: type[SkillCommsSpec] = LCMSkillComms - _skill_thread_pool: ThreadPoolExecutor | None = None - _skill_transport: SkillCommsSpec | None = None - - @rpc - def dynamic_skills(self) -> bool: - return False - - def __str__(self) -> str: - return f"SkillContainer({self.__class__.__name__})" - - @rpc - def stop(self) -> None: - if self._skill_transport: - self._skill_transport.stop() - self._skill_transport = None - - if self._skill_thread_pool: - self._skill_thread_pool.shutdown(wait=True) - self._skill_thread_pool = None - - # Continue the MRO chain if there's a parent stop() method - if hasattr(super(), "stop"): - super().stop() # type: ignore[misc] - - # TODO: figure out standard args/kwargs passing format, - # use same interface as skill coordinator call_skill method - @threaded - def call_skill( - self, call_id: str, skill_name: str, args: tuple[Any, ...], kwargs: dict[str, Any] - ) -> None: - f = getattr(self, skill_name, None) - - if f is None: - raise ValueError(f"Function '{skill_name}' not found in {self.__class__.__name__}") - - config = getattr(f, "_skill_config", None) - if config is None: - raise ValueError(f"Function '{skill_name}' in {self.__class__.__name__} is not a skill") - - # we notify the skill transport about the start of the skill call - self.skill_transport.publish(SkillMsg(call_id, skill_name, None, type=MsgType.start)) - - try: - val = f(*args, **kwargs) - - # check if the skill returned a coroutine, if it is, block until it resolves - if isinstance(val, asyncio.Future): - val = asyncio.run(val) # type: ignore[arg-type] - - # check if the skill is a generator, if it is, we need to iterate over it - if hasattr(val, "__iter__") and not isinstance(val, str): - last_value = None - for v in val: - last_value = v - self.skill_transport.publish( - SkillMsg(call_id, skill_name, v, type=MsgType.stream) - ) - self.skill_transport.publish( - SkillMsg(call_id, skill_name, last_value, type=MsgType.ret) - ) - - else: - self.skill_transport.publish(SkillMsg(call_id, skill_name, val, type=MsgType.ret)) - - except Exception as e: - import traceback - - formatted_traceback = "".join(traceback.TracebackException.from_exception(e).format()) - - self.skill_transport.publish( - SkillMsg( - call_id, - skill_name, - {"msg": str(e), "traceback": formatted_traceback}, - type=MsgType.error, - ) - ) - - @rpc - def skills(self) -> dict[str, SkillConfig]: - # Avoid recursion by excluding this property itself - # Also exclude known properties that shouldn't be accessed - excluded = {"skills", "tf", "rpc", "skill_transport"} - return { - name: getattr(self, name)._skill_config - for name in dir(self) - if not name.startswith("_") - and name not in excluded - and hasattr(getattr(self, name), "_skill_config") - } - - @property - def skill_transport(self) -> SkillCommsSpec: - if self._skill_transport is None: - self._skill_transport = self.skill_transport_class() - return self._skill_transport diff --git a/dimos/protocol/skill/test_coordinator.py b/dimos/protocol/skill/test_coordinator.py deleted file mode 100644 index bd00ea69c2..0000000000 --- a/dimos/protocol/skill/test_coordinator.py +++ /dev/null @@ -1,163 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -import asyncio -from collections.abc import Generator -import datetime -import time - -import pytest # type: ignore[import-not-found] - -from dimos.core import Module, rpc -from dimos.msgs.sensor_msgs import Image -from dimos.protocol.skill.coordinator import SkillCoordinator -from dimos.protocol.skill.skill import skill -from dimos.protocol.skill.type import Output, Reducer, Stream -from dimos.utils.data import get_data - - -class SkillContainerTest(Module): - @rpc - def start(self) -> None: - super().start() - - @rpc - def stop(self) -> None: - super().stop() - - @skill() - def add(self, x: int, y: int) -> int: - """adds x and y.""" - time.sleep(2) - return x + y - - @skill() - def delayadd(self, x: int, y: int) -> int: - """waits 0.3 seconds before adding x and y.""" - time.sleep(0.3) - return x + y - - @skill(stream=Stream.call_agent, reducer=Reducer.all) # type: ignore[arg-type] - def counter(self, count_to: int, delay: float | None = 0.05) -> Generator[int, None, None]: - """Counts from 1 to count_to, with an optional delay between counts.""" - for i in range(1, count_to + 1): - if delay is not None and delay > 0: - time.sleep(delay) - yield i - - @skill(stream=Stream.passive, reducer=Reducer.sum) # type: ignore[arg-type] - def counter_passive_sum( - self, count_to: int, delay: float | None = 0.05 - ) -> Generator[int, None, None]: - """Counts from 1 to count_to, with an optional delay between counts.""" - for i in range(1, count_to + 1): - if delay is not None and delay > 0: - time.sleep(delay) - yield i - - @skill(stream=Stream.passive, reducer=Reducer.latest) # type: ignore[arg-type] - def current_time(self, frequency: float | None = 10) -> Generator[str, None, None]: - """Provides current time.""" - while True: - yield str(datetime.datetime.now()) - if frequency is not None: - time.sleep(1 / frequency) - - @skill(stream=Stream.passive, reducer=Reducer.latest) # type: ignore[arg-type] - def uptime_seconds(self, frequency: float | None = 10) -> Generator[float, None, None]: - """Provides current uptime.""" - start_time = datetime.datetime.now() - while True: - yield (datetime.datetime.now() - start_time).total_seconds() - if frequency is not None: - time.sleep(1 / frequency) - - @skill() - def current_date(self, frequency: float | None = 10) -> str: - """Provides current date.""" - return str(datetime.datetime.now()) - - @skill(output=Output.image) - def take_photo(self) -> Image: - """Takes a camera photo""" - print("Taking photo...") - img = Image.from_file(str(get_data("cafe-smol.jpg"))) - print("Photo taken.") - return img - - -@pytest.mark.integration -@pytest.mark.asyncio # type: ignore[untyped-decorator] -async def test_coordinator_parallel_calls() -> None: - container = SkillContainerTest() - skillCoordinator = SkillCoordinator() - skillCoordinator.register_skills(container) - - skillCoordinator.start() - skillCoordinator.call_skill("test-call-0", "add", {"args": [0, 2]}) - - time.sleep(0.1) - - cnt = 0 - while await skillCoordinator.wait_for_updates(1): - print(skillCoordinator) - - skillstates = skillCoordinator.generate_snapshot() - - skill_id = f"test-call-{cnt}" - tool_msg = skillstates[skill_id].agent_encode() - assert tool_msg.content == cnt + 2 # type: ignore[union-attr] - - cnt += 1 - if cnt < 5: - skillCoordinator.call_skill( - f"test-call-{cnt}-delay", - "delayadd", - {"args": [cnt, 2]}, - ) - skillCoordinator.call_skill( - f"test-call-{cnt}", - "add", - {"args": [cnt, 2]}, - ) - - await asyncio.sleep(0.1 * cnt) - - container.stop() - skillCoordinator.stop() - - -@pytest.mark.integration -@pytest.mark.asyncio # type: ignore[untyped-decorator] -async def test_coordinator_generator() -> None: - container = SkillContainerTest() - skillCoordinator = SkillCoordinator() - skillCoordinator.register_skills(container) - skillCoordinator.start() - - # here we call a skill that generates a sequence of messages - skillCoordinator.call_skill("test-gen-0", "counter", {"args": [10]}) - skillCoordinator.call_skill("test-gen-1", "counter_passive_sum", {"args": [5]}) - skillCoordinator.call_skill("test-gen-2", "take_photo", {"args": []}) - - # periodically agent is stopping it's thinking cycle and asks for updates - while await skillCoordinator.wait_for_updates(2): - print(skillCoordinator) - agent_update = skillCoordinator.generate_snapshot(clear=True) - print(agent_update) - await asyncio.sleep(0.125) - - print("coordinator loop finished") - print(skillCoordinator) - container.stop() - skillCoordinator.stop() diff --git a/dimos/protocol/skill/test_utils.py b/dimos/protocol/skill/test_utils.py deleted file mode 100644 index d9fe9f6f91..0000000000 --- a/dimos/protocol/skill/test_utils.py +++ /dev/null @@ -1,87 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from dimos.protocol.skill.utils import interpret_tool_call_args - - -def test_list() -> None: - args, kwargs = interpret_tool_call_args([1, 2, 3]) - assert args == [1, 2, 3] - assert kwargs == {} - - -def test_none() -> None: - args, kwargs = interpret_tool_call_args(None) - assert args == [] - assert kwargs == {} - - -def test_none_nested() -> None: - args, kwargs = interpret_tool_call_args({"args": None}) - assert args == [] - assert kwargs == {} - - -def test_non_dict() -> None: - args, kwargs = interpret_tool_call_args("test") - assert args == ["test"] - assert kwargs == {} - - -def test_dict_with_args_and_kwargs() -> None: - args, kwargs = interpret_tool_call_args({"args": [1, 2], "kwargs": {"key": "value"}}) - assert args == [1, 2] - assert kwargs == {"key": "value"} - - -def test_dict_with_only_kwargs() -> None: - args, kwargs = interpret_tool_call_args({"kwargs": {"a": 1, "b": 2}}) - assert args == [] - assert kwargs == {"a": 1, "b": 2} - - -def test_dict_as_kwargs() -> None: - args, kwargs = interpret_tool_call_args({"x": 10, "y": 20}) - assert args == [] - assert kwargs == {"x": 10, "y": 20} - - -def test_dict_with_only_args_first_pass() -> None: - args, kwargs = interpret_tool_call_args({"args": [5, 6, 7]}) - assert args == [5, 6, 7] - assert kwargs == {} - - -def test_dict_with_only_args_nested() -> None: - args, kwargs = interpret_tool_call_args({"args": {"inner": "value"}}) - assert args == [] - assert kwargs == {"inner": "value"} - - -def test_empty_list() -> None: - args, kwargs = interpret_tool_call_args([]) - assert args == [] - assert kwargs == {} - - -def test_empty_dict() -> None: - args, kwargs = interpret_tool_call_args({}) - assert args == [] - assert kwargs == {} - - -def test_integer() -> None: - args, kwargs = interpret_tool_call_args(42) - assert args == [42] - assert kwargs == {} diff --git a/dimos/protocol/skill/type.py b/dimos/protocol/skill/type.py deleted file mode 100644 index 7881dcd94e..0000000000 --- a/dimos/protocol/skill/type.py +++ /dev/null @@ -1,272 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -from __future__ import annotations - -from collections.abc import Callable -from dataclasses import dataclass -from enum import Enum -import time -from typing import Any, Generic, Literal, TypeVar - -from dimos.types.timestamped import Timestamped -from dimos.utils.generic import truncate_display_string - -# This file defines protocol messages used for communication between skills and agents - - -class Output(Enum): - standard = 0 - human = 1 - image = 2 # this is same as separate_message, but maybe clearer for users - - -class Stream(Enum): - # no streaming - none = 0 - # passive stream, doesn't schedule an agent call, but returns the value to the agent - passive = 1 - # calls the agent with every value emitted, schedules an agent call - call_agent = 2 - - -class Return(Enum): - # doesn't return anything to an agent - none = 0 - # returns the value to the agent, but doesn't schedule an agent call - passive = 1 - # calls the agent with the value, scheduling an agent call - call_agent = 2 - # calls the function to get a value, when the agent is being called - callback = 3 # TODO: this is a work in progress, not implemented yet - - -@dataclass -class SkillConfig: - name: str - reducer: ReducerF - stream: Stream - ret: Return - output: Output - schema: dict[str, Any] - f: Callable | None = None # type: ignore[type-arg] - autostart: bool = False - hide_skill: bool = False - - def bind(self, f: Callable) -> SkillConfig: # type: ignore[type-arg] - self.f = f - return self - - def call(self, call_id, *args, **kwargs) -> Any: # type: ignore[no-untyped-def] - if self.f is None: - raise ValueError( - "Function is not bound to the SkillConfig. This should be called only within AgentListener." - ) - - return self.f(*args, **kwargs, call_id=call_id) - - def __str__(self) -> str: - parts = [f"name={self.name}"] - - # Only show reducer if stream is not none (streaming is happening) - if self.stream != Stream.none: - parts.append(f"stream={self.stream.name}") - - # Always show return mode - parts.append(f"ret={self.ret.name}") - return f"Skill({', '.join(parts)})" - - -class MsgType(Enum): - pending = 0 - start = 1 - stream = 2 - reduced_stream = 3 - ret = 4 - error = 5 - - -M = TypeVar("M", bound="MsgType") - - -def maybe_encode(something: Any) -> str: - if hasattr(something, "agent_encode"): - return something.agent_encode() # type: ignore[no-any-return] - return something # type: ignore[no-any-return] - - -class SkillMsg(Timestamped, Generic[M]): - ts: float - type: M - call_id: str - skill_name: str - content: str | int | float | dict | list # type: ignore[type-arg] - - def __init__( - self, - call_id: str, - skill_name: str, - content: Any, - type: M, - ) -> None: - self.ts = time.time() - self.call_id = call_id - self.skill_name = skill_name - # any tool output can be a custom type that knows how to encode itself - # like a costmap, path, transform etc could be translatable into strings - - self.content = maybe_encode(content) - self.type = type - - @property - def end(self) -> bool: - return self.type == MsgType.ret or self.type == MsgType.error - - @property - def start(self) -> bool: - return self.type == MsgType.start - - def __str__(self) -> str: # type: ignore[return] - time_ago = time.time() - self.ts - - if self.type == MsgType.start: - return f"Start({time_ago:.1f}s ago)" - if self.type == MsgType.ret: - return f"Ret({time_ago:.1f}s ago, val={truncate_display_string(self.content)})" - if self.type == MsgType.error: - return f"Error({time_ago:.1f}s ago, val={truncate_display_string(self.content)})" - if self.type == MsgType.pending: - return f"Pending({time_ago:.1f}s ago)" - if self.type == MsgType.stream: - return f"Stream({time_ago:.1f}s ago, val={truncate_display_string(self.content)})" - if self.type == MsgType.reduced_stream: - return f"Stream({time_ago:.1f}s ago, val={truncate_display_string(self.content)})" - - -# typing looks complex but it's a standard reducer function signature, using SkillMsgs -# (Optional[accumulator], msg) -> accumulator -ReducerF = Callable[ - [SkillMsg[Literal[MsgType.reduced_stream]] | None, SkillMsg[Literal[MsgType.stream]]], - SkillMsg[Literal[MsgType.reduced_stream]], -] - - -C = TypeVar("C") # content type -A = TypeVar("A") # accumulator type -# define a naive reducer function type that's generic in terms of the accumulator type -SimpleReducerF = Callable[[A | None, C], A] - - -def make_reducer(simple_reducer: SimpleReducerF) -> ReducerF: # type: ignore[type-arg] - """ - Converts a naive reducer function into a standard reducer function. - The naive reducer function should accept an accumulator and a message, - and return the updated accumulator. - """ - - def reducer( - accumulator: SkillMsg[Literal[MsgType.reduced_stream]] | None, - msg: SkillMsg[Literal[MsgType.stream]], - ) -> SkillMsg[Literal[MsgType.reduced_stream]]: - # Extract the content from the accumulator if it exists - acc_value = accumulator.content if accumulator else None - - # Apply the simple reducer to get the new accumulated value - new_value = simple_reducer(acc_value, msg.content) - - # Wrap the result in a SkillMsg with reduced_stream type - return SkillMsg( - call_id=msg.call_id, - skill_name=msg.skill_name, - content=new_value, - type=MsgType.reduced_stream, - ) - - return reducer - - -# just a convinience class to hold reducer functions -def _make_skill_msg( - msg: SkillMsg[Literal[MsgType.stream]], content: Any -) -> SkillMsg[Literal[MsgType.reduced_stream]]: - """Helper to create a reduced stream message with new content.""" - return SkillMsg( - call_id=msg.call_id, - skill_name=msg.skill_name, - content=content, - type=MsgType.reduced_stream, - ) - - -def sum_reducer( - accumulator: SkillMsg[Literal[MsgType.reduced_stream]] | None, - msg: SkillMsg[Literal[MsgType.stream]], -) -> SkillMsg[Literal[MsgType.reduced_stream]]: - """Sum reducer that adds values together.""" - acc_value = accumulator.content if accumulator else None - new_value = acc_value + msg.content if acc_value else msg.content # type: ignore[operator] - return _make_skill_msg(msg, new_value) - - -def latest_reducer( - accumulator: SkillMsg[Literal[MsgType.reduced_stream]] | None, - msg: SkillMsg[Literal[MsgType.stream]], -) -> SkillMsg[Literal[MsgType.reduced_stream]]: - """Latest reducer that keeps only the most recent value.""" - return _make_skill_msg(msg, msg.content) - - -def all_reducer( - accumulator: SkillMsg[Literal[MsgType.reduced_stream]] | None, - msg: SkillMsg[Literal[MsgType.stream]], -) -> SkillMsg[Literal[MsgType.reduced_stream]]: - """All reducer that collects all values into a list.""" - acc_value = accumulator.content if accumulator else None - new_value = [*acc_value, msg.content] if acc_value else [msg.content] # type: ignore[misc] - return _make_skill_msg(msg, new_value) - - -def accumulate_list( - accumulator: SkillMsg[Literal[MsgType.reduced_stream]] | None, - msg: SkillMsg[Literal[MsgType.stream]], -) -> SkillMsg[Literal[MsgType.reduced_stream]]: - """All reducer that collects all values into a list.""" - acc_value = accumulator.content if accumulator else [] - return _make_skill_msg(msg, acc_value + msg.content) # type: ignore[operator] - - -def accumulate_dict( - accumulator: SkillMsg[Literal[MsgType.reduced_stream]] | None, - msg: SkillMsg[Literal[MsgType.stream]], -) -> SkillMsg[Literal[MsgType.reduced_stream]]: - """All reducer that collects all values into a list.""" - acc_value = accumulator.content if accumulator else {} - return _make_skill_msg(msg, {**acc_value, **msg.content}) # type: ignore[dict-item] - - -def accumulate_string( - accumulator: SkillMsg[Literal[MsgType.reduced_stream]] | None, - msg: SkillMsg[Literal[MsgType.stream]], -) -> SkillMsg[Literal[MsgType.reduced_stream]]: - """All reducer that collects all values into a list.""" - acc_value = accumulator.content if accumulator else "" - return _make_skill_msg(msg, acc_value + "\n" + msg.content) # type: ignore[operator] - - -class Reducer: - sum = sum_reducer - latest = latest_reducer - all = all_reducer - accumulate_list = accumulate_list - accumulate_dict = accumulate_dict - string = accumulate_string diff --git a/dimos/protocol/skill/utils.py b/dimos/protocol/skill/utils.py deleted file mode 100644 index 278134c525..0000000000 --- a/dimos/protocol/skill/utils.py +++ /dev/null @@ -1,41 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from typing import Any - - -def interpret_tool_call_args( - args: Any, first_pass: bool = True -) -> tuple[list[Any], dict[str, Any]]: - """ - Agents sometimes produce bizarre calls. This tries to interpret the args better. - """ - - if isinstance(args, list): - return args, {} - if args is None: - return [], {} - if not isinstance(args, dict): - return [args], {} - if args.keys() == {"args", "kwargs"}: - return args["args"], args["kwargs"] - if args.keys() == {"kwargs"}: - return [], args["kwargs"] - if args.keys() != {"args"}: - return [], args - - if first_pass: - return interpret_tool_call_args(args["args"], first_pass=False) - - return [], args diff --git a/dimos/protocol/tf/test_tf.py b/dimos/protocol/tf/test_tf.py index 0b5b332c3d..bdbd808cbb 100644 --- a/dimos/protocol/tf/test_tf.py +++ b/dimos/protocol/tf/test_tf.py @@ -48,6 +48,7 @@ def test_tf_ros_example() -> None: time.sleep(0.2) end_effector_global_pose = tf.get("base_link", "end_effector") + assert end_effector_global_pose is not None assert end_effector_global_pose.translation.x == pytest.approx(1.366, abs=1e-3) assert end_effector_global_pose.translation.y == pytest.approx(0.366, abs=1e-3) @@ -116,6 +117,7 @@ def test_tf_main() -> None: # The chain should compose: world->robot (1,2,3) + robot->sensor (0.5,0,0.2) # Expected translation: (1.5, 2.0, 3.2) + assert chain_transform is not None assert abs(chain_transform.translation.x - 1.5) < 0.001 assert abs(chain_transform.translation.y - 2.0) < 0.001 assert abs(chain_transform.translation.z - 3.2) < 0.001 @@ -163,12 +165,14 @@ def test_tf_main() -> None: # if you have "diagon" https://diagon.arthursonzogni.com/ installed you can draw a graph print(broadcaster.graph()) + assert world_object is not None assert abs(world_object.translation.x - 1.5) < 0.001 assert abs(world_object.translation.y - 3.0) < 0.001 assert abs(world_object.translation.z - 3.2) < 0.001 # this doesn't work atm robot_to_charger = broadcaster.get("robot", "charger") + assert robot_to_charger is not None # Expected: robot->world->charger print(f"robot_to_charger translation: {robot_to_charger.translation}") @@ -196,7 +200,7 @@ def test_add_transform(self) -> None: buffer.add(transform) assert len(buffer) == 1 - assert buffer[0] == transform + assert buffer.first() == transform def test_get(self) -> None: buffer = TBuffer() @@ -250,7 +254,9 @@ def test_buffer_pruning(self) -> None: # Old transform should be pruned assert len(buffer) == 1 - assert buffer[0].translation.x == 2.0 + first = buffer.first() + assert first is not None + assert first.translation.x == 2.0 class TestMultiTBuffer: diff --git a/dimos/protocol/tf/tf.py b/dimos/protocol/tf/tf.py index 44d5303a05..825e89fc8c 100644 --- a/dimos/protocol/tf/tf.py +++ b/dimos/protocol/tf/tf.py @@ -20,12 +20,12 @@ from functools import reduce from typing import TypeVar -from dimos.msgs.geometry_msgs import Transform +from dimos.memory.timeseries.inmemory import InMemoryStore +from dimos.msgs.geometry_msgs import PoseStamped, Transform from dimos.msgs.tf2_msgs import TFMessage from dimos.protocol.pubsub.impl.lcmpubsub import LCM, Topic from dimos.protocol.pubsub.spec import PubSub from dimos.protocol.service.lcmservice import Service # type: ignore[attr-defined] -from dimos.types.timestamped import TimestampedCollection CONFIG = TypeVar("CONFIG") @@ -52,13 +52,13 @@ def get_frames(self) -> set[str]: return set() @abstractmethod - def get( # type: ignore[no-untyped-def] + def get( self, parent_frame: str, child_frame: str, time_point: float | None = None, time_tolerance: float | None = None, - ): ... + ) -> Transform | None: ... def receive_transform(self, *args: Transform) -> None: ... @@ -71,64 +71,44 @@ def receive_tfmessage(self, msg: TFMessage) -> None: TopicT = TypeVar("TopicT") -# stores a single transform -class TBuffer(TimestampedCollection[Transform]): +class TBuffer(InMemoryStore[Transform]): def __init__(self, buffer_size: float = 10.0) -> None: super().__init__() self.buffer_size = buffer_size def add(self, transform: Transform) -> None: - super().add(transform) - self._prune_old_transforms(transform.ts) - - def _prune_old_transforms(self, current_time) -> None: # type: ignore[no-untyped-def] - if not self._items: - return - - cutoff_time = current_time - self.buffer_size - - while self._items and self._items[0].ts < cutoff_time: - self._items.pop(0) + self.save(transform) + self.prune_old(transform.ts - self.buffer_size) def get(self, time_point: float | None = None, time_tolerance: float = 1.0) -> Transform | None: """Get transform at specified time or latest if no time given.""" if time_point is None: - # Return the latest transform - return self[-1] if len(self) > 0 else None - + return self.last() return self.find_closest(time_point, time_tolerance) def __str__(self) -> str: - if not self._items: + if len(self) == 0: return "TBuffer(empty)" - # Get unique frame info from the transforms - frame_pairs = set() - if self._items: - frame_pairs.add((self._items[0].frame_id, self._items[0].child_frame_id)) - + first_item = self.first() time_range = self.time_range() - if time_range: + if time_range and first_item: from dimos.types.timestamped import to_human_readable start_time = to_human_readable(time_range[0]) end_time = to_human_readable(time_range[1]) duration = time_range[1] - time_range[0] - frame_str = ( - f"{self._items[0].frame_id} -> {self._items[0].child_frame_id}" - if self._items - else "unknown" - ) + frame_str = f"{first_item.frame_id} -> {first_item.child_frame_id}" return ( f"TBuffer(" f"{frame_str}, " - f"{len(self._items)} msgs, " + f"{len(self)} msgs, " f"{duration:.2f}s [{start_time} - {end_time}])" ) - return f"TBuffer({len(self._items)} msgs)" + return f"TBuffer({len(self)} msgs)" # stores multiple transform buffers @@ -334,6 +314,18 @@ def get( ) -> Transform | None: return super().get(parent_frame, child_frame, time_point, time_tolerance) + def get_pose( + self, + parent_frame: str, + child_frame: str, + time_point: float | None = None, + time_tolerance: float | None = None, + ) -> PoseStamped | None: + tf = self.get(parent_frame, child_frame, time_point, time_tolerance) + if not tf: + return None + return tf.to_pose() + def receive_msg(self, msg: TFMessage, topic: Topic) -> None: self.receive_tfmessage(msg) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 294d2a19fc..a041a4f7aa 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -31,6 +31,8 @@ "coordinator-velocity-xarm6": "dimos.control.blueprints:coordinator_velocity_xarm6", "coordinator-xarm6": "dimos.control.blueprints:coordinator_xarm6", "coordinator-xarm7": "dimos.control.blueprints:coordinator_xarm7", + "demo-agent": "dimos.agents.demo_agent:demo_agent", + "demo-agent-camera": "dimos.agents.demo_agent:demo_agent_camera", "demo-camera": "dimos.hardware.sensors.camera.module:demo_camera", "demo-error-on-name-conflicts": "dimos.robot.unitree.demo_error_on_name_conflicts:demo_error_on_name_conflicts", "demo-google-maps-skill": "dimos.agents.skills.demo_google_maps_skill:demo_google_maps_skill", @@ -74,6 +76,7 @@ all_modules = { + "agent": "dimos.agents.agent", "arm_teleop_module": "dimos.teleop.quest.quest_extensions", "camera_module": "dimos.hardware.sensors.camera.module", "cartesian_motion_controller": "dimos.manipulation.control.servo_control.cartesian_motion_controller", @@ -93,10 +96,8 @@ "google_maps_skill": "dimos.agents.skills.google_maps_skill_container", "gps_nav_skill": "dimos.agents.skills.gps_nav_skill", "grasping_module": "dimos.manipulation.grasping.grasping", - "human_input": "dimos.agents.cli.human", "joint_trajectory_controller": "dimos.manipulation.control.trajectory_controller.joint_trajectory_controller", "keyboard_teleop": "dimos.robot.unitree.keyboard_teleop", - "llm_agent": "dimos.agents.agent", "manipulation_module": "dimos.manipulation.manipulation_module", "mapper": "dimos.robot.unitree.type.map", "mid360_module": "dimos.hardware.sensors.lidar.livox.module", @@ -123,7 +124,7 @@ "vlm_stream_tester": "dimos.agents.vlm_stream_tester", "voxel_mapper": "dimos.mapping.voxels", "wavefront_frontier_explorer": "dimos.navigation.frontier_exploration.wavefront_frontier_goal_selector", - "web_input": "dimos.agents.cli.web", + "web_input": "dimos.agents.web_human_input", "websocket_vis": "dimos.web.websocket_vis.websocket_vis_module", "zed_camera": "dimos.hardware.sensors.camera.zed.camera", } diff --git a/dimos/robot/cli/dimos.py b/dimos/robot/cli/dimos.py index fc9235a834..c390d3b76c 100644 --- a/dimos/robot/cli/dimos.py +++ b/dimos/robot/cli/dimos.py @@ -17,6 +17,7 @@ import sys from typing import Any, get_args, get_origin +from dotenv import load_dotenv import typer from dimos.core.global_config import GlobalConfig, global_config @@ -29,6 +30,8 @@ no_args_is_help=True, ) +load_dotenv() + def create_dynamic_callback(): # type: ignore[no-untyped-def] fields = GlobalConfig.model_fields @@ -155,15 +158,6 @@ def lcmspy(ctx: typer.Context) -> None: lcmspy_main() -@main.command(context_settings={"allow_extra_args": True, "ignore_unknown_options": True}) -def skillspy(ctx: typer.Context) -> None: - """Skills spy tool for monitoring skills.""" - from dimos.utils.cli.skillspy.skillspy import main as skillspy_main - - sys.argv = ["skillspy", *ctx.args] - skillspy_main() - - @main.command(context_settings={"allow_extra_args": True, "ignore_unknown_options": True}) def agentspy(ctx: typer.Context) -> None: """Agent spy tool for monitoring agents.""" diff --git a/dimos/robot/drone/README.md b/dimos/robot/drone/README.md index fbd7ddf2ae..6e8ceb4d63 100644 --- a/dimos/robot/drone/README.md +++ b/dimos/robot/drone/README.md @@ -265,9 +265,9 @@ Connect Foxglove Studio to `ws://localhost:8765` to see: ## Development ### Adding New Skills -Add to `connection_module.py` with `@skill()` decorator: +Add to `connection_module.py` with `@skill` decorator: ```python -@skill() +@skill def my_skill(self, param: float) -> str: """Skill description for LLM.""" # Implementation diff --git a/dimos/robot/drone/connection_module.py b/dimos/robot/drone/connection_module.py index 92dfb1db38..db5c4ca4cc 100644 --- a/dimos/robot/drone/connection_module.py +++ b/dimos/robot/drone/connection_module.py @@ -24,14 +24,11 @@ from dimos_lcm.std_msgs import String from reactivex.disposable import CompositeDisposable, Disposable -from dimos.core.core import rpc -from dimos.core.module import Module -from dimos.core.stream import In, Out +from dimos.agents.annotation import skill +from dimos.core import In, Module, Out, rpc from dimos.mapping.types import LatLon from dimos.msgs.geometry_msgs import PoseStamped, Quaternion, Transform, Twist, Vector3 from dimos.msgs.sensor_msgs import Image -from dimos.protocol.skill.skill import skill -from dimos.protocol.skill.type import Output from dimos.robot.drone.dji_video_stream import DJIDroneVideoStream from dimos.robot.drone.mavlink_connection import MavlinkConnection from dimos.utils.logging_config import setup_logger @@ -269,7 +266,7 @@ def get_status(self) -> dict[str, Any]: """ return self._status.copy() - @skill() + @skill def move(self, vector: Vector3, duration: float = 0.0) -> None: """Send movement command to drone. @@ -289,7 +286,7 @@ def move(self, vector: Vector3, duration: float = 0.0) -> None: ) self.connection.move(vector, duration) - @skill() + @skill def takeoff(self, altitude: float = 3.0) -> bool: """Takeoff to specified altitude. @@ -303,7 +300,7 @@ def takeoff(self, altitude: float = 3.0) -> bool: return self.connection.takeoff(altitude) return False - @skill() + @skill def land(self) -> bool: """Land the drone. @@ -314,7 +311,7 @@ def land(self) -> bool: return self.connection.land() return False - @skill() + @skill def arm(self) -> bool: """Arm the drone. @@ -325,7 +322,7 @@ def arm(self) -> bool: return self.connection.arm() return False - @skill() + @skill def disarm(self) -> bool: """Disarm the drone. @@ -336,7 +333,7 @@ def disarm(self) -> bool: return self.connection.disarm() return False - @skill() + @skill def set_mode(self, mode: str) -> bool: """Set flight mode. @@ -365,7 +362,7 @@ def move_twist(self, twist: Twist, duration: float = 0.0, lock_altitude: bool = return self.connection.move_twist(twist, duration, lock_altitude) return False - @skill() + @skill def is_flying_to_target(self) -> bool: """Check if drone is currently flying to a GPS target. @@ -376,7 +373,7 @@ def is_flying_to_target(self) -> bool: return self.connection.is_flying_to_target return False - @skill() + @skill def fly_to(self, lat: float, lon: float, alt: float) -> str: """Fly drone to GPS coordinates (blocking operation). @@ -392,7 +389,7 @@ def fly_to(self, lat: float, lon: float, alt: float) -> str: return self.connection.fly_to(lat, lon, alt) return "Failed: No connection to drone" - @skill() + @skill def follow_object( self, object_description: str, duration: float = 120.0 ) -> Generator[str, None, None]: @@ -481,7 +478,7 @@ def stop(self) -> None: # Call parent stop to clean up Module infrastructure (event loop, LCM, disposables, etc.) super().stop() - @skill(output=Output.image) + @skill def observe(self) -> Image | None: """Returns the latest video frame from the drone camera. Use this skill for any visual world queries. diff --git a/dimos/robot/drone/drone.py b/dimos/robot/drone/drone.py index d2e2f3ee0e..8e72d56ed1 100644 --- a/dimos/robot/drone/drone.py +++ b/dimos/robot/drone/drone.py @@ -20,7 +20,6 @@ import functools import logging import os -import time from typing import Any from dimos_lcm.sensor_msgs import CameraInfo @@ -28,8 +27,11 @@ from reactivex import Observable from dimos import core +from dimos.agents.agent import agent from dimos.agents.skills.google_maps_skill_container import GoogleMapsSkillContainer from dimos.agents.skills.osm import OsmSkill +from dimos.agents.web_human_input import web_input +from dimos.core.blueprints import Blueprint, autoconnect from dimos.mapping.types import LatLon from dimos.msgs.geometry_msgs import PoseStamped, Twist, Vector3 from dimos.msgs.sensor_msgs import Image @@ -271,7 +273,8 @@ def get_odom(self) -> PoseStamped | None: @functools.cached_property def gps_position_stream(self) -> Observable[LatLon]: assert self.connection is not None - return self.connection.gps_location.transport.pure_observable() + result: Observable[LatLon] = self.connection.gps_location.transport.pure_observable() + return result def get_status(self) -> dict[str, Any]: """Get drone status. @@ -394,6 +397,51 @@ def stop(self) -> None: logger.info("Drone system stopped") +DRONE_SYSTEM_PROMPT = """\ +You are controlling a DJI drone with MAVLink interface. +You have access to drone control skills you are already flying so only run move_twist, set_mode, and fly_to. +When the user gives commands, use the appropriate skills to control the drone. +Always confirm actions and report results. Send fly_to commands only at above 200 meters altitude to be safe. +Here are some GPS locations to remember +6th and Natoma intersection: 37.78019978319006, -122.40770815020853, +454 Natoma (Office): 37.780967465525244, -122.40688342010769 +5th and mission intersection: 37.782598539339695, -122.40649441875473 +6th and mission intersection: 37.781007204789354, -122.40868447123661""" + + +def drone_agentic( + connection_string: str = "udp:0.0.0.0:14550", + video_port: int = 5600, + outdoor: bool = False, + camera_intrinsics: list[float] | None = None, + system_prompt: str = DRONE_SYSTEM_PROMPT, + model: str = "gpt-4o", +) -> Blueprint: + if camera_intrinsics is None: + camera_intrinsics = [1000.0, 1000.0, 960.0, 540.0] + + return autoconnect( + DroneConnectionModule.blueprint( + connection_string=connection_string, + video_port=video_port, + outdoor=outdoor, + ), + DroneCameraModule.blueprint(camera_intrinsics=camera_intrinsics), + DroneTrackingModule.blueprint(outdoor=outdoor), + WebsocketVisModule.blueprint(), + FoxgloveBridge.blueprint(), + GoogleMapsSkillContainer.blueprint(), + OsmSkill.blueprint(), + agent(system_prompt=system_prompt, model=model), + web_input(), + ).remappings( + [ + (DroneTrackingModule, "video_input", "video"), + (DroneTrackingModule, "cmd_vel", "movecmd_twist"), + ] + ) + + def main() -> None: """Main entry point for drone system.""" import argparse @@ -434,67 +482,13 @@ def main() -> None: pubsub.lcm.autoconf() # type: ignore[attr-defined] - drone = Drone(connection_string=connection, video_port=video_port, outdoor=args.outdoor) - - drone.start() - - print("\n✓ Drone system started successfully!") - print("\nLCM Topics:") - print(" • /drone/odom - Odometry (PoseStamped)") - print(" • /drone/status - Status (String/JSON)") - print(" • /drone/telemetry - Full telemetry (String/JSON)") - print(" • /drone/color_image - RGB Video (Image)") - print(" • /drone/depth_image - Depth estimation (Image)") - print(" • /drone/depth_colorized - Colorized depth (Image)") - print(" • /drone/camera_info - Camera calibration") - print(" • /drone/cmd_vel - Movement commands (Vector3)") - print(" • /drone/tracking_overlay - Object tracking visualization (Image)") - print(" • /drone/tracking_status - Tracking status (String/JSON)") - - from dimos.agents import Agent # type: ignore[attr-defined] - from dimos.agents.cli.human import HumanInput - from dimos.agents.spec import Model, Provider - - assert drone.dimos is not None - human_input = drone.dimos.deploy(HumanInput) # type: ignore[attr-defined] - google_maps = drone.dimos.deploy(GoogleMapsSkillContainer) # type: ignore[attr-defined] - osm_skill = drone.dimos.deploy(OsmSkill) # type: ignore[attr-defined] - - google_maps.gps_location.transport = core.pLCMTransport("/gps_location") - osm_skill.gps_location.transport = core.pLCMTransport("/gps_location") - - agent = Agent( - system_prompt="""You are controlling a DJI drone with MAVLink interface. - You have access to drone control skills you are already flying so only run move_twist, set_mode, and fly_to. - When the user gives commands, use the appropriate skills to control the drone. - Always confirm actions and report results. Send fly_to commands only at above 200 meters altitude to be safe. - Here are some GPS locations to remember - 6th and Natoma intersection: 37.78019978319006, -122.40770815020853, - 454 Natoma (Office): 37.780967465525244, -122.40688342010769 - 5th and mission intersection: 37.782598539339695, -122.40649441875473 - 6th and mission intersection: 37.781007204789354, -122.40868447123661""", - model=Model.GPT_4O, - provider=Provider.OPENAI, # type: ignore[attr-defined] + blueprint = drone_agentic( + connection_string=connection, + video_port=video_port, + outdoor=args.outdoor, ) - agent.register_skills(drone.connection) - agent.register_skills(human_input) - agent.register_skills(google_maps) - agent.register_skills(osm_skill) - agent.run_implicit_skill("human") - - agent.start() - agent.loop_thread() - - # Testing - # from dimos_lcm.geometry_msgs import Twist,Vector3 - # twist = Twist() - # twist.linear = Vector3(-0.5, 0.5, 0.5) - # drone.connection.move_twist(twist, duration=2.0, lock_altitude=True) - # time.sleep(10) - # drone.tracking.track_object("water bottle") - while True: - time.sleep(1) + blueprint.build().loop() if __name__ == "__main__": diff --git a/dimos/robot/unitree/g1/blueprints/agentic/_agentic_skills.py b/dimos/robot/unitree/g1/blueprints/agentic/_agentic_skills.py index 5ca139f968..74ce41f7f1 100644 --- a/dimos/robot/unitree/g1/blueprints/agentic/_agentic_skills.py +++ b/dimos/robot/unitree/g1/blueprints/agentic/_agentic_skills.py @@ -15,15 +15,13 @@ """Agentic skills used by higher-level G1 blueprints.""" -from dimos.agents.agent import llm_agent -from dimos.agents.cli.human import human_input +from dimos.agents.agent import agent from dimos.agents.skills.navigation import navigation_skill from dimos.core.blueprints import autoconnect from dimos.robot.unitree.g1.skill_container import g1_skills _agentic_skills = autoconnect( - llm_agent(), - human_input(), + agent(), navigation_skill(), g1_skills(), ) diff --git a/dimos/robot/unitree/g1/skill_container.py b/dimos/robot/unitree/g1/skill_container.py index 881bf27d6e..7ce9730686 100644 --- a/dimos/robot/unitree/g1/skill_container.py +++ b/dimos/robot/unitree/g1/skill_container.py @@ -19,10 +19,10 @@ import difflib +from dimos.agents.annotation import skill from dimos.core.core import rpc -from dimos.core.skill_module import SkillModule +from dimos.core.module import Module from dimos.msgs.geometry_msgs import Twist, Vector3 -from dimos.protocol.skill.skill import skill from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -61,7 +61,7 @@ } -class UnitreeG1SkillContainer(SkillModule): +class UnitreeG1SkillContainer(Module): rpc_calls: list[str] = [ "G1Connection.move", "G1Connection.publish_request", @@ -75,7 +75,7 @@ def start(self) -> None: def stop(self) -> None: super().stop() - @skill() + @skill def move(self, x: float, y: float = 0.0, yaw: float = 0.0, duration: float = 0.0) -> str: """Move the robot using direct velocity commands. Determine duration required based on user distance instructions. @@ -95,11 +95,11 @@ def move(self, x: float, y: float = 0.0, yaw: float = 0.0, duration: float = 0.0 move_rpc(twist, duration=duration) return f"Started moving with velocity=({x}, {y}, {yaw}) for {duration} seconds" - @skill() + @skill def execute_arm_command(self, command_name: str) -> str: return self._execute_g1_command(_ARM_COMMANDS, 7106, "rt/api/arm/request", command_name) - @skill() + @skill def execute_mode_command(self, command_name: str) -> str: return self._execute_g1_command(_MODE_COMMANDS, 7101, "rt/api/sport/request", command_name) diff --git a/dimos/robot/unitree/go2/blueprints/agentic/_common_agentic.py b/dimos/robot/unitree/go2/blueprints/agentic/_common_agentic.py index cf9a0ae086..817d5e3a7d 100644 --- a/dimos/robot/unitree/go2/blueprints/agentic/_common_agentic.py +++ b/dimos/robot/unitree/go2/blueprints/agentic/_common_agentic.py @@ -13,17 +13,15 @@ # See the License for the specific language governing permissions and # limitations under the License. -from dimos.agents.cli.human import human_input -from dimos.agents.cli.web import web_input from dimos.agents.skills.navigation import navigation_skill from dimos.agents.skills.person_follow import person_follow_skill from dimos.agents.skills.speak_skill import speak_skill +from dimos.agents.web_human_input import web_input from dimos.core.blueprints import autoconnect from dimos.robot.unitree.go2.connection import GO2Connection from dimos.robot.unitree.unitree_skill_container import unitree_skills _common_agentic = autoconnect( - human_input(), navigation_skill(), person_follow_skill(camera_info=GO2Connection.camera_info_static), unitree_skills(), diff --git a/dimos/robot/unitree/go2/blueprints/agentic/unitree_go2_agentic.py b/dimos/robot/unitree/go2/blueprints/agentic/unitree_go2_agentic.py index 0db6d16980..2fb1a4cb74 100644 --- a/dimos/robot/unitree/go2/blueprints/agentic/unitree_go2_agentic.py +++ b/dimos/robot/unitree/go2/blueprints/agentic/unitree_go2_agentic.py @@ -13,14 +13,14 @@ # See the License for the specific language governing permissions and # limitations under the License. -from dimos.agents.agent import llm_agent +from dimos.agents.agent import agent from dimos.core.blueprints import autoconnect from dimos.robot.unitree.go2.blueprints.agentic._common_agentic import _common_agentic from dimos.robot.unitree.go2.blueprints.smart.unitree_go2_spatial import unitree_go2_spatial unitree_go2_agentic = autoconnect( unitree_go2_spatial, - llm_agent(), + agent(), _common_agentic, ) diff --git a/dimos/robot/unitree/go2/blueprints/agentic/unitree_go2_agentic_huggingface.py b/dimos/robot/unitree/go2/blueprints/agentic/unitree_go2_agentic_huggingface.py index 90198c2493..1c998b7495 100644 --- a/dimos/robot/unitree/go2/blueprints/agentic/unitree_go2_agentic_huggingface.py +++ b/dimos/robot/unitree/go2/blueprints/agentic/unitree_go2_agentic_huggingface.py @@ -13,18 +13,14 @@ # See the License for the specific language governing permissions and # limitations under the License. -from dimos.agents.agent import llm_agent -from dimos.agents.spec import Provider +from dimos.agents.agent import agent from dimos.core.blueprints import autoconnect from dimos.robot.unitree.go2.blueprints.agentic._common_agentic import _common_agentic from dimos.robot.unitree.go2.blueprints.smart.unitree_go2_spatial import unitree_go2_spatial unitree_go2_agentic_huggingface = autoconnect( unitree_go2_spatial, - llm_agent( - model="Qwen/Qwen2.5-1.5B-Instruct", - provider=Provider.HUGGINGFACE, # type: ignore[attr-defined] - ), + agent(model="huggingface:Qwen/Qwen2.5-1.5B-Instruct"), _common_agentic, ) diff --git a/dimos/robot/unitree/go2/blueprints/agentic/unitree_go2_agentic_ollama.py b/dimos/robot/unitree/go2/blueprints/agentic/unitree_go2_agentic_ollama.py index 529f50f4ae..6a518ad831 100644 --- a/dimos/robot/unitree/go2/blueprints/agentic/unitree_go2_agentic_ollama.py +++ b/dimos/robot/unitree/go2/blueprints/agentic/unitree_go2_agentic_ollama.py @@ -13,19 +13,15 @@ # See the License for the specific language governing permissions and # limitations under the License. -from dimos.agents.agent import llm_agent +from dimos.agents.agent import agent from dimos.agents.ollama_agent import ollama_installed -from dimos.agents.spec import Provider from dimos.core.blueprints import autoconnect from dimos.robot.unitree.go2.blueprints.agentic._common_agentic import _common_agentic from dimos.robot.unitree.go2.blueprints.smart.unitree_go2_spatial import unitree_go2_spatial unitree_go2_agentic_ollama = autoconnect( unitree_go2_spatial, - llm_agent( - model="qwen3:8b", - provider=Provider.OLLAMA, # type: ignore[attr-defined] - ), + agent(model="ollama:qwen3:8b"), _common_agentic, ).requirements( ollama_installed, diff --git a/dimos/robot/unitree/go2/connection.py b/dimos/robot/unitree/go2/connection.py index ba708ef4df..82aa34c97a 100644 --- a/dimos/robot/unitree/go2/connection.py +++ b/dimos/robot/unitree/go2/connection.py @@ -22,9 +22,9 @@ import rerun.blueprint as rrb from dimos import spec +from dimos.agents.annotation import skill from dimos.core import DimosCluster, In, LCMTransport, Module, Out, pSHMTransport, rpc from dimos.core.global_config import GlobalConfig, global_config -from dimos.core.skill_module import SkillModule from dimos.msgs.geometry_msgs import ( PoseStamped, Quaternion, @@ -34,12 +34,10 @@ ) from dimos.msgs.sensor_msgs import CameraInfo, Image, PointCloud2 from dimos.msgs.sensor_msgs.Image import ImageFormat -from dimos.protocol.skill.skill import skill -from dimos.protocol.skill.type import Output from dimos.robot.unitree.connection import UnitreeWebRTCConnection from dimos.utils.data import get_data from dimos.utils.decorators.decorators import simple_mcache -from dimos.utils.testing import TimedSensorReplay, TimedSensorStorage +from dimos.utils.testing.replay import TimedSensorReplay, TimedSensorStorage logger = logging.getLogger(__name__) @@ -142,7 +140,7 @@ def publish_request(self, topic: str, data: dict): # type: ignore[no-untyped-de return {"status": "ok", "message": "Fake publish"} -class GO2Connection(SkillModule, spec.Camera, spec.Pointcloud): +class GO2Connection(Module, spec.Camera, spec.Pointcloud): cmd_vel: In[Twist] pointcloud: Out[PointCloud2] odom: Out[PoseStamped] @@ -194,13 +192,13 @@ def __init__( # type: ignore[no-untyped-def] @rpc def record(self, recording_name: str) -> None: lidar_store: TimedSensorStorage = TimedSensorStorage(f"{recording_name}/lidar") # type: ignore[type-arg] - lidar_store.save_stream(self.connection.lidar_stream()).subscribe(lambda x: x) # type: ignore[arg-type] + lidar_store.consume_stream(self.connection.lidar_stream()) odom_store: TimedSensorStorage = TimedSensorStorage(f"{recording_name}/odom") # type: ignore[type-arg] - odom_store.save_stream(self.connection.odom_stream()).subscribe(lambda x: x) # type: ignore[arg-type] + odom_store.consume_stream(self.connection.odom_stream()) video_store: TimedSensorStorage = TimedSensorStorage(f"{recording_name}/video") # type: ignore[type-arg] - video_store.save_stream(self.connection.video_stream()).subscribe(lambda x: x) # type: ignore[arg-type] + video_store.consume_stream(self.connection.video_stream()) @rpc def start(self) -> None: @@ -299,7 +297,7 @@ def publish_request(self, topic: str, data: dict[str, Any]) -> dict[Any, Any]: """ return self.connection.publish_request(topic, data) - @skill(output=Output.image) + @skill def observe(self) -> Image | None: """Returns the latest video frame from the robot camera. Use this skill for any visual world queries. diff --git a/dimos/robot/unitree/type/test_odometry.py b/dimos/robot/unitree/type/test_odometry.py index 0fdd5f5ad9..d0fe2b290e 100644 --- a/dimos/robot/unitree/type/test_odometry.py +++ b/dimos/robot/unitree/type/test_odometry.py @@ -14,10 +14,7 @@ from __future__ import annotations -from operator import add, sub - import pytest -import reactivex.operators as ops from dimos.robot.unitree.type.odometry import Odometry from dimos.utils.testing import SensorReplay @@ -38,19 +35,6 @@ def test_odometry_conversion_and_count() -> None: assert isinstance(odom, Odometry) -def test_last_yaw_value() -> None: - """Verify yaw of the final message (regression guard).""" - last_msg = SensorReplay(name="raw_odometry_rotate_walk").stream().pipe(ops.last()).run() - - assert last_msg is not None, "Replay is empty" - assert last_msg["data"]["pose"]["orientation"] == { - "x": 0.01077, - "y": 0.008505, - "z": 0.499171, - "w": -0.866395, - } - - def test_total_rotation_travel_iterate() -> None: total_rad = 0.0 prev_yaw: float | None = None @@ -63,19 +47,3 @@ def test_total_rotation_travel_iterate() -> None: prev_yaw = yaw assert total_rad == pytest.approx(_EXPECTED_TOTAL_RAD, abs=0.001) - - -def test_total_rotation_travel_rxpy() -> None: - total_rad = ( - SensorReplay(name="raw_odometry_rotate_walk", autocast=Odometry.from_msg) - .stream() - .pipe( - ops.map(lambda odom: odom.orientation.radians.z), - ops.pairwise(), # [1,2,3,4] -> [[1,2], [2,3], [3,4]] - ops.starmap(sub), # [sub(1,2), sub(2,3), sub(3,4)] - ops.reduce(add), - ) - .run() - ) - - assert total_rad == pytest.approx(4.05, abs=0.01) diff --git a/dimos/robot/unitree/unitree_skill_container.py b/dimos/robot/unitree/unitree_skill_container.py index 5cab15c369..d2f15b9efe 100644 --- a/dimos/robot/unitree/unitree_skill_container.py +++ b/dimos/robot/unitree/unitree_skill_container.py @@ -21,18 +21,165 @@ from unitree_webrtc_connect.constants import RTC_TOPIC +from dimos.agents.annotation import skill from dimos.core.core import rpc -from dimos.core.skill_module import SkillModule +from dimos.core.module import Module from dimos.msgs.geometry_msgs import PoseStamped, Quaternion, Vector3 from dimos.navigation.base import NavigationState -from dimos.protocol.skill.skill import skill -from dimos.protocol.skill.type import Reducer, Stream -from dimos.robot.unitree.unitree_skills import UNITREE_WEBRTC_CONTROLS from dimos.utils.logging_config import setup_logger logger = setup_logger() +UNITREE_WEBRTC_CONTROLS: list[tuple[str, int, str]] = [ + # ("Damp", 1001, "Lowers the robot to the ground fully."), + ( + "BalanceStand", + 1002, + "Activates a mode that maintains the robot in a balanced standing position.", + ), + ( + "StandUp", + 1004, + "Commands the robot to transition from a sitting or prone position to a standing posture.", + ), + ( + "StandDown", + 1005, + "Instructs the robot to move from a standing position to a sitting or prone posture.", + ), + ( + "RecoveryStand", + 1006, + "Recovers the robot to a state from which it can take more commands. Useful to run after multiple dynamic commands like front flips, Must run after skills like sit and jump and standup.", + ), + ("Sit", 1009, "Commands the robot to sit down from a standing or moving stance."), + ( + "RiseSit", + 1010, + "Commands the robot to rise back to a standing position from a sitting posture.", + ), + ( + "SwitchGait", + 1011, + "Switches the robot's walking pattern or style dynamically, suitable for different terrains or speeds.", + ), + ("Trigger", 1012, "Triggers a specific action or custom routine programmed into the robot."), + ( + "BodyHeight", + 1013, + "Adjusts the height of the robot's body from the ground, useful for navigating various obstacles.", + ), + ( + "FootRaiseHeight", + 1014, + "Controls how high the robot lifts its feet during movement, which can be adjusted for different surfaces.", + ), + ( + "SpeedLevel", + 1015, + "Sets or adjusts the speed at which the robot moves, with various levels available for different operational needs.", + ), + ( + "Hello", + 1016, + "Performs a greeting action, which could involve a wave or other friendly gesture.", + ), + ("Stretch", 1017, "Engages the robot in a stretching routine."), + ( + "TrajectoryFollow", + 1018, + "Directs the robot to follow a predefined trajectory, which could involve complex paths or maneuvers.", + ), + ( + "ContinuousGait", + 1019, + "Enables a mode for continuous walking or running, ideal for long-distance travel.", + ), + ("Content", 1020, "To display or trigger when the robot is happy."), + ("Wallow", 1021, "The robot falls onto its back and rolls around."), + ( + "Dance1", + 1022, + "Performs a predefined dance routine 1, programmed for entertainment or demonstration.", + ), + ("Dance2", 1023, "Performs another variant of a predefined dance routine 2."), + ("GetBodyHeight", 1024, "Retrieves the current height of the robot's body from the ground."), + ( + "GetFootRaiseHeight", + 1025, + "Retrieves the current height at which the robot's feet are being raised during movement.", + ), + ( + "GetSpeedLevel", + 1026, + "Retrieves the current speed level setting of the robot.", + ), + ( + "SwitchJoystick", + 1027, + "Switches the robot's control mode to respond to joystick input for manual operation.", + ), + ( + "Pose", + 1028, + "Commands the robot to assume a specific pose or posture as predefined in its programming.", + ), + ("Scrape", 1029, "The robot performs a scraping motion."), + ( + "FrontFlip", + 1030, + "Commands the robot to perform a front flip, showcasing its agility and dynamic movement capabilities.", + ), + ( + "FrontJump", + 1031, + "Instructs the robot to jump forward, demonstrating its explosive movement capabilities.", + ), + ( + "FrontPounce", + 1032, + "Commands the robot to perform a pouncing motion forward.", + ), + ( + "WiggleHips", + 1033, + "The robot performs a hip wiggling motion, often used for entertainment or demonstration purposes.", + ), + ( + "GetState", + 1034, + "Retrieves the current operational state of the robot, including its mode, position, and status.", + ), + ( + "EconomicGait", + 1035, + "Engages a more energy-efficient walking or running mode to conserve battery life.", + ), + ("FingerHeart", 1036, "Performs a finger heart gesture while on its hind legs."), + ( + "Handstand", + 1301, + "Commands the robot to perform a handstand, demonstrating balance and control.", + ), + ( + "CrossStep", + 1302, + "Commands the robot to perform cross-step movements.", + ), + ( + "OnesidedStep", + 1303, + "Commands the robot to perform one-sided step movements.", + ), + ("Bound", 1304, "Commands the robot to perform bounding movements."), + ("MoonWalk", 1305, "Commands the robot to perform a moonwalk motion."), + ("LeftFlip", 1042, "Executes a flip towards the left side."), + ("RightFlip", 1043, "Performs a flip towards the right side."), + ("Backflip", 1044, "Executes a backflip, a complex and dynamic maneuver."), +] + + _UNITREE_COMMANDS = { name: (id_, description) for name, id_, description in UNITREE_WEBRTC_CONTROLS @@ -40,7 +187,7 @@ } -class UnitreeSkillContainer(SkillModule): +class UnitreeSkillContainer(Module): """Container for Unitree Go2 robot skills using the new framework.""" rpc_calls: list[str] = [ @@ -61,7 +208,7 @@ def start(self) -> None: def stop(self) -> None: super().stop() - @skill() + @skill def relative_move(self, forward: float = 0.0, left: float = 0.0, degrees: float = 0.0) -> str: """Move the robot relative to its current position. @@ -132,7 +279,7 @@ def _generate_new_goal( return PoseStamped(position=goal_position, orientation=goal_orientation) - @skill() + @skill def wait(self, seconds: float) -> str: """Wait for a specified amount of time. @@ -142,15 +289,12 @@ def wait(self, seconds: float) -> str: time.sleep(seconds) return f"Wait completed with length={seconds}s" - @skill(stream=Stream.passive, reducer=Reducer.latest, hide_skill=True) # type: ignore[arg-type] - def current_time(self): # type: ignore[no-untyped-def] - """Provides current time implicitly, don't call this skill directly.""" - print("Starting current_time skill") - while True: - yield str(datetime.datetime.now()) - time.sleep(1) + @skill + def current_time(self) -> str: + """Provides current time.""" + return str(datetime.datetime.now()) - @skill() + @skill def execute_sport_command(self, command_name: str) -> str: try: publish_request = self.get_rpc_calls("GO2Connection.publish_request") diff --git a/dimos/robot/unitree/unitree_skills.py b/dimos/robot/unitree/unitree_skills.py deleted file mode 100644 index 05e01f63fb..0000000000 --- a/dimos/robot/unitree/unitree_skills.py +++ /dev/null @@ -1,357 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -import time -from typing import TYPE_CHECKING - -from pydantic import Field - -if TYPE_CHECKING: - from dimos.robot.robot import MockRobot, Robot # type: ignore[attr-defined] -else: - Robot = "Robot" - MockRobot = "MockRobot" - -from unitree_webrtc_connect.constants import RTC_TOPIC - -from dimos.msgs.geometry_msgs import Twist, Vector3 -from dimos.skills.skills import AbstractRobotSkill, AbstractSkill, SkillLibrary -from dimos.types.constants import Colors - -# Module-level constant for Unitree Go2 WebRTC control definitions -UNITREE_WEBRTC_CONTROLS: list[tuple[str, int, str]] = [ - ("Damp", 1001, "Lowers the robot to the ground fully."), - ( - "BalanceStand", - 1002, - "Activates a mode that maintains the robot in a balanced standing position.", - ), - ( - "StandUp", - 1004, - "Commands the robot to transition from a sitting or prone position to a standing posture.", - ), - ( - "StandDown", - 1005, - "Instructs the robot to move from a standing position to a sitting or prone posture.", - ), - ( - "RecoveryStand", - 1006, - "Recovers the robot to a state from which it can take more commands. Useful to run after multiple dynamic commands like front flips, Must run after skills like sit and jump and standup.", - ), - ("Sit", 1009, "Commands the robot to sit down from a standing or moving stance."), - ( - "RiseSit", - 1010, - "Commands the robot to rise back to a standing position from a sitting posture.", - ), - ( - "SwitchGait", - 1011, - "Switches the robot's walking pattern or style dynamically, suitable for different terrains or speeds.", - ), - ("Trigger", 1012, "Triggers a specific action or custom routine programmed into the robot."), - ( - "BodyHeight", - 1013, - "Adjusts the height of the robot's body from the ground, useful for navigating various obstacles.", - ), - ( - "FootRaiseHeight", - 1014, - "Controls how high the robot lifts its feet during movement, which can be adjusted for different surfaces.", - ), - ( - "SpeedLevel", - 1015, - "Sets or adjusts the speed at which the robot moves, with various levels available for different operational needs.", - ), - ( - "Hello", - 1016, - "Performs a greeting action, which could involve a wave or other friendly gesture.", - ), - ("Stretch", 1017, "Engages the robot in a stretching routine."), - ( - "TrajectoryFollow", - 1018, - "Directs the robot to follow a predefined trajectory, which could involve complex paths or maneuvers.", - ), - ( - "ContinuousGait", - 1019, - "Enables a mode for continuous walking or running, ideal for long-distance travel.", - ), - ("Content", 1020, "To display or trigger when the robot is happy."), - ("Wallow", 1021, "The robot falls onto its back and rolls around."), - ( - "Dance1", - 1022, - "Performs a predefined dance routine 1, programmed for entertainment or demonstration.", - ), - ("Dance2", 1023, "Performs another variant of a predefined dance routine 2."), - ("GetBodyHeight", 1024, "Retrieves the current height of the robot's body from the ground."), - ( - "GetFootRaiseHeight", - 1025, - "Retrieves the current height at which the robot's feet are being raised during movement.", - ), - ( - "GetSpeedLevel", - 1026, - "Retrieves the current speed level setting of the robot.", - ), - ( - "SwitchJoystick", - 1027, - "Switches the robot's control mode to respond to joystick input for manual operation.", - ), - ( - "Pose", - 1028, - "Commands the robot to assume a specific pose or posture as predefined in its programming.", - ), - ("Scrape", 1029, "The robot performs a scraping motion."), - ( - "FrontFlip", - 1030, - "Commands the robot to perform a front flip, showcasing its agility and dynamic movement capabilities.", - ), - ( - "FrontJump", - 1031, - "Instructs the robot to jump forward, demonstrating its explosive movement capabilities.", - ), - ( - "FrontPounce", - 1032, - "Commands the robot to perform a pouncing motion forward.", - ), - ( - "WiggleHips", - 1033, - "The robot performs a hip wiggling motion, often used for entertainment or demonstration purposes.", - ), - ( - "GetState", - 1034, - "Retrieves the current operational state of the robot, including its mode, position, and status.", - ), - ( - "EconomicGait", - 1035, - "Engages a more energy-efficient walking or running mode to conserve battery life.", - ), - ("FingerHeart", 1036, "Performs a finger heart gesture while on its hind legs."), - ( - "Handstand", - 1301, - "Commands the robot to perform a handstand, demonstrating balance and control.", - ), - ( - "CrossStep", - 1302, - "Commands the robot to perform cross-step movements.", - ), - ( - "OnesidedStep", - 1303, - "Commands the robot to perform one-sided step movements.", - ), - ("Bound", 1304, "Commands the robot to perform bounding movements."), - ("MoonWalk", 1305, "Commands the robot to perform a moonwalk motion."), - ("LeftFlip", 1042, "Executes a flip towards the left side."), - ("RightFlip", 1043, "Performs a flip towards the right side."), - ("Backflip", 1044, "Executes a backflip, a complex and dynamic maneuver."), -] - -# Module-level constants for Unitree G1 WebRTC control definitions -# G1 Arm Actions - all use api_id 7106 on topic "rt/api/arm/request" -G1_ARM_CONTROLS: list[tuple[str, int, str]] = [ - ("Handshake", 27, "Perform a handshake gesture with the right hand."), - ("HighFive", 18, "Give a high five with the right hand."), - ("Hug", 19, "Perform a hugging gesture with both arms."), - ("HighWave", 26, "Wave with the hand raised high."), - ("Clap", 17, "Clap hands together."), - ("FaceWave", 25, "Wave near the face level."), - ("LeftKiss", 12, "Blow a kiss with the left hand."), - ("ArmHeart", 20, "Make a heart shape with both arms overhead."), - ("RightHeart", 21, "Make a heart gesture with the right hand."), - ("HandsUp", 15, "Raise both hands up in the air."), - ("XRay", 24, "Hold arms in an X-ray pose position."), - ("RightHandUp", 23, "Raise only the right hand up."), - ("Reject", 22, "Make a rejection or 'no' gesture."), - ("CancelAction", 99, "Cancel any current arm action and return hands to neutral position."), -] - -# G1 Movement Modes - all use api_id 7101 on topic "rt/api/sport/request" -G1_MODE_CONTROLS: list[tuple[str, int, str]] = [ - ("WalkMode", 500, "Switch to normal walking mode."), - ("WalkControlWaist", 501, "Switch to walking mode with waist control."), - ("RunMode", 801, "Switch to running mode."), -] - -# region MyUnitreeSkills - - -class MyUnitreeSkills(SkillLibrary): - """My Unitree Skills for WebRTC interface.""" - - def __init__(self, robot: Robot | None = None, robot_type: str = "go2") -> None: - """Initialize Unitree skills library. - - Args: - robot: Optional robot instance - robot_type: Type of robot ("go2" or "g1"), defaults to "go2" - """ - super().__init__() - self._robot: Robot = None # type: ignore[assignment] - self.robot_type = robot_type.lower() - - if self.robot_type not in ["go2", "g1"]: - raise ValueError(f"Unsupported robot type: {robot_type}. Must be 'go2' or 'g1'") - - # Add dynamic skills to this class based on robot type - dynamic_skills = self.create_skills_live() - self.register_skills(dynamic_skills) # type: ignore[arg-type] - - @classmethod - def register_skills(cls, skill_classes: AbstractSkill | list[AbstractSkill]) -> None: - """Add multiple skill classes as class attributes. - - Args: - skill_classes: List of skill classes to add - """ - if not isinstance(skill_classes, list): - skill_classes = [skill_classes] - - for skill_class in skill_classes: - # Add to the class as a skill - setattr(cls, skill_class.__name__, skill_class) # type: ignore[attr-defined] - - def initialize_skills(self) -> None: - for skill_class in self.get_class_skills(): - self.create_instance(skill_class.__name__, robot=self._robot) # type: ignore[attr-defined] - - # Refresh the class skills - self.refresh_class_skills() - - def create_skills_live(self) -> list[AbstractRobotSkill]: - # ================================================ - # Procedurally created skills - # ================================================ - class BaseUnitreeSkill(AbstractRobotSkill): - """Base skill for dynamic skill creation.""" - - def __call__(self) -> str: - super().__call__() # type: ignore[no-untyped-call] - - # For Go2: Simple api_id based call - if hasattr(self, "_app_id"): - string = f"{Colors.GREEN_PRINT_COLOR}Executing Go2 skill: {self.__class__.__name__} with api_id={self._app_id}{Colors.RESET_COLOR}" - print(string) - self._robot.connection.publish_request( # type: ignore[attr-defined] - RTC_TOPIC["SPORT_MOD"], {"api_id": self._app_id} - ) - return f"{self.__class__.__name__} executed successfully" - - # For G1: Fixed api_id with parameter data - elif hasattr(self, "_data_value"): - string = f"{Colors.GREEN_PRINT_COLOR}Executing G1 skill: {self.__class__.__name__} with data={self._data_value}{Colors.RESET_COLOR}" - print(string) - self._robot.connection.publish_request( # type: ignore[attr-defined] - self._topic, # type: ignore[attr-defined] - {"api_id": self._api_id, "parameter": {"data": self._data_value}}, # type: ignore[attr-defined] - ) - return f"{self.__class__.__name__} executed successfully" - else: - raise RuntimeError( - f"Skill {self.__class__.__name__} missing required attributes" - ) - - skills_classes = [] - - if self.robot_type == "g1": - # Create G1 arm skills - for name, data_value, description in G1_ARM_CONTROLS: - skill_class = type( - name, - (BaseUnitreeSkill,), - { - "__doc__": description, - "_topic": "rt/api/arm/request", - "_api_id": 7106, - "_data_value": data_value, - }, - ) - skills_classes.append(skill_class) - - # Create G1 mode skills - for name, data_value, description in G1_MODE_CONTROLS: - skill_class = type( - name, - (BaseUnitreeSkill,), - { - "__doc__": description, - "_topic": "rt/api/sport/request", - "_api_id": 7101, - "_data_value": data_value, - }, - ) - skills_classes.append(skill_class) - else: - # Go2 skills (existing code) - for name, app_id, description in UNITREE_WEBRTC_CONTROLS: - if name not in ["Reverse", "Spin"]: # Exclude reverse and spin skills - skill_class = type( - name, (BaseUnitreeSkill,), {"__doc__": description, "_app_id": app_id} - ) - skills_classes.append(skill_class) - - return skills_classes # type: ignore[return-value] - - # region Class-based Skills - - class Move(AbstractRobotSkill): - """Move the robot using direct velocity commands. Determine duration required based on user distance instructions.""" - - x: float = Field(..., description="Forward velocity (m/s).") - y: float = Field(default=0.0, description="Left/right velocity (m/s)") - yaw: float = Field(default=0.0, description="Rotational velocity (rad/s)") - duration: float = Field(default=0.0, description="How long to move (seconds).") - - def __call__(self) -> str: - self._robot.move( # type: ignore[attr-defined] - Twist(linear=Vector3(self.x, self.y, 0.0), angular=Vector3(0.0, 0.0, self.yaw)), - duration=self.duration, - ) - return f"started moving with velocity={self.x}, {self.y}, {self.yaw} for {self.duration} seconds" - - class Wait(AbstractSkill): - """Wait for a specified amount of time.""" - - seconds: float = Field(..., description="Seconds to wait") - - def __call__(self) -> str: - time.sleep(self.seconds) - return f"Wait completed with length={self.seconds}s" - - # endregion - - -# endregion diff --git a/dimos/robot/unitree_webrtc/__init__.py b/dimos/robot/unitree_webrtc/__init__.py index b8660ff6fd..4524bba226 100644 --- a/dimos/robot/unitree_webrtc/__init__.py +++ b/dimos/robot/unitree_webrtc/__init__.py @@ -26,7 +26,6 @@ "type": "dimos.robot.unitree.type", "unitree_g1_skill_container": "dimos.robot.unitree.g1.skill_container", "unitree_skill_container": "dimos.robot.unitree.unitree_skill_container", - "unitree_skills": "dimos.robot.unitree.unitree_skills", } for alias, target in _ALIAS_MODULES.items(): diff --git a/dimos/types/test_timestamped.py b/dimos/types/test_timestamped.py index 88a8d65102..7de82e8f9a 100644 --- a/dimos/types/test_timestamped.py +++ b/dimos/types/test_timestamped.py @@ -19,11 +19,11 @@ from reactivex import operators as ops from reactivex.scheduler import ThreadPoolScheduler +from dimos.memory.timeseries.inmemory import InMemoryStore from dimos.msgs.sensor_msgs import Image from dimos.types.timestamped import ( Timestamped, TimestampedBufferCollection, - TimestampedCollection, align_timestamped, to_datetime, to_ros_stamp, @@ -133,13 +133,20 @@ def sample_items(): ] +def make_store(items: list[SimpleTimestamped] | None = None) -> InMemoryStore[SimpleTimestamped]: + store: InMemoryStore[SimpleTimestamped] = InMemoryStore() + if items: + store.save(*items) + return store + + @pytest.fixture def collection(sample_items): - return TimestampedCollection(sample_items) + return make_store(sample_items) def test_empty_collection() -> None: - collection = TimestampedCollection() + collection = make_store() assert len(collection) == 0 assert collection.duration() == 0.0 assert collection.time_range() is None @@ -147,16 +154,17 @@ def test_empty_collection() -> None: def test_add_items() -> None: - collection = TimestampedCollection() + collection = make_store() item1 = SimpleTimestamped(2.0, "two") item2 = SimpleTimestamped(1.0, "one") - collection.add(item1) - collection.add(item2) + collection.save(item1) + collection.save(item2) assert len(collection) == 2 - assert collection[0].data == "one" # Should be sorted by timestamp - assert collection[1].data == "two" + items = list(collection) + assert items[0].data == "one" # Should be sorted by timestamp + assert items[1].data == "two" def test_find_closest(collection) -> None: @@ -196,21 +204,13 @@ def test_find_before_after(collection) -> None: assert collection.find_after(7.0) is None # Nothing after last item -def test_merge_collections() -> None: - collection1 = TimestampedCollection( - [ - SimpleTimestamped(1.0, "a"), - SimpleTimestamped(3.0, "c"), - ] - ) - collection2 = TimestampedCollection( - [ - SimpleTimestamped(2.0, "b"), - SimpleTimestamped(4.0, "d"), - ] - ) +def test_save_from_multiple_stores() -> None: + store1 = make_store([SimpleTimestamped(1.0, "a"), SimpleTimestamped(3.0, "c")]) + store2 = make_store([SimpleTimestamped(2.0, "b"), SimpleTimestamped(4.0, "d")]) - merged = collection1.merge(collection2) + merged = make_store() + merged.save(*store1) + merged.save(*store2) assert len(merged) == 4 assert [item.data for item in merged] == ["a", "b", "c", "d"] @@ -244,7 +244,7 @@ def test_iteration(collection) -> None: def test_single_item_collection() -> None: - single = TimestampedCollection([SimpleTimestamped(5.0, "only")]) + single = make_store([SimpleTimestamped(5.0, "only")]) assert single.duration() == 0.0 assert single.time_range() == (5.0, 5.0) @@ -264,14 +264,17 @@ def test_time_window_collection() -> None: # Add a message at t=4.0, should keep messages from t=2.0 onwards window.add(SimpleTimestamped(4.0, "msg4")) assert len(window) == 3 # msg1 should be dropped - assert window[0].data == "msg2" # oldest is now msg2 - assert window[-1].data == "msg4" # newest is msg4 + first = window.first() + last = window.last() + assert first is not None and first.data == "msg2" # oldest is now msg2 + assert last is not None and last.data == "msg4" # newest is msg4 # Add a message at t=5.5, should drop msg2 and msg3 window.add(SimpleTimestamped(5.5, "msg5")) assert len(window) == 2 # only msg4 and msg5 remain - assert window[0].data == "msg4" - assert window[1].data == "msg5" + items = list(window) + assert items[0].data == "msg4" + assert items[1].data == "msg5" # Verify time range assert window.start_ts == 4.0 diff --git a/dimos/types/timestamped.py b/dimos/types/timestamped.py index 765b1adbcb..b229a2478e 100644 --- a/dimos/types/timestamped.py +++ b/dimos/types/timestamped.py @@ -12,7 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. from collections import defaultdict -from collections.abc import Iterable, Iterator from datetime import datetime, timezone from typing import Generic, TypeVar, Union @@ -22,8 +21,8 @@ # from dimos_lcm.std_msgs import Time as ROSTime from reactivex.observable import Observable -from sortedcontainers import SortedKeyList # type: ignore[import-untyped] +from dimos.memory.timeseries.inmemory import InMemoryStore from dimos.types.weaklist import WeakList from dimos.utils.logging_config import setup_logger @@ -117,152 +116,29 @@ def ros_timestamp(self) -> list[int]: T = TypeVar("T", bound=Timestamped) -class TimestampedCollection(Generic[T]): - """A collection of timestamped objects with efficient time-based operations.""" - - def __init__(self, items: Iterable[T] | None = None) -> None: - self._items = SortedKeyList(items or [], key=lambda x: x.ts) - - def add(self, item: T) -> None: - """Add a timestamped item to the collection.""" - self._items.add(item) - - def find_closest(self, timestamp: float, tolerance: float | None = None) -> T | None: - """Find the timestamped object closest to the given timestamp.""" - if not self._items: - return None - - # Use binary search to find insertion point - idx = self._items.bisect_key_left(timestamp) - - # Check exact match - if idx < len(self._items) and self._items[idx].ts == timestamp: - return self._items[idx] # type: ignore[no-any-return] - - # Find candidates: item before and after - candidates = [] - - # Item before - if idx > 0: - candidates.append((idx - 1, abs(self._items[idx - 1].ts - timestamp))) - - # Item after - if idx < len(self._items): - candidates.append((idx, abs(self._items[idx].ts - timestamp))) - - if not candidates: - return None - - # Find closest - # When distances are equal, prefer the later item (higher index) - closest_idx, closest_distance = min(candidates, key=lambda x: (x[1], -x[0])) - - # Check tolerance if provided - if tolerance is not None and closest_distance > tolerance: - return None - - return self._items[closest_idx] # type: ignore[no-any-return] - - def find_before(self, timestamp: float) -> T | None: - """Find the last item before the given timestamp.""" - idx = self._items.bisect_key_left(timestamp) - return self._items[idx - 1] if idx > 0 else None - - def find_after(self, timestamp: float) -> T | None: - """Find the first item after the given timestamp.""" - idx = self._items.bisect_key_right(timestamp) - return self._items[idx] if idx < len(self._items) else None - - def merge(self, other: "TimestampedCollection[T]") -> "TimestampedCollection[T]": - """Merge two timestamped collections into a new one.""" - result = TimestampedCollection[T]() - result._items = SortedKeyList(self._items + other._items, key=lambda x: x.ts) - return result - - def duration(self) -> float: - """Get the duration of the collection in seconds.""" - if len(self._items) < 2: - return 0.0 - return self._items[-1].ts - self._items[0].ts # type: ignore[no-any-return] - - def time_range(self) -> tuple[float, float] | None: - """Get the time range (start, end) of the collection.""" - if not self._items: - return None - return (self._items[0].ts, self._items[-1].ts) - - def slice_by_time(self, start: float, end: float) -> "TimestampedCollection[T]": - """Get a subset of items within the given time range.""" - start_idx = self._items.bisect_key_left(start) - end_idx = self._items.bisect_key_right(end) - return TimestampedCollection(self._items[start_idx:end_idx]) - - @property - def start_ts(self) -> float | None: - """Get the start timestamp of the collection.""" - return self._items[0].ts if self._items else None - - @property - def end_ts(self) -> float | None: - """Get the end timestamp of the collection.""" - return self._items[-1].ts if self._items else None - - def __len__(self) -> int: - return len(self._items) - - def __iter__(self) -> Iterator: # type: ignore[type-arg] - return iter(self._items) - - def __getitem__(self, idx: int) -> T: - return self._items[idx] # type: ignore[no-any-return] - - PRIMARY = TypeVar("PRIMARY", bound=Timestamped) SECONDARY = TypeVar("SECONDARY", bound=Timestamped) -class TimestampedBufferCollection(TimestampedCollection[T]): - """A timestamped collection that maintains a sliding time window, dropping old messages.""" - - def __init__(self, window_duration: float, items: Iterable[T] | None = None) -> None: - """ - Initialize with a time window duration in seconds. +class TimestampedBufferCollection(InMemoryStore[T]): + """A sliding time window buffer backed by InMemoryStore.""" - Args: - window_duration: Maximum age of messages to keep in seconds - items: Optional initial items - """ - super().__init__(items) + def __init__(self, window_duration: float) -> None: + super().__init__() self.window_duration = window_duration def add(self, item: T) -> None: - """Add a timestamped item and remove any items outside the time window.""" - super().add(item) - self._prune_old_messages(item.ts) - - def _prune_old_messages(self, current_ts: float) -> None: - """Remove messages older than window_duration from the given timestamp.""" - cutoff_ts = current_ts - self.window_duration - - # Find the index of the first item that should be kept - keep_idx = self._items.bisect_key_left(cutoff_ts) + """Add a timestamped item and prune items outside the time window.""" + self.save(item) + self.prune_old(item.ts - self.window_duration) - # Remove old items - if keep_idx > 0: - del self._items[:keep_idx] + def remove(self, item: T) -> bool: + """Remove a timestamped item. Returns True if found and removed.""" + return self._delete(item.ts) is not None def remove_by_timestamp(self, timestamp: float) -> bool: - """Remove an item with the given timestamp. Returns True if item was found and removed.""" - idx = self._items.bisect_key_left(timestamp) - - if idx < len(self._items) and self._items[idx].ts == timestamp: - del self._items[idx] - return True - return False - - def remove(self, item: T) -> bool: - """Remove a timestamped item from the collection. Returns True if item was found and removed.""" - return self.remove_by_timestamp(item.ts) + """Remove an item by timestamp. Returns True if found and removed.""" + return self._delete(timestamp) is not None class MatchContainer(Timestamped, Generic[PRIMARY, SECONDARY]): diff --git a/dimos/utils/cli/skillspy/demo_skillspy.py b/dimos/utils/cli/skillspy/demo_skillspy.py deleted file mode 100644 index 602381020a..0000000000 --- a/dimos/utils/cli/skillspy/demo_skillspy.py +++ /dev/null @@ -1,111 +0,0 @@ -#!/usr/bin/env python3 -# 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. - -"""Demo script that runs skills in the background while agentspy monitors them.""" - -import threading -import time - -from dimos.protocol.skill.coordinator import SkillCoordinator -from dimos.protocol.skill.skill import SkillContainer, skill - - -class DemoSkills(SkillContainer): - @skill() - def count_to(self, n: int) -> str: - """Count to n with delays.""" - for _i in range(n): - time.sleep(0.5) - return f"Counted to {n}" - - @skill() - def compute_fibonacci(self, n: int) -> int: - """Compute nth fibonacci number.""" - if n <= 1: - return n - a, b = 0, 1 - for _ in range(2, n + 1): - time.sleep(0.1) # Simulate computation - a, b = b, a + b - return b - - @skill() - def simulate_error(self) -> None: - """Skill that always errors.""" - time.sleep(0.3) - raise RuntimeError("Simulated error for testing") - - @skill() - def quick_task(self, name: str) -> str: - """Quick task that completes fast.""" - time.sleep(0.1) - return f"Quick task '{name}' done!" - - -def run_demo_skills() -> None: - """Run demo skills in background.""" - # Create and start agent interface - agent_interface = SkillCoordinator() - agent_interface.start() - - # Register skills - demo_skills = DemoSkills() - agent_interface.register_skills(demo_skills) - - # Run various skills periodically - def skill_runner() -> None: - counter = 0 - while True: - time.sleep(2) - - # Generate unique call_id for each invocation - call_id = f"demo-{counter}" - - # Run different skills based on counter - if counter % 4 == 0: - # Run multiple count_to in parallel to show parallel execution - agent_interface.call_skill(f"{call_id}-count-1", "count_to", {"args": [3]}) - agent_interface.call_skill(f"{call_id}-count-2", "count_to", {"args": [5]}) - agent_interface.call_skill(f"{call_id}-count-3", "count_to", {"args": [2]}) - elif counter % 4 == 1: - agent_interface.call_skill(f"{call_id}-fib", "compute_fibonacci", {"args": [10]}) - elif counter % 4 == 2: - agent_interface.call_skill( - f"{call_id}-quick", "quick_task", {"args": [f"task-{counter}"]} - ) - else: - agent_interface.call_skill(f"{call_id}-error", "simulate_error", {}) - - counter += 1 - - # Start skill runner in background - thread = threading.Thread(target=skill_runner, daemon=True) - thread.start() - - print("Demo skills running in background. Start agentspy in another terminal to monitor.") - print("Run: agentspy") - - # Keep running - try: - while True: - time.sleep(1) - except KeyboardInterrupt: - print("\nDemo stopped.") - - agent_interface.stop() - - -if __name__ == "__main__": - run_demo_skills() diff --git a/dimos/utils/cli/skillspy/skillspy.py b/dimos/utils/cli/skillspy/skillspy.py deleted file mode 100644 index beb2421eec..0000000000 --- a/dimos/utils/cli/skillspy/skillspy.py +++ /dev/null @@ -1,281 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -import threading -import time -from typing import TYPE_CHECKING - -from rich.text import Text -from textual.app import App, ComposeResult -from textual.binding import Binding -from textual.widgets import DataTable, Footer - -from dimos.protocol.skill.coordinator import SkillCoordinator, SkillState, SkillStateEnum -from dimos.utils.cli import theme - -if TYPE_CHECKING: - from collections.abc import Callable - - from dimos.protocol.skill.comms import SkillMsg # type: ignore[attr-defined] - - -class AgentSpy: - """Spy on agent skill executions via LCM messages.""" - - def __init__(self) -> None: - self.agent_interface = SkillCoordinator() - self.message_callbacks: list[Callable[[dict[str, SkillState]], None]] = [] - self._lock = threading.Lock() - self._latest_state: dict[str, SkillState] = {} - self._running = False - - def start(self) -> None: - """Start spying on agent messages.""" - self._running = True - # Start the agent interface - self.agent_interface.start() - - # Subscribe to the agent interface's comms - self.agent_interface.skill_transport.subscribe(self._handle_message) - - def stop(self) -> None: - """Stop spying.""" - self._running = False - # Give threads a moment to finish processing - time.sleep(0.2) - self.agent_interface.stop() - - def _handle_message(self, msg: SkillMsg) -> None: # type: ignore[type-arg] - """Handle incoming skill messages.""" - if not self._running: - return - - # Small delay to ensure agent_interface has processed the message - def delayed_update() -> None: - time.sleep(0.1) - if not self._running: - return - with self._lock: - self._latest_state = self.agent_interface.generate_snapshot(clear=False) - for callback in self.message_callbacks: - callback(self._latest_state) - - # Run in separate thread to not block LCM - threading.Thread(target=delayed_update, daemon=True).start() - - def subscribe(self, callback: Callable[[dict[str, SkillState]], None]) -> None: - """Subscribe to state updates.""" - self.message_callbacks.append(callback) - - def get_state(self) -> dict[str, SkillState]: - """Get current state snapshot.""" - with self._lock: - return self._latest_state.copy() - - -def state_color(state: SkillStateEnum) -> str: - """Get color for skill state.""" - if state == SkillStateEnum.pending: - return theme.WARNING - elif state == SkillStateEnum.running: - return theme.AGENT - elif state == SkillStateEnum.completed: - return theme.SUCCESS - elif state == SkillStateEnum.error: - return theme.ERROR - return theme.FOREGROUND - - -def format_duration(duration: float) -> str: - """Format duration in human readable format.""" - if duration < 1: - return f"{duration * 1000:.0f}ms" - elif duration < 60: - return f"{duration:.1f}s" - elif duration < 3600: - return f"{duration / 60:.1f}m" - else: - return f"{duration / 3600:.1f}h" - - -class AgentSpyApp(App): # type: ignore[type-arg] - """A real-time CLI dashboard for agent skill monitoring using Textual.""" - - CSS_PATH = theme.CSS_PATH - - CSS = f""" - Screen {{ - layout: vertical; - background: {theme.BACKGROUND}; - }} - DataTable {{ - height: 100%; - border: solid $border; - background: {theme.BACKGROUND}; - }} - DataTable > .datatable--header {{ - background: transparent; - }} - Footer {{ - background: transparent; - }} - """ - - BINDINGS = [ - Binding("q", "quit", "Quit"), - Binding("c", "clear", "Clear History"), - Binding("ctrl+c", "quit", "Quit", show=False), - ] - - def __init__(self, *args, **kwargs) -> None: # type: ignore[no-untyped-def] - super().__init__(*args, **kwargs) - self.spy = AgentSpy() - self.table: DataTable | None = None # type: ignore[type-arg] - self.skill_history: list[tuple[str, SkillState, float]] = [] # (call_id, state, start_time) - - def compose(self) -> ComposeResult: - self.table = DataTable(zebra_stripes=False, cursor_type=None) # type: ignore[arg-type] - self.table.add_column("Call ID") - self.table.add_column("Skill Name") - self.table.add_column("State") - self.table.add_column("Duration") - self.table.add_column("Messages") - self.table.add_column("Details") - - yield self.table - yield Footer() - - def on_mount(self) -> None: - """Start the spy when app mounts.""" - self.spy.subscribe(self.update_state) - self.spy.start() - - # Set up periodic refresh to update durations - self.set_interval(1.0, self.refresh_table) - - def on_unmount(self) -> None: - """Stop the spy when app unmounts.""" - self.spy.stop() - - def update_state(self, state: dict[str, SkillState]) -> None: - """Update state from spy callback. State dict is keyed by call_id.""" - # Update history with current state - current_time = time.time() - - # Add new skills or update existing ones - for call_id, skill_state in state.items(): - # Find if this call_id already in history - found = False - for i, (existing_call_id, _old_state, start_time) in enumerate(self.skill_history): - if existing_call_id == call_id: - # Update existing entry - self.skill_history[i] = (call_id, skill_state, start_time) - found = True - break - - if not found: - # Add new entry with current time as start - start_time = current_time - if skill_state.start_msg: - # Use start message timestamp if available - start_time = skill_state.start_msg.ts - self.skill_history.append((call_id, skill_state, start_time)) - - # Schedule UI update - self.call_from_thread(self.refresh_table) - - def refresh_table(self) -> None: - """Refresh the table display.""" - if not self.table: - return - - # Clear table - self.table.clear(columns=False) - - # Sort by start time (newest first) - sorted_history = sorted(self.skill_history, key=lambda x: x[2], reverse=True) - - # Get terminal height and calculate how many rows we can show - height = self.size.height - 6 # Account for header, footer, column headers - max_rows = max(1, height) - - # Show only top N entries - for call_id, skill_state, start_time in sorted_history[:max_rows]: - # Calculate how long ago it started (for progress indicator) - time_ago = time.time() - start_time - - # Duration - duration_str = format_duration(skill_state.duration()) - - # Message count - msg_count = len(skill_state) - - # Details based on state and last message - details = "" - if skill_state.state == SkillStateEnum.error and skill_state.error_msg: - # Show error message - error_content = skill_state.error_msg.content - if isinstance(error_content, dict): - details = error_content.get("msg", "Error")[:40] - else: - details = str(error_content)[:40] - elif skill_state.state == SkillStateEnum.completed and skill_state.ret_msg: - # Show return value - details = f"→ {str(skill_state.ret_msg.content)[:37]}" - elif skill_state.state == SkillStateEnum.running: - # Show progress indicator - details = "⋯ " + "▸" * min(int(time_ago), 20) - - # Format call_id for display (truncate if too long) - display_call_id = call_id - if len(call_id) > 16: - display_call_id = call_id[:13] + "..." - - # Add row with colored state - self.table.add_row( - Text(display_call_id, style=theme.BRIGHT_BLUE), - Text(skill_state.name, style=theme.YELLOW), - Text(skill_state.state.name, style=state_color(skill_state.state)), - Text(duration_str, style=theme.WHITE), - Text(str(msg_count), style=theme.YELLOW), - Text(details, style=theme.FOREGROUND), - ) - - def action_clear(self) -> None: - """Clear the skill history.""" - self.skill_history.clear() - self.refresh_table() - - -def main() -> None: - """Main entry point for agentspy CLI.""" - import sys - - # Check if running in web mode - if len(sys.argv) > 1 and sys.argv[1] == "web": - import os - - from textual_serve.server import Server # type: ignore[import-not-found] - - server = Server(f"python {os.path.abspath(__file__)}") - server.serve() - else: - app = AgentSpyApp() - app.run() - - -if __name__ == "__main__": - main() diff --git a/dimos/utils/data.py b/dimos/utils/data.py index 7666343dbb..094d4a6a7c 100644 --- a/dimos/utils/data.py +++ b/dimos/utils/data.py @@ -105,7 +105,7 @@ def _get_repo_root() -> Path: @cache -def _get_data_dir(extra_path: str | None = None) -> Path: +def get_data_dir(extra_path: str | None = None) -> Path: if extra_path: return _get_repo_root() / "data" / extra_path return _get_repo_root() / "data" @@ -113,7 +113,7 @@ def _get_data_dir(extra_path: str | None = None) -> Path: @cache def _get_lfs_dir() -> Path: - return _get_data_dir() / ".lfs" + return get_data_dir() / ".lfs" def _check_git_lfs_available() -> bool: @@ -174,7 +174,7 @@ def _lfs_pull(file_path: Path, repo_root: Path) -> None: def _decompress_archive(filename: str | Path) -> Path: - target_dir = _get_data_dir() + target_dir = get_data_dir() filename_path = Path(filename) with tarfile.open(filename_path, "r:gz") as tar: tar.extractall(target_dir) @@ -211,7 +211,7 @@ def _pull_lfs_archive(filename: str | Path) -> Path: return file_path -def get_data(filename: str | Path) -> Path: +def get_data(name: str | Path) -> Path: """ Get the path to a test data, downloading from LFS if needed. @@ -222,29 +222,43 @@ def get_data(filename: str | Path) -> Path: 4. Download the file from LFS if it's a pointer file 5. Return the Path object to the actual file or dir + Supports nested paths like "dataset/subdir/file.jpg" - will download and + decompress "dataset" archive but return the full nested path. + Args: - filename: Name of the test file (e.g., "lidar_sample.bin") + name: Name of the test file or dir, optionally with nested path + (e.g., "lidar_sample.bin" or "dataset/frames/001.png") Returns: - Path: Path object to the test file + Path: Path object to the test file or dir Raises: RuntimeError: If Git LFS is not available or LFS operations fail FileNotFoundError: If the test file doesn't exist Usage: - # As string path - file_path = str(testFile("sample.bin")) + # Simple file/dir + file_path = get_data("sample.bin") - # As context manager for file operations - with testFile("sample.bin").open('rb') as f: - data = f.read() + # Nested path - downloads "dataset" archive, returns path to nested file + frame = get_data("dataset/frames/001.png") """ - data_dir = _get_data_dir() - file_path = data_dir / filename + data_dir = get_data_dir() + file_path = data_dir / name # already pulled and decompressed, return it directly if file_path.exists(): return file_path - return _decompress_archive(_pull_lfs_archive(filename)) + # extract archive root (first path component) and nested path + path_parts = Path(name).parts + archive_name = path_parts[0] + nested_path = Path(*path_parts[1:]) if len(path_parts) > 1 else None + + # download and decompress the archive root + archive_path = _decompress_archive(_pull_lfs_archive(archive_name)) + + # return full path including nested components + if nested_path: + return archive_path / nested_path + return archive_path diff --git a/dimos/utils/reactive.py b/dimos/utils/reactive.py index d76bd518cc..4397e0171e 100644 --- a/dimos/utils/reactive.py +++ b/dimos/utils/reactive.py @@ -19,6 +19,7 @@ import reactivex as rx from reactivex import operators as ops +from reactivex.abc import DisposableBase from reactivex.disposable import Disposable from reactivex.observable import Observable from reactivex.scheduler import ThreadPoolScheduler @@ -64,7 +65,7 @@ def _subscribe(observer, sch=None): # type: ignore[no-untyped-def] return rx.defer(lambda *_: per_sub()) # type: ignore[no-untyped-call] -class LatestReader(Generic[T]): +class LatestReader(DisposableBase, Generic[T]): """A callable object that returns the latest value from an observable.""" def __init__(self, initial_value: T, subscription, connection=None) -> None: # type: ignore[no-untyped-def] @@ -259,7 +260,9 @@ def spyfun(x): # type: ignore[no-untyped-def] return ops.map(spyfun) -def quality_barrier(quality_func: Callable[[T], float], target_frequency: float): # type: ignore[no-untyped-def] +def quality_barrier( + quality_func: Callable[[T], float], target_frequency: float +) -> Callable[[Observable[T]], Observable[T]]: """ RxPY pipe operator that selects the highest quality item within each time window. diff --git a/dimos/utils/test_data.py b/dimos/utils/test_data.py index 01f145f60c..ba135fe255 100644 --- a/dimos/utils/test_data.py +++ b/dimos/utils/test_data.py @@ -26,7 +26,7 @@ def test_pull_file() -> None: repo_root = data._get_repo_root() test_file_name = "cafe.jpg" test_file_compressed = data._get_lfs_dir() / (test_file_name + ".tar.gz") - test_file_decompressed = data._get_data_dir() / test_file_name + test_file_decompressed = data.get_data_dir() / test_file_name # delete decompressed test file if it exists if test_file_decompressed.exists(): @@ -82,7 +82,7 @@ def test_pull_dir() -> None: repo_root = data._get_repo_root() test_dir_name = "ab_lidar_frames" test_dir_compressed = data._get_lfs_dir() / (test_dir_name + ".tar.gz") - test_dir_decompressed = data._get_data_dir() / test_dir_name + test_dir_decompressed = data.get_data_dir() / test_dir_name # delete decompressed test directory if it exists if test_dir_decompressed.exists(): diff --git a/dimos/utils/testing/moment.py b/dimos/utils/testing/moment.py index 436240a48b..e92d771687 100644 --- a/dimos/utils/testing/moment.py +++ b/dimos/utils/testing/moment.py @@ -17,12 +17,13 @@ from typing import TYPE_CHECKING, Any, Generic, TypeVar from dimos.core.resource import Resource +from dimos.types.timestamped import Timestamped from dimos.utils.testing.replay import TimedSensorReplay if TYPE_CHECKING: from dimos.core import Transport -T = TypeVar("T") +T = TypeVar("T", bound=Timestamped) class SensorMoment(Generic[T], Resource): diff --git a/dimos/utils/testing/replay.py b/dimos/utils/testing/replay.py index 89225c322e..588b63e099 100644 --- a/dimos/utils/testing/replay.py +++ b/dimos/utils/testing/replay.py @@ -11,399 +11,12 @@ # 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 collections.abc import Callable, Iterator -import functools -import glob -import os -from pathlib import Path -import pickle -import re -import time -from typing import Any, Generic, TypeVar -from reactivex import ( - from_iterable, - interval, - operators as ops, -) -from reactivex.observable import Observable -from reactivex.scheduler import TimeoutScheduler +"""Shim for TimedSensorReplay/TimedSensorStorage.""" -from dimos.utils.data import _get_data_dir, get_data +from dimos.memory.timeseries.legacy import LegacyPickleStore -T = TypeVar("T") - - -class SensorReplay(Generic[T]): - """Generic sensor data replay utility. - - Args: - name: The name of the test dataset - autocast: Optional function that takes unpickled data and returns a processed result. - For example: pointcloud2_from_webrtc_lidar - """ - - def __init__(self, name: str, autocast: Callable[[Any], T] | None = None) -> None: - self.root_dir = get_data(name) - self.autocast = autocast - - def load(self, *names: int | str) -> T | Any | list[T] | list[Any]: - if len(names) == 1: - return self.load_one(names[0]) - return list(map(lambda name: self.load_one(name), names)) - - def load_one(self, name: int | str | Path) -> T | Any: - if isinstance(name, int): - full_path = self.root_dir / f"/{name:03d}.pickle" - elif isinstance(name, Path): - full_path = name - else: - full_path = self.root_dir / Path(f"{name}.pickle") - - with open(full_path, "rb") as f: - data = pickle.load(f) - if self.autocast: - return self.autocast(data) - return data - - def first(self) -> T | Any | None: - try: - return next(self.iterate()) - except StopIteration: - return None - - @functools.cached_property - def files(self) -> list[Path]: - def extract_number(filepath): # type: ignore[no-untyped-def] - """Extract last digits before .pickle extension""" - basename = os.path.basename(filepath) - match = re.search(r"(\d+)\.pickle$", basename) - return int(match.group(1)) if match else 0 - - return sorted( - glob.glob(os.path.join(self.root_dir, "*")), # type: ignore[arg-type] - key=extract_number, - ) - - def iterate(self, loop: bool = False) -> Iterator[T | Any]: - while True: - for file_path in self.files: - yield self.load_one(Path(file_path)) - if not loop: - break - - def stream(self, rate_hz: float | None = None, loop: bool = False) -> Observable[T | Any]: - if rate_hz is None: - return from_iterable(self.iterate(loop=loop)) - - sleep_time = 1.0 / rate_hz - - return from_iterable(self.iterate(loop=loop)).pipe( - ops.zip(interval(sleep_time)), - ops.map(lambda x: x[0] if isinstance(x, tuple) else x), - ) - - -class SensorStorage(Generic[T]): - """Generic sensor data storage utility - . - Creates a directory in the test data directory and stores pickled sensor data. - - Args: - name: The name of the storage directory - autocast: Optional function that takes data and returns a processed result before storage. - """ - - def __init__(self, name: str, autocast: Callable[[T], Any] | None = None) -> None: - self.name = name - self.autocast = autocast - self.cnt = 0 - - # Create storage directory in the data dir - self.root_dir = _get_data_dir() / name - - # Check if directory exists and is not empty - if self.root_dir.exists(): - existing_files = list(self.root_dir.glob("*.pickle")) - if existing_files: - raise RuntimeError( - f"Storage directory '{name}' already exists and contains {len(existing_files)} files. " - f"Please use a different name or clean the directory first." - ) - else: - # Create the directory - self.root_dir.mkdir(parents=True, exist_ok=True) - - def consume_stream(self, observable: Observable[T | Any]) -> None: - """Consume an observable stream of sensor data without saving.""" - return observable.subscribe(self.save_one) # type: ignore[arg-type, return-value] - - def save_stream(self, observable: Observable[T | Any]) -> Observable[int]: - """Save an observable stream of sensor data to pickle files.""" - return observable.pipe(ops.map(lambda frame: self.save_one(frame))) - - def save(self, *frames) -> int: # type: ignore[no-untyped-def] - """Save one or more frames to pickle files.""" - for frame in frames: - self.save_one(frame) - return self.cnt - - def save_one(self, frame) -> int: # type: ignore[no-untyped-def] - """Save a single frame to a pickle file.""" - file_name = f"{self.cnt:03d}.pickle" - full_path = self.root_dir / file_name - - if full_path.exists(): - raise RuntimeError(f"File {full_path} already exists") - - # Apply autocast if provided - data_to_save = frame - if self.autocast: - data_to_save = self.autocast(frame) - # Convert to raw message if frame has a raw_msg attribute - elif hasattr(frame, "raw_msg"): - data_to_save = frame.raw_msg - - with open(full_path, "wb") as f: - pickle.dump(data_to_save, f) - - self.cnt += 1 - return self.cnt - - -class TimedSensorStorage(SensorStorage[T]): - def save_one(self, frame: T) -> int: - return super().save_one((time.time(), frame)) - - -class TimedSensorReplay(SensorReplay[T]): - def load_one(self, name: int | str | Path) -> T | Any: - if isinstance(name, int): - full_path = self.root_dir / f"/{name:03d}.pickle" - elif isinstance(name, Path): - full_path = name - else: - full_path = self.root_dir / Path(f"{name}.pickle") - - with open(full_path, "rb") as f: - data = pickle.load(f) - if self.autocast: - return (data[0], self.autocast(data[1])) - return data - - def find_closest(self, timestamp: float, tolerance: float | None = None) -> T | Any | None: - """Find the frame closest to the given timestamp. - - Args: - timestamp: The target timestamp to search for - tolerance: Optional maximum time difference allowed - - Returns: - The data frame closest to the timestamp, or None if no match within tolerance - """ - closest_data = None - closest_diff = float("inf") - - # Check frames before and after the timestamp - for ts, data in self.iterate_ts(): - diff = abs(ts - timestamp) - - if diff < closest_diff: - closest_diff = diff - closest_data = data - elif diff > closest_diff: - # We're moving away from the target, can stop - break - - if tolerance is not None and closest_diff > tolerance: - return None - - return closest_data - - def find_closest_seek( - self, relative_seconds: float, tolerance: float | None = None - ) -> T | Any | None: - """Find the frame closest to a time relative to the start. - - Args: - relative_seconds: Seconds from the start of the dataset - tolerance: Optional maximum time difference allowed - - Returns: - The data frame closest to the relative timestamp, or None if no match within tolerance - """ - # Get the first timestamp - first_ts = self.first_timestamp() - if first_ts is None: - return None - - # Calculate absolute timestamp and use find_closest - target_timestamp = first_ts + relative_seconds - return self.find_closest(target_timestamp, tolerance) - - def first_timestamp(self) -> float | None: - """Get the timestamp of the first item in the dataset. - - Returns: - The first timestamp, or None if dataset is empty - """ - try: - ts, _ = next(self.iterate_ts()) - return ts - except StopIteration: - return None - - def iterate(self, loop: bool = False) -> Iterator[T | Any]: - return (x[1] for x in super().iterate(loop=loop)) # type: ignore[index] - - def iterate_duration(self, **kwargs: Any) -> Iterator[tuple[float, T] | Any]: - """Iterate with timestamps relative to the start of the dataset.""" - first_ts = self.first_timestamp() - if first_ts is None: - return - for ts, data in self.iterate_ts(**kwargs): - yield (ts - first_ts, data) - - def iterate_realtime(self, speed: float = 1.0, **kwargs: Any) -> Iterator[T | Any]: - """Iterate data, sleeping to match original timing. - - Args: - speed: Playback speed multiplier (1.0 = realtime, 2.0 = 2x speed) - **kwargs: Passed to iterate_ts (seek, duration, from_timestamp, loop) - """ - iterator = self.iterate_ts(**kwargs) - - try: - first_ts, first_data = next(iterator) - except StopIteration: - return - - start_time = time.time() - start_ts = first_ts - yield first_data - - for ts, data in iterator: - target_time = start_time + (ts - start_ts) / speed - sleep_duration = target_time - time.time() - if sleep_duration > 0: - time.sleep(sleep_duration) - yield data - - def iterate_ts( - self, - seek: float | None = None, - duration: float | None = None, - from_timestamp: float | None = None, - loop: bool = False, - ) -> Iterator[tuple[float, T] | Any]: - """Iterate with absolute timestamps, with optional seek and duration.""" - first_ts = None - if (seek is not None) or (duration is not None): - first_ts = self.first_timestamp() - if first_ts is None: - return - - if seek is not None: - from_timestamp = first_ts + seek # type: ignore[operator] - - end_timestamp = None - if duration is not None: - end_timestamp = (from_timestamp if from_timestamp else first_ts) + duration # type: ignore[operator] - - while True: - for ts, data in super().iterate(): # type: ignore[misc] - if from_timestamp is None or ts >= from_timestamp: - if end_timestamp is not None and ts >= end_timestamp: - break - yield (ts, data) - if not loop: - break - - def stream( # type: ignore[override] - self, - speed: float = 1.0, - seek: float | None = None, - duration: float | None = None, - from_timestamp: float | None = None, - loop: bool = False, - ) -> Observable[T | Any]: - def _subscribe(observer, scheduler=None): # type: ignore[no-untyped-def] - from reactivex.disposable import CompositeDisposable, Disposable - - scheduler = scheduler or TimeoutScheduler() - disp = CompositeDisposable() - is_disposed = False - - iterator = self.iterate_ts( - seek=seek, duration=duration, from_timestamp=from_timestamp, loop=loop - ) - - # Get first message - try: - first_ts, first_data = next(iterator) - except StopIteration: - observer.on_completed() - return Disposable() - - # Establish timing reference - start_local_time = time.time() - start_replay_time = first_ts - - # Emit first sample immediately - observer.on_next(first_data) - - # Pre-load next message - try: - next_message = next(iterator) - except StopIteration: - observer.on_completed() - return disp - - def schedule_emission(message) -> None: # type: ignore[no-untyped-def] - nonlocal next_message, is_disposed - - if is_disposed: - return - - ts, data = message - - # Pre-load the following message while we have time - try: - next_message = next(iterator) - except StopIteration: - next_message = None - - # Calculate absolute emission time - target_time = start_local_time + (ts - start_replay_time) / speed - delay = max(0.0, target_time - time.time()) - - def emit() -> None: - if is_disposed: - return - observer.on_next(data) - if next_message is not None: - schedule_emission(next_message) - else: - observer.on_completed() - # Dispose of the scheduler to clean up threads - if hasattr(scheduler, "dispose"): - scheduler.dispose() - - scheduler.schedule_relative(delay, lambda sc, _: emit()) - - schedule_emission(next_message) - - # Create a custom disposable that properly cleans up - def dispose() -> None: - nonlocal is_disposed - is_disposed = True - disp.dispose() - # Ensure scheduler is disposed to clean up any threads - if hasattr(scheduler, "dispose"): - scheduler.dispose() - - return Disposable(dispose) - - from reactivex import create - - return create(_subscribe) +SensorReplay = LegacyPickleStore +SensorStorage = LegacyPickleStore +TimedSensorReplay = LegacyPickleStore +TimedSensorStorage = LegacyPickleStore diff --git a/dimos/utils/testing/test_replay.py b/dimos/utils/testing/test_replay.py index 6b16525148..e3020777b4 100644 --- a/dimos/utils/testing/test_replay.py +++ b/dimos/utils/testing/test_replay.py @@ -23,27 +23,9 @@ from dimos.utils.testing import replay -def test_sensor_replay() -> None: - counter = 0 - for message in replay.SensorReplay(name="office_lidar").iterate(): - counter += 1 - assert isinstance(message, dict) - assert counter == 500 - - -def test_sensor_replay_cast() -> None: - counter = 0 - for message in replay.SensorReplay( - name="office_lidar", autocast=pointcloud2_from_webrtc_lidar - ).iterate(): - counter += 1 - assert isinstance(message, PointCloud2) - assert counter == 500 - - def test_timed_sensor_replay() -> None: get_data("unitree_office_walk") - odom_store = replay.TimedSensorReplay("unitree_office_walk/odom", autocast=Odometry.from_msg) + odom_store = replay.TimedSensorReplay("unitree_office_walk/odom") itermsgs = [] for msg in odom_store.iterate(): @@ -87,7 +69,7 @@ def test_iterate_ts_no_seek() -> None: def test_iterate_ts_with_from_timestamp() -> None: """Test iterate_ts with from_timestamp (absolute timestamp)""" - odom_store = replay.TimedSensorReplay("unitree_office_walk/odom", autocast=Odometry.from_msg) + odom_store = replay.TimedSensorReplay("unitree_office_walk/odom") # First get all messages to find a good seek point all_msgs = [] @@ -115,7 +97,7 @@ def test_iterate_ts_with_from_timestamp() -> None: def test_iterate_ts_with_relative_seek() -> None: """Test iterate_ts with seek (relative seconds after first timestamp)""" - odom_store = replay.TimedSensorReplay("unitree_office_walk/odom", autocast=Odometry.from_msg) + odom_store = replay.TimedSensorReplay("unitree_office_walk/odom") # Get first few messages to understand timing all_msgs = [] @@ -144,7 +126,7 @@ def test_iterate_ts_with_relative_seek() -> None: def test_stream_with_seek() -> None: """Test stream method with seek parameters""" - odom_store = replay.TimedSensorReplay("unitree_office_walk/odom", autocast=Odometry.from_msg) + odom_store = replay.TimedSensorReplay("unitree_office_walk/odom") # Test stream with relative seek msgs_with_seek = [] @@ -170,7 +152,7 @@ def test_stream_with_seek() -> None: def test_duration_with_loop() -> None: """Test duration parameter with looping in TimedSensorReplay""" - odom_store = replay.TimedSensorReplay("unitree_office_walk/odom", autocast=Odometry.from_msg) + odom_store = replay.TimedSensorReplay("unitree_office_walk/odom") # Collect timestamps from a small duration window collected_ts = [] diff --git a/docker/navigation/.env.hardware b/docker/navigation/.env.hardware index 05e08bd375..234e58545c 100644 --- a/docker/navigation/.env.hardware +++ b/docker/navigation/.env.hardware @@ -57,6 +57,43 @@ MOTOR_SERIAL_DEVICE=/dev/ttyACM0 # Set to true if using wireless base station ENABLE_WIFI_BUFFER=false +# ============================================ +# Unitree Robot Configuration +# ============================================ +# Enable Unitree WebRTC control (for Go2, G1) +#USE_UNITREE=true + +# Unitree robot IP address +UNITREE_IP=192.168.12.1 + +# Unitree connection method (LocalAP or Ethernet) +UNITREE_CONN=LocalAP + +# ============================================ +# Navigation Options +# ============================================ +# Enable route planner (FAR planner for goal navigation) +USE_ROUTE_PLANNER=false + +# Enable RViz visualization +USE_RVIZ=false + +# Map path for localization mode (leave empty for SLAM/mapping mode) +# Set to file prefix (no .pcd extension), e.g., /ros2_ws/maps/warehouse +# The system will load: MAP_PATH.pcd for SLAM, MAP_PATH_tomogram.pickle for PCT planner +MAP_PATH= + +# ============================================ +# Device Group IDs +# ============================================ +# Group ID for /dev/input devices (joystick) +# Find with: getent group input | cut -d: -f3 +INPUT_GID=995 + +# Group ID for serial devices +# Find with: getent group dialout | cut -d: -f3 +DIALOUT_GID=20 + # ============================================ # Display Configuration # ============================================ diff --git a/docker/navigation/Dockerfile b/docker/navigation/Dockerfile index 08a24c0128..fa51fd621c 100644 --- a/docker/navigation/Dockerfile +++ b/docker/navigation/Dockerfile @@ -1,28 +1,40 @@ # ============================================================================= -# OPTIMIZED DOCKERFILE - Multi-stage build for reduced image size +# DimOS Navigation Docker Image # ============================================================================= # -# Key optimizations: -# 1. Multi-stage build with ros:desktop-full base image -# 2. Multi-stage build to discard build artifacts -# 3. No Python ML dependencies (~14 GB saved) -# 4. COPY dimos source for editable pip install (volume-mounted at runtime overlays it) -# 5. Clean up build directories after compile (~800 MB saved) -# 6. Minimal apt packages with --no-install-recommends -# 7. DDS configuration for optimized ROS 2 communication +# Multi-stage build for ROS 2 navigation with SLAM support. +# Includes both arise_slam and FASTLIO2 - select at runtime via LOCALIZATION_METHOD. # -# Supported ROS distributions: jazzy, humble -# Build with: docker build --build-arg ROS_DISTRO=humble ... +# Supported configurations: +# - ROS distributions: humble, jazzy +# - SLAM methods: arise_slam (default), fastlio (set LOCALIZATION_METHOD=fastlio) +# +# Build: +# ./build.sh --humble # Build for ROS 2 Humble +# ./build.sh --jazzy # Build for ROS 2 Jazzy +# +# Run: +# ./start.sh --hardware --route-planner # Uses arise_slam +# LOCALIZATION_METHOD=fastlio ./start.sh --hardware --route-planner # Uses FASTLIO2 # # ============================================================================= # Build argument for ROS distribution (default: humble) ARG ROS_DISTRO=humble +ARG TARGETARCH + +# ----------------------------------------------------------------------------- +# Platform-specific base images +# - amd64: Use osrf/ros desktop-full (includes Gazebo, full GUI) +# - arm64: Use ros-base (desktop-full not available for ARM) +# ----------------------------------------------------------------------------- +FROM osrf/ros:${ROS_DISTRO}-desktop-full AS base-amd64 +FROM ros:${ROS_DISTRO}-ros-base AS base-arm64 # ----------------------------------------------------------------------------- # STAGE 1: Build Stage - compile all C++ dependencies # ----------------------------------------------------------------------------- -FROM osrf/ros:${ROS_DISTRO}-desktop-full AS builder +FROM base-${TARGETARCH} AS builder ARG ROS_DISTRO ENV DEBIAN_FRONTEND=noninteractive @@ -48,12 +60,103 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ ros-${ROS_DISTRO}-cv-bridge \ && rm -rf /var/lib/apt/lists/* +# On arm64, ros-base doesn't include rviz2 (unlike desktop-full on amd64) +# Install it separately for building rviz plugins +# Note: ARG must be re-declared after FROM; placed here to maximize layer caching above +ARG TARGETARCH +RUN if [ "${TARGETARCH}" = "arm64" ]; then \ + apt-get update && apt-get install -y --no-install-recommends \ + ros-${ROS_DISTRO}-rviz2 \ + && rm -rf /var/lib/apt/lists/*; \ + fi + +# On arm64, build open3d from source (no Linux aarch64 wheels on PyPI) +# Cached as a separate layer; the wheel is copied to the runtime stage +# mkdir runs unconditionally so COPY --from=builder works on all architectures +RUN mkdir -p /opt/open3d-wheel && \ + PYTHON_MINOR=$(python3 -c "import sys; print(sys.version_info.minor)") && \ + if [ "${TARGETARCH}" = "arm64" ] && [ "$PYTHON_MINOR" -ge 12 ]; then \ + echo "Building open3d from source for arm64 + Python 3.${PYTHON_MINOR} (no PyPI wheel)..." && \ + apt-get update && apt-get install -y --no-install-recommends \ + python3-dev \ + python3-pip \ + python3-setuptools \ + python3-wheel \ + libblas-dev \ + liblapack-dev \ + libgl1-mesa-dev \ + libglib2.0-dev \ + libxinerama-dev \ + libxcursor-dev \ + libxrandr-dev \ + libxi-dev \ + gfortran \ + && rm -rf /var/lib/apt/lists/* && \ + cd /tmp && \ + git clone --depth 1 --branch v0.19.0 https://github.com/isl-org/Open3D.git && \ + cd Open3D && \ + util/install_deps_ubuntu.sh assume-yes && \ + mkdir build && cd build && \ + cmake .. \ + -DCMAKE_BUILD_TYPE=Release \ + -DBUILD_CUDA_MODULE=OFF \ + -DBUILD_GUI=OFF \ + -DBUILD_TENSORFLOW_OPS=OFF \ + -DBUILD_PYTORCH_OPS=OFF \ + -DBUILD_UNIT_TESTS=OFF \ + -DBUILD_BENCHMARKS=OFF \ + -DBUILD_EXAMPLES=OFF \ + -DBUILD_WEBRTC=OFF && \ + make -j$(($(nproc) > 4 ? 4 : $(nproc))) && \ + make pip-package -j$(($(nproc) > 4 ? 4 : $(nproc))) && \ + mkdir -p /opt/open3d-wheel && \ + cp lib/python_package/pip_package/open3d*.whl /opt/open3d-wheel/ && \ + cd / && rm -rf /tmp/Open3D; \ + fi + +# On arm64, build or-tools from source (pre-built binaries are x86_64 only) +# This is cached as a separate layer since it takes significant time to build +ENV OR_TOOLS_VERSION=9.8 +RUN if [ "${TARGETARCH}" = "arm64" ]; then \ + echo "Building or-tools v${OR_TOOLS_VERSION} from source for arm64..." && \ + apt-get update && apt-get install -y --no-install-recommends \ + lsb-release \ + wget \ + && rm -rf /var/lib/apt/lists/* && \ + cd /tmp && \ + wget -q https://github.com/google/or-tools/archive/refs/tags/v${OR_TOOLS_VERSION}.tar.gz && \ + tar xzf v${OR_TOOLS_VERSION}.tar.gz && \ + cd or-tools-${OR_TOOLS_VERSION} && \ + cmake -S . -B build \ + -DCMAKE_BUILD_TYPE=Release \ + -DBUILD_DEPS=ON \ + -DBUILD_SAMPLES=OFF \ + -DBUILD_EXAMPLES=OFF \ + -DBUILD_FLATZINC=OFF \ + -DUSE_SCIP=OFF \ + -DUSE_COINOR=OFF && \ + cmake --build build --config Release -j$(($(nproc) > 4 ? 4 : $(nproc))) && \ + cmake --install build --prefix /opt/or-tools && \ + rm -rf /tmp/or-tools-${OR_TOOLS_VERSION} /tmp/v${OR_TOOLS_VERSION}.tar.gz; \ + fi + # Create workspace RUN mkdir -p ${WORKSPACE}/src # Copy autonomy stack source COPY docker/navigation/ros-navigation-autonomy-stack ${WORKSPACE}/src/ros-navigation-autonomy-stack +# On arm64, replace pre-built x86_64 or-tools with arm64 built version +RUN if [ "${TARGETARCH}" = "arm64" ] && [ -d "/opt/or-tools" ]; then \ + echo "Replacing x86_64 or-tools with arm64 build..." && \ + OR_TOOLS_DIR=${WORKSPACE}/src/ros-navigation-autonomy-stack/src/exploration_planner/tare_planner/or-tools && \ + rm -rf ${OR_TOOLS_DIR}/lib/*.so* ${OR_TOOLS_DIR}/lib/*.a && \ + cp -r /opt/or-tools/lib/* ${OR_TOOLS_DIR}/lib/ && \ + rm -rf ${OR_TOOLS_DIR}/include && \ + cp -r /opt/or-tools/include ${OR_TOOLS_DIR}/ && \ + ldconfig; \ + fi + # Compatibility fix: In Humble, cv_bridge uses .h extension, but Jazzy uses .hpp # Create a symlink so code written for Jazzy works on Humble RUN if [ "${ROS_DISTRO}" = "humble" ]; then \ @@ -91,22 +194,26 @@ RUN cd ${WORKSPACE}/src/ros-navigation-autonomy-stack/src/slam/dependency/gtsam make -j$(nproc) && make install && ldconfig && \ rm -rf ${WORKSPACE}/src/ros-navigation-autonomy-stack/src/slam/dependency/gtsam/build -# Build ROS workspace (no --symlink-install for multi-stage build compatibility) +# Build ROS workspace with both SLAM systems (no --symlink-install for multi-stage build compatibility) RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash && \ cd ${WORKSPACE} && \ + echo 'Building with both arise_slam and FASTLIO2' && \ colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release" # ----------------------------------------------------------------------------- # STAGE 2: Runtime Stage - minimal image for running # ----------------------------------------------------------------------------- ARG ROS_DISTRO -FROM osrf/ros:${ROS_DISTRO}-desktop-full AS runtime +ARG TARGETARCH +FROM base-${TARGETARCH} AS runtime ARG ROS_DISTRO ENV DEBIAN_FRONTEND=noninteractive ENV ROS_DISTRO=${ROS_DISTRO} ENV WORKSPACE=/ros2_ws ENV DIMOS_PATH=/workspace/dimos +# LOCALIZATION_METHOD: arise_slam (default) or fastlio +ENV LOCALIZATION_METHOD=arise_slam # DDS Configuration - Use FastDDS (default ROS 2 middleware) ENV RMW_IMPLEMENTATION=rmw_fastrtps_cpp @@ -120,6 +227,7 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ ros-${ROS_DISTRO}-foxglove-bridge \ ros-${ROS_DISTRO}-rviz2 \ ros-${ROS_DISTRO}-rqt* \ + ros-${ROS_DISTRO}-joy \ # DDS middleware (FastDDS is default, just ensure it's installed) ros-${ROS_DISTRO}-rmw-fastrtps-cpp \ # Runtime libraries @@ -164,9 +272,26 @@ COPY --from=builder ${WORKSPACE}/install ${WORKSPACE}/install COPY --from=builder ${WORKSPACE}/src/ros-navigation-autonomy-stack/src/base_autonomy/vehicle_simulator/rviz ${WORKSPACE}/src/ros-navigation-autonomy-stack/src/base_autonomy/vehicle_simulator/rviz COPY --from=builder ${WORKSPACE}/src/ros-navigation-autonomy-stack/src/route_planner/far_planner/rviz ${WORKSPACE}/src/ros-navigation-autonomy-stack/src/route_planner/far_planner/rviz COPY --from=builder ${WORKSPACE}/src/ros-navigation-autonomy-stack/src/exploration_planner/tare_planner/rviz ${WORKSPACE}/src/ros-navigation-autonomy-stack/src/exploration_planner/tare_planner/rviz -COPY --from=builder ${WORKSPACE}/src/ros-navigation-autonomy-stack/src/slam/arise_slam_mid360/config ${WORKSPACE}/src/ros-navigation-autonomy-stack/src/slam/arise_slam_mid360/config +# Copy SLAM config files based on SLAM_TYPE COPY --from=builder ${WORKSPACE}/src/ros-navigation-autonomy-stack/src/utilities/livox_ros_driver2/config ${WORKSPACE}/src/ros-navigation-autonomy-stack/src/utilities/livox_ros_driver2/config +# Copy config files for both SLAM systems +RUN --mount=from=builder,source=${WORKSPACE}/src/ros-navigation-autonomy-stack/src,target=/tmp/src \ + echo "Copying arise_slam configs" && \ + mkdir -p ${WORKSPACE}/src/ros-navigation-autonomy-stack/src/slam/arise_slam_mid360 && \ + cp -r /tmp/src/slam/arise_slam_mid360/config ${WORKSPACE}/src/ros-navigation-autonomy-stack/src/slam/arise_slam_mid360/ 2>/dev/null || true && \ + echo "Copying FASTLIO2 configs" && \ + mkdir -p ${WORKSPACE}/src/ros-navigation-autonomy-stack/src/slam/FASTLIO2_ROS2 && \ + for pkg in fastlio2 localizer pgo hba; do \ + if [ -d "/tmp/src/slam/FASTLIO2_ROS2/$pkg/config" ]; then \ + mkdir -p ${WORKSPACE}/src/ros-navigation-autonomy-stack/src/slam/FASTLIO2_ROS2/$pkg && \ + cp -r /tmp/src/slam/FASTLIO2_ROS2/$pkg/config ${WORKSPACE}/src/ros-navigation-autonomy-stack/src/slam/FASTLIO2_ROS2/$pkg/; \ + fi; \ + if [ -d "/tmp/src/slam/FASTLIO2_ROS2/$pkg/rviz" ]; then \ + cp -r /tmp/src/slam/FASTLIO2_ROS2/$pkg/rviz ${WORKSPACE}/src/ros-navigation-autonomy-stack/src/slam/FASTLIO2_ROS2/$pkg/; \ + fi; \ + done + # Copy simulation shell scripts (real robot mode uses volume mount) COPY --from=builder ${WORKSPACE}/src/ros-navigation-autonomy-stack/system_simulation*.sh ${WORKSPACE}/src/ros-navigation-autonomy-stack/ @@ -235,6 +360,15 @@ RUN python3 -m venv /opt/dimos-venv && \ /opt/dimos-venv/bin/pip install --no-cache-dir \ pyyaml +# On arm64, install open3d wheel built from source in the builder stage +COPY --from=builder /opt/open3d-wheel /opt/open3d-wheel +ARG TARGETARCH +RUN if [ "${TARGETARCH}" = "arm64" ] && ls /opt/open3d-wheel/open3d*.whl 1>/dev/null 2>&1; then \ + echo "Installing open3d from pre-built arm64 wheel..." && \ + /opt/dimos-venv/bin/pip install --no-cache-dir /opt/open3d-wheel/open3d*.whl && \ + rm -rf /opt/open3d-wheel; \ + fi + # Copy dimos source and install as editable package # The volume mount at runtime will overlay /workspace/dimos, but the editable # install creates a link that will use the volume-mounted files diff --git a/docker/navigation/README.md b/docker/navigation/README.md index 3b0bfe1eec..32483b6512 100644 --- a/docker/navigation/README.md +++ b/docker/navigation/README.md @@ -16,17 +16,17 @@ This is an optimistic overview. Use the commands below for an in depth version. ```bash cd docker/navigation -./build.sh --humble # Build with ROS 2 Humble (default) -# or -./build.sh --jazzy # Build with ROS 2 Jazzy +./build.sh --humble # Build for ROS 2 Humble +./build.sh --jazzy # Build for ROS 2 Jazzy ``` This will: -- Clone the ros-navigation-autonomy-stack repository (matching branch: humble or jazzy) -- Build a Docker image with both ROS and DimOS dependencies -- Set up the environment for both systems +- Clone the ros-navigation-autonomy-stack repository +- Build a Docker image with both arise_slam and FASTLIO2 +- Set up the environment for both ROS and DimOS -The resulting image will be named `dimos_autonomy_stack:humble` or `dimos_autonomy_stack:jazzy` depending on the option used. +The resulting image will be named `dimos_autonomy_stack:{distro}` (e.g., `humble`, `jazzy`). +Select SLAM method at runtime via `--localization arise_slam` or `--localization fastlio`. Note that the build will take a while and produce an image of approximately 24 GB. @@ -35,9 +35,9 @@ Note that the build will take a while and produce an image of approximately 24 G Use the same ROS distribution flag as your build: ```bash -./start.sh --simulation --humble # If built with --humble +./start.sh --simulation --image humble # If built with --humble # or -./start.sh --simulation --jazzy # If built with --jazzy +./start.sh --simulation --image jazzy # If built with --jazzy ```
@@ -114,12 +114,20 @@ ROBOT_IP=192.168.12.1 # For WebRTC local AP mode (optional, need additional wif #### Start with Route Planner automatically -Use --humble or --jazzy matching your build: - ```bash -./start.sh --hardware --humble --route-planner # Run route planner automatically -./start.sh --hardware --humble --route-planner --rviz # Route planner + RViz2 visualization -./start.sh --hardware --humble --dev # Development mode (mount src for config editing) +# arise_slam (default) +./start.sh --hardware --route-planner +./start.sh --hardware --route-planner --rviz + +# FASTLIO2 +./start.sh --hardware --localization fastlio --route-planner +./start.sh --hardware --localization fastlio --route-planner --rviz + +# Jazzy image +./start.sh --hardware --image jazzy --route-planner + +# Development mode (mount src for config editing) +./start.sh --hardware --dev ``` [Foxglove Studio](https://foxglove.dev/download) is the default visualization tool. It's ideal for remote operation - SSH with port forwarding to the robot's mini PC and run commands there: @@ -130,7 +138,7 @@ ssh -L 8765:localhost:8765 user@robot-ip Then on your local machine: 1. Open Foxglove and connect to `ws://localhost:8765` -2. Load the layout from `docker/navigation/Overwatch.json` (Layout menu → Import) +2. Load the layout from `dimos/assets/foxglove_dashboards/Overwatch.json` (Layout menu → Import) 3. Click in the 3D panel to drop a target pose (similar to RViz). The "Autonomy ON" indicator should be green, and "Goal Reached" will show when the robot arrives.
@@ -139,9 +147,9 @@ Then on your local machine: Start the container and leave it open. Use the same ROS distribution flag as your build: ```bash -./start.sh --hardware --humble # If built with --humble +./start.sh --hardware --image humble # If built with --humble # or -./start.sh --hardware --jazzy # If built with --jazzy +./start.sh --hardware --image jazzy # If built with --jazzy ``` It doesn't do anything by default. You have to run commands on it by `exec`-ing: diff --git a/docker/navigation/build.sh b/docker/navigation/build.sh index ea1729fb63..371db08b49 100755 --- a/docker/navigation/build.sh +++ b/docker/navigation/build.sh @@ -29,10 +29,12 @@ while [[ $# -gt 0 ]]; do echo " --jazzy Build with ROS 2 Jazzy" echo " --help, -h Show this help message" echo "" + echo "The image includes both arise_slam and FASTLIO2." + echo "Select SLAM method at runtime via LOCALIZATION_METHOD env var." + echo "" echo "Examples:" - echo " $0 # Build with ROS Humble (default)" + echo " $0 # Build with ROS Humble" echo " $0 --jazzy # Build with ROS Jazzy" - echo " $0 --humble # Build with ROS Humble" exit 0 ;; *) @@ -44,37 +46,54 @@ while [[ $# -gt 0 ]]; do done export ROS_DISTRO +export IMAGE_TAG="${ROS_DISTRO}" echo -e "${GREEN}================================================${NC}" echo -e "${GREEN}Building DimOS + ROS Autonomy Stack Docker Image${NC}" echo -e "${GREEN}ROS Distribution: ${ROS_DISTRO}${NC}" +echo -e "${GREEN}Image Tag: ${IMAGE_TAG}${NC}" +echo -e "${GREEN}SLAM: arise_slam + FASTLIO2 (both included)${NC}" echo -e "${GREEN}================================================${NC}" echo "" SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" cd "$SCRIPT_DIR" -# Clone or checkout ros-navigation-autonomy-stack with dev branch +# Use fastlio2 branch which has both arise_slam and FASTLIO2 +TARGET_BRANCH="fastlio2" +TARGET_REMOTE="origin" +CLONE_URL="https://github.com/dimensionalOS/ros-navigation-autonomy-stack.git" + +# Clone or checkout ros-navigation-autonomy-stack if [ ! -d "ros-navigation-autonomy-stack" ]; then - echo -e "${YELLOW}Cloning ros-navigation-autonomy-stack repository (dev branch)...${NC}" - git clone -b dev git@github.com:dimensionalOS/ros-navigation-autonomy-stack.git + echo -e "${YELLOW}Cloning ros-navigation-autonomy-stack repository (${TARGET_BRANCH} branch)...${NC}" + git clone -b ${TARGET_BRANCH} ${CLONE_URL} ros-navigation-autonomy-stack echo -e "${GREEN}Repository cloned successfully!${NC}" else - # Directory exists, ensure we're on the dev branch + # Directory exists, ensure we're on the correct branch cd ros-navigation-autonomy-stack + CURRENT_BRANCH=$(git branch --show-current) - if [ "$CURRENT_BRANCH" != "dev" ]; then - echo -e "${YELLOW}Switching from ${CURRENT_BRANCH} to dev branch...${NC}" + if [ "$CURRENT_BRANCH" != "${TARGET_BRANCH}" ]; then + echo -e "${YELLOW}Switching from ${CURRENT_BRANCH} to ${TARGET_BRANCH} branch...${NC}" # Stash any local changes (e.g., auto-generated config files) if git stash --quiet 2>/dev/null; then echo -e "${YELLOW}Stashed local changes${NC}" fi - git fetch origin dev - git checkout dev - git pull origin dev - echo -e "${GREEN}Switched to dev branch${NC}" + git fetch ${TARGET_REMOTE} ${TARGET_BRANCH} + git checkout -B ${TARGET_BRANCH} ${TARGET_REMOTE}/${TARGET_BRANCH} + echo -e "${GREEN}Switched to ${TARGET_BRANCH} branch${NC}" else - echo -e "${GREEN}Already on dev branch${NC}" + echo -e "${GREEN}Already on ${TARGET_BRANCH} branch${NC}" + # Check for local changes before pulling latest + if ! git diff --quiet || ! git diff --cached --quiet; then + echo -e "${RED}Local changes detected in ros-navigation-autonomy-stack.${NC}" + echo -e "${RED}Please commit or discard them before building.${NC}" + git status --short + exit 1 + fi + git fetch ${TARGET_REMOTE} ${TARGET_BRANCH} + git reset --hard ${TARGET_REMOTE}/${TARGET_BRANCH} fi cd .. fi @@ -90,7 +109,7 @@ echo -e "${YELLOW}Building Docker image with docker compose...${NC}" echo "This will take a while as it needs to:" echo " - Download base ROS ${ROS_DISTRO^} image" echo " - Install ROS packages and dependencies" -echo " - Build the autonomy stack" +echo " - Build the autonomy stack (arise_slam + FASTLIO2)" echo " - Build Livox-SDK2 for Mid-360 lidar" echo " - Build SLAM dependencies (Sophus, Ceres, GTSAM)" echo " - Install Python dependencies for DimOS" @@ -103,11 +122,12 @@ docker compose -f docker/navigation/docker-compose.yml build echo "" echo -e "${GREEN}============================================${NC}" echo -e "${GREEN}Docker image built successfully!${NC}" -echo -e "${GREEN}Image: dimos_autonomy_stack:${ROS_DISTRO}${NC}" +echo -e "${GREEN}Image: dimos_autonomy_stack:${IMAGE_TAG}${NC}" +echo -e "${GREEN}SLAM: arise_slam + FASTLIO2 (both included)${NC}" echo -e "${GREEN}============================================${NC}" echo "" echo "To run in SIMULATION mode:" -echo -e "${YELLOW} ./start.sh --${ROS_DISTRO}${NC}" +echo -e "${YELLOW} ./start.sh --simulation --${ROS_DISTRO}${NC}" echo "" echo "To run in HARDWARE mode:" echo " 1. Configure your hardware settings in .env file" @@ -115,5 +135,6 @@ echo " (copy from .env.hardware if needed)" echo " 2. Run the hardware container:" echo -e "${YELLOW} ./start.sh --hardware --${ROS_DISTRO}${NC}" echo "" -echo "The script runs in foreground. Press Ctrl+C to stop." +echo "To use FASTLIO2 instead of arise_slam, set LOCALIZATION_METHOD:" +echo -e "${YELLOW} LOCALIZATION_METHOD=fastlio ./start.sh --hardware --${ROS_DISTRO}${NC}" echo "" diff --git a/docker/navigation/docker-compose.yml b/docker/navigation/docker-compose.yml index d2abf73296..6546968757 100644 --- a/docker/navigation/docker-compose.yml +++ b/docker/navigation/docker-compose.yml @@ -7,7 +7,7 @@ services: network: host args: ROS_DISTRO: ${ROS_DISTRO:-humble} - image: dimos_autonomy_stack:${ROS_DISTRO:-humble} + image: dimos_autonomy_stack:${IMAGE_TAG:-humble} container_name: dimos_simulation_container profiles: ["", "simulation"] # Active by default (empty profile) AND with --profile simulation @@ -41,6 +41,8 @@ services: # DDS Configuration (FastDDS) - RMW_IMPLEMENTATION=rmw_fastrtps_cpp - FASTRTPS_DEFAULT_PROFILES_FILE=/ros2_ws/config/fastdds.xml + # Localization method: arise_slam (default) or fastlio + - LOCALIZATION_METHOD=${LOCALIZATION_METHOD:-arise_slam} # Volume mounts volumes: @@ -79,7 +81,7 @@ services: network: host args: ROS_DISTRO: ${ROS_DISTRO:-humble} - image: dimos_autonomy_stack:${ROS_DISTRO:-humble} + image: dimos_autonomy_stack:${IMAGE_TAG:-humble} container_name: dimos_hardware_container profiles: ["hardware"] @@ -121,6 +123,8 @@ services: # DDS Configuration (FastDDS) - RMW_IMPLEMENTATION=rmw_fastrtps_cpp - FASTRTPS_DEFAULT_PROFILES_FILE=/ros2_ws/config/fastdds.xml + # Localization method: arise_slam (default) or fastlio + - LOCALIZATION_METHOD=${LOCALIZATION_METHOD:-arise_slam} # Mid-360 Lidar configuration - LIDAR_INTERFACE=${LIDAR_INTERFACE:-} - LIDAR_COMPUTER_IP=${LIDAR_COMPUTER_IP:-192.168.1.5} @@ -137,6 +141,8 @@ services: # Unitree robot configuration - UNITREE_IP=${UNITREE_IP:-192.168.12.1} - UNITREE_CONN=${UNITREE_CONN:-LocalAP} + # Map path for localization mode (e.g., /ros2_ws/maps/warehouse) + - MAP_PATH=${MAP_PATH:-} # Volume mounts volumes: @@ -151,6 +157,8 @@ services: - ./bagfiles:/ros2_ws/bagfiles:rw # Mount config files for easy editing - ./config:/ros2_ws/config:rw + # Mount maps directory for localization + - ./maps:/ros2_ws/maps:rw # Hardware-specific volumes - ./logs:/ros2_ws/logs:rw - /etc/localtime:/etc/localtime:ro @@ -185,12 +193,25 @@ services: cd /ros2_ws source install/setup.bash source /opt/dimos-venv/bin/activate - if [ "$USE_ROUTE_PLANNER" = "true" ]; then - echo "Starting real robot system WITH route planner..." - ros2 launch vehicle_simulator system_real_robot_with_route_planner.launch.py & + # Launch with SLAM method based on LOCALIZATION_METHOD + if [ "$LOCALIZATION_METHOD" = "fastlio" ]; then + echo "Using FASTLIO2 localization" + if [ "$USE_ROUTE_PLANNER" = "true" ]; then + echo "Starting real robot system WITH route planner..." + ros2 launch vehicle_simulator system_real_robot_with_route_planner.launch.py use_fastlio2:=true & + else + echo "Starting real robot system (base autonomy)..." + ros2 launch vehicle_simulator system_real_robot.launch.py use_fastlio2:=true & + fi else - echo "Starting real robot system (base autonomy)..." - ros2 launch vehicle_simulator system_real_robot.launch.py & + echo "Using arise_slam localization" + if [ "$USE_ROUTE_PLANNER" = "true" ]; then + echo "Starting real robot system WITH route planner..." + ros2 launch vehicle_simulator system_real_robot_with_route_planner.launch.py & + else + echo "Starting real robot system (base autonomy)..." + ros2 launch vehicle_simulator system_real_robot.launch.py & + fi fi sleep 2 if [ "$USE_RVIZ" = "true" ]; then @@ -221,3 +242,112 @@ services: - NET_ADMIN # Network interface configuration - SYS_ADMIN # System operations - SYS_TIME # Time synchronization + + # Bagfile profile - for bagfile playback with use_sim_time=true + dimos_bagfile: + build: + context: ../.. + dockerfile: docker/navigation/Dockerfile + network: host + args: + ROS_DISTRO: ${ROS_DISTRO:-humble} + image: dimos_autonomy_stack:${IMAGE_TAG:-humble} + container_name: dimos_bagfile_container + profiles: ["bagfile"] + + # Shared memory size for ROS 2 FastDDS + shm_size: '8gb' + + # Enable interactive terminal + stdin_open: true + tty: true + + # Network configuration + network_mode: host + + # Use nvidia runtime for GPU acceleration (falls back to runc if not available) + runtime: ${DOCKER_RUNTIME:-nvidia} + + # Environment variables + environment: + - DISPLAY=${DISPLAY} + - QT_X11_NO_MITSHM=1 + - NVIDIA_VISIBLE_DEVICES=${NVIDIA_VISIBLE_DEVICES:-all} + - NVIDIA_DRIVER_CAPABILITIES=${NVIDIA_DRIVER_CAPABILITIES:-all} + - ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-42} + # DDS Configuration (FastDDS) + - RMW_IMPLEMENTATION=rmw_fastrtps_cpp + - FASTRTPS_DEFAULT_PROFILES_FILE=/ros2_ws/config/fastdds.xml + # Localization method: arise_slam (default) or fastlio + - LOCALIZATION_METHOD=${LOCALIZATION_METHOD:-arise_slam} + # Route planner option + - USE_ROUTE_PLANNER=${USE_ROUTE_PLANNER:-false} + # RViz option + - USE_RVIZ=${USE_RVIZ:-false} + # Map path for localization mode (e.g., /ros2_ws/maps/warehouse) + - MAP_PATH=${MAP_PATH:-} + + # Volume mounts + volumes: + # X11 socket for GUI + - /tmp/.X11-unix:/tmp/.X11-unix:rw + - ${HOME}/.Xauthority:/root/.Xauthority:rw + # Mount bagfiles directory + - ./bagfiles:/ros2_ws/bagfiles:rw + # Mount config files for easy editing + - ./config:/ros2_ws/config:rw + # Mount maps directory for localization + - ./maps:/ros2_ws/maps:rw + + # Device access (for joystick controllers) + devices: + - /dev/input:/dev/input + - /dev/dri:/dev/dri + + # Working directory + working_dir: /ros2_ws + + # Command - launch bagfile system (use_sim_time=true by default in launch files) + command: + - bash + - -c + - | + source install/setup.bash + echo "Bagfile playback mode (use_sim_time=true)" + echo "" + echo "Launch files ready. Play your bagfile with:" + echo " ros2 bag play --clock /ros2_ws/bagfiles/" + echo "" + # Launch with SLAM method based on LOCALIZATION_METHOD + if [ "$LOCALIZATION_METHOD" = "fastlio" ]; then + echo "Using FASTLIO2 localization" + if [ "$USE_ROUTE_PLANNER" = "true" ]; then + echo "Starting bagfile system WITH route planner..." + ros2 launch vehicle_simulator system_bagfile_with_route_planner.launch.py use_fastlio2:=true & + else + echo "Starting bagfile system (base autonomy)..." + ros2 launch vehicle_simulator system_bagfile.launch.py use_fastlio2:=true & + fi + else + echo "Using arise_slam localization" + if [ "$USE_ROUTE_PLANNER" = "true" ]; then + echo "Starting bagfile system WITH route planner..." + ros2 launch vehicle_simulator system_bagfile_with_route_planner.launch.py & + else + echo "Starting bagfile system (base autonomy)..." + ros2 launch vehicle_simulator system_bagfile.launch.py & + fi + fi + sleep 2 + if [ "$USE_RVIZ" = "true" ]; then + echo "Starting RViz2..." + if [ "$USE_ROUTE_PLANNER" = "true" ]; then + ros2 run rviz2 rviz2 -d /ros2_ws/src/ros-navigation-autonomy-stack/src/route_planner/far_planner/rviz/default.rviz & + else + ros2 run rviz2 rviz2 -d /ros2_ws/src/ros-navigation-autonomy-stack/src/base_autonomy/vehicle_simulator/rviz/vehicle_simulator.rviz & + fi + fi + # Keep container running + echo "" + echo "Container ready. Waiting for bagfile playback..." + wait diff --git a/docker/navigation/start.sh b/docker/navigation/start.sh index 9b27a1a3ce..be45908a33 100755 --- a/docker/navigation/start.sh +++ b/docker/navigation/start.sh @@ -13,6 +13,7 @@ USE_ROUTE_PLANNER="false" USE_RVIZ="false" DEV_MODE="false" ROS_DISTRO="humble" +LOCALIZATION_METHOD="${LOCALIZATION_METHOD:-arise_slam}" while [[ $# -gt 0 ]]; do case $1 in --hardware) @@ -23,6 +24,10 @@ while [[ $# -gt 0 ]]; do MODE="simulation" shift ;; + --bagfile) + MODE="bagfile" + shift + ;; --route-planner) USE_ROUTE_PLANNER="true" shift @@ -35,35 +40,48 @@ while [[ $# -gt 0 ]]; do DEV_MODE="true" shift ;; - --humble) - ROS_DISTRO="humble" - shift + --image) + if [ -z "$2" ] || [[ "$2" == --* ]]; then + echo -e "${RED}--image requires a value (humble or jazzy)${NC}" + exit 1 + fi + ROS_DISTRO="$2" + shift 2 ;; - --jazzy) - ROS_DISTRO="jazzy" - shift + --localization) + if [ -z "$2" ] || [[ "$2" == --* ]]; then + echo -e "${RED}--localization requires a value (arise_slam or fastlio)${NC}" + exit 1 + fi + LOCALIZATION_METHOD="$2" + shift 2 ;; --help|-h) echo "Usage: $0 [OPTIONS]" echo "" - echo "Options:" - echo " --simulation Start simulation container (default)" - echo " --hardware Start hardware container for real robot" - echo " --route-planner Enable FAR route planner (for hardware mode)" - echo " --rviz Launch RViz2 visualization" - echo " --dev Development mode (mount src for config editing)" - echo " --humble Use ROS 2 Humble image (default)" - echo " --jazzy Use ROS 2 Jazzy image" - echo " --help, -h Show this help message" + echo "Mode (mutually exclusive):" + echo " --simulation Start simulation container (default)" + echo " --hardware Start hardware container" + echo " --bagfile Start bagfile playback container (use_sim_time=true)" + echo "" + echo "Image and localization:" + echo " --image ROS 2 distribution: humble (default), jazzy" + echo " --localization SLAM method: arise_slam (default), fastlio" + echo "" + echo "Additional options:" + echo " --route-planner Enable FAR route planner (for hardware mode)" + echo " --rviz Launch RViz2 visualization" + echo " --dev Development mode (mount src for config editing)" + echo " --help, -h Show this help message" echo "" echo "Examples:" - echo " $0 # Start simulation (Humble)" - echo " $0 --jazzy # Start simulation (Jazzy)" - echo " $0 --hardware # Start hardware (base autonomy, Humble)" - echo " $0 --hardware --jazzy # Start hardware (Jazzy)" - echo " $0 --hardware --route-planner # Hardware with route planner" - echo " $0 --hardware --route-planner --rviz # Hardware with route planner + RViz" - echo " $0 --hardware --dev # Hardware with src mounted for development" + echo " $0 --simulation # Start simulation" + echo " $0 --hardware --image jazzy # Hardware with Jazzy" + echo " $0 --hardware --localization fastlio # Hardware with FASTLIO2" + echo " $0 --hardware --route-planner --rviz # Hardware with route planner + RViz" + echo " $0 --hardware --dev # Hardware with src mounted" + echo " $0 --bagfile # Bagfile playback" + echo " $0 --bagfile --localization fastlio --route-planner # Bagfile with FASTLIO2 + route planner" echo "" echo "Press Ctrl+C to stop the container" exit 0 @@ -77,6 +95,8 @@ while [[ $# -gt 0 ]]; do done export ROS_DISTRO +export LOCALIZATION_METHOD +export IMAGE_TAG="${ROS_DISTRO}" SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" cd "$SCRIPT_DIR" @@ -85,9 +105,14 @@ echo -e "${GREEN}================================================${NC}" echo -e "${GREEN}Starting DimOS Docker Container${NC}" echo -e "${GREEN}Mode: ${MODE}${NC}" echo -e "${GREEN}ROS Distribution: ${ROS_DISTRO}${NC}" +echo -e "${GREEN}ROS Domain ID: ${ROS_DOMAIN_ID:-42}${NC}" +echo -e "${GREEN}Localization: ${LOCALIZATION_METHOD}${NC}" +echo -e "${GREEN}Image Tag: ${IMAGE_TAG}${NC}" echo -e "${GREEN}================================================${NC}" echo "" +# Pull image option removed - use build.sh to build locally + # Hardware-specific checks if [ "$MODE" = "hardware" ]; then # Check if .env file exists @@ -218,10 +243,12 @@ if [ "$MODE" = "hardware" ]; then fi -# Check if the correct ROS distro image exists -if ! docker images | grep -q "dimos_autonomy_stack.*${ROS_DISTRO}"; then - echo -e "${YELLOW}Docker image for ROS ${ROS_DISTRO} not found. Building...${NC}" - ./build.sh --${ROS_DISTRO} +# Check if the image exists +if ! docker images --format '{{.Repository}}:{{.Tag}}' | grep -q "^dimos_autonomy_stack:${IMAGE_TAG}$"; then + echo -e "${RED}Docker image dimos_autonomy_stack:${IMAGE_TAG} not found.${NC}" + echo -e "${YELLOW}Please build it first with:${NC}" + echo -e " ./build.sh --${ROS_DISTRO}" + exit 1 fi # Check for X11 display @@ -268,6 +295,8 @@ fi # Set container name for reference if [ "$MODE" = "hardware" ]; then CONTAINER_NAME="dimos_hardware_container" +elif [ "$MODE" = "bagfile" ]; then + CONTAINER_NAME="dimos_bagfile_container" else CONTAINER_NAME="dimos_simulation_container" fi @@ -303,6 +332,27 @@ if [ "$MODE" = "hardware" ]; then echo "" echo "To enter the container from another terminal:" echo -e " ${YELLOW}docker exec -it ${CONTAINER_NAME} bash${NC}" +elif [ "$MODE" = "bagfile" ]; then + if [ "$USE_ROUTE_PLANNER" = "true" ]; then + echo "Bagfile mode - Starting bagfile playback system WITH route planner" + echo "" + echo "The container will run (use_sim_time=true):" + echo " - ROS navigation stack (system_bagfile_with_route_planner.launch)" + echo " - FAR Planner for goal-based navigation" + else + echo "Bagfile mode - Starting bagfile playback system (base autonomy)" + echo "" + echo "The container will run (use_sim_time=true):" + echo " - ROS navigation stack (system_bagfile.launch)" + fi + if [ "$USE_RVIZ" = "true" ]; then + echo " - RViz2 visualization" + fi + echo "" + echo -e "${YELLOW}Remember to play bagfile with: ros2 bag play --clock ${NC}" + echo "" + echo "To enter the container from another terminal:" + echo -e " ${YELLOW}docker exec -it ${CONTAINER_NAME} bash${NC}" else echo "Simulation mode - Auto-starting ROS simulation and DimOS" echo "" @@ -317,7 +367,14 @@ fi # Note: DISPLAY is now passed directly via environment variable # No need to write RUNTIME_DISPLAY to .env for local host running -# Build compose command with optional dev mode +# Create required directories +if [ "$MODE" = "hardware" ]; then + mkdir -p bagfiles config logs maps +elif [ "$MODE" = "bagfile" ]; then + mkdir -p bagfiles config maps +fi + +# Build compose command COMPOSE_CMD="docker compose -f docker-compose.yml" if [ "$DEV_MODE" = "true" ]; then COMPOSE_CMD="$COMPOSE_CMD -f docker-compose.dev.yml" @@ -325,6 +382,8 @@ fi if [ "$MODE" = "hardware" ]; then $COMPOSE_CMD --profile hardware up +elif [ "$MODE" = "bagfile" ]; then + $COMPOSE_CMD --profile bagfile up else $COMPOSE_CMD up fi diff --git a/docker/python/Dockerfile b/docker/python/Dockerfile index b85404f51a..1572bbc3ba 100644 --- a/docker/python/Dockerfile +++ b/docker/python/Dockerfile @@ -2,8 +2,7 @@ ARG FROM_IMAGE=ghcr.io/dimensionalos/ros:dev FROM ${FROM_IMAGE} # Install basic requirements -RUN apt-get update -RUN apt-get install -y \ +RUN apt-get update && apt-get install -y \ python-is-python3 \ curl \ gnupg2 \ @@ -13,7 +12,7 @@ RUN apt-get install -y \ portaudio19-dev \ git \ mesa-utils \ - libgl1-mesa-glx \ + libgl1 \ libgl1-mesa-dri \ software-properties-common \ libxcb1-dev \ diff --git a/docker/ros/Dockerfile b/docker/ros/Dockerfile index 2eccd643c0..5a0d16656a 100644 --- a/docker/ros/Dockerfile +++ b/docker/ros/Dockerfile @@ -14,8 +14,7 @@ ENV LANG=en_US.UTF-8 ENV ROS_DISTRO=humble # Install basic requirements -RUN apt-get update -RUN apt-get install -y \ +RUN apt-get update && apt-get install -y \ curl \ gnupg2 \ lsb-release \ @@ -24,7 +23,7 @@ RUN apt-get install -y \ portaudio19-dev \ git \ mesa-utils \ - libgl1-mesa-glx \ + libgl1 \ libgl1-mesa-dri \ software-properties-common \ libxcb1-dev \ diff --git a/docs/concepts/blueprints.md b/docs/concepts/blueprints.md index e525537439..54b52ba3c0 100644 --- a/docs/concepts/blueprints.md +++ b/docs/concepts/blueprints.md @@ -201,7 +201,7 @@ blueprint.remappings([ ## Overriding global configuration. -Each module can optionally take a `cfg` option in `__init__`. E.g.: +Each module can optionally take global config as a `cfg` option in `__init__`. E.g.: ```python session=blueprint-ex3 from dimos.core import Module, rpc @@ -210,6 +210,7 @@ from dimos.core.global_config import GlobalConfig class ModuleA(Module): def __init__(self, cfg: GlobalConfig | None = None): + self._global_config: GlobalConfig = cfg ... ``` @@ -279,45 +280,19 @@ class ModuleB(Module): ## Defining skills -Skills have to be registered with `AgentSpec.register_skills(self)`. +Skills are methods on a `Module` decorated with `@skill`. The agent automatically discovers all skills from launched modules at startup. ```python session=blueprint-ex4 from dimos.core import Module, rpc -from dimos.core.skill_module import SkillModule -from dimos.protocol.skill.skill import skill -from dimos.core.rpc_client import RpcCall +from dimos.agents.annotation import skill from dimos.core.global_config import GlobalConfig class SomeSkill(Module): @skill - def some_skill(self) -> None: - ... - - @rpc - def set_AgentSpec_register_skills(self, register_skills: RpcCall) -> None: - register_skills.set_rpc(self.rpc) - register_skills(RPCClient(self, self.__class__)) - - # The agent is just interested in the `@skill` methods, so you'll need this if your class - # has things that cannot be pickled. - def __getstate__(self): - pass - def __setstate__(self, _state): - pass -``` - -Or, you can avoid all of this by inheriting from `SkillModule` which does the above automatically: - -```python session=blueprint-ex4 -from dimos.core.skill_module import SkillModule -from dimos.protocol.skill.skill import skill - -class SomeSkill(SkillModule): - - @skill - def some_skill(self) -> None: - ... + def some_skill(self) -> str: + """Description of the skill for the LLM.""" + return "result" ``` ## Building diff --git a/docs/concepts/modules.md b/docs/concepts/modules.md index 344efa0774..2fefc7eb04 100644 --- a/docs/concepts/modules.md +++ b/docs/concepts/modules.md @@ -50,7 +50,7 @@ print(CameraModule.io()) ├─ RPC start() ├─ RPC stop() │ - ├─ Skill video_stream (stream=passive, reducer=latest_reducer, output=image) + ├─ Skill take_a_picture ``` We can see that the camera module outputs two streams: @@ -60,7 +60,7 @@ We can see that the camera module outputs two streams: It offers two RPC calls: `start()` and `stop()` (lifecycle methods). -It also exposes an agentic [skill](/docs/concepts/blueprints.md#defining-skills) called `video_stream` (more on skills in the Blueprints guide). +It also exposes an agentic [skill](/docs/concepts/blueprints.md#defining-skills) called `take_a_picture` (more on skills in the Blueprints guide). We can start this module and explore the output of its streams in real time (this will use your webcam). diff --git a/docs/concepts/transports.md b/docs/concepts/transports.md index e4b62b01ce..371cb0c2a3 100644 --- a/docs/concepts/transports.md +++ b/docs/concepts/transports.md @@ -148,6 +148,7 @@ if __name__ == "__main__": ``` + ``` Initialized dimos local cluster with 2 workers, memory limit: auto 2026-01-24T13:17:50.190559Z [info ] Deploying module. [dimos/core/__init__.py] module=CameraModule @@ -288,6 +289,41 @@ shm.stop() Received: [{'data': [1, 2, 3]}] ``` +### DDS Transport + +For network communication, DDS uses the Data Distribution Service (DDS) protocol: + +```python session=dds_demo ansi=false +from dataclasses import dataclass +from cyclonedds.idl import IdlStruct + +from dimos.protocol.pubsub.impl.ddspubsub import DDS, Topic + +@dataclass +class SensorReading(IdlStruct): + value: float + +dds = DDS() +dds.start() + +received = [] +sensor_topic = Topic(name="sensors/temperature", data_type=SensorReading) + +dds.subscribe(sensor_topic, lambda msg, t: received.append(msg)) +dds.publish(sensor_topic, SensorReading(value=22.5)) + +import time +time.sleep(0.1) + +print(f"Received: {received}") +dds.stop() +``` + + +``` +Received: [SensorReading(value=22.5)] +``` + --- ## A minimal transport: `Memory` diff --git a/docs/development/README.md b/docs/development/README.md index 0e5a4a2021..e574be9d8f 100644 --- a/docs/development/README.md +++ b/docs/development/README.md @@ -71,6 +71,35 @@ uv add git+https://github.com/openai/CLIP.git uv add git+https://github.com/facebookresearch/detectron2.git ``` +### Optional: DDS Transport Support + +The `dds` extra provides DDS (Data Distribution Service) transport support via [Eclipse Cyclone DDS](https://cyclonedds.io/docs/cyclonedds-python/latest/). This requires installing system libraries before the Python package can be built. + +**Ubuntu/Debian:** + +```bash +# Install the CycloneDDS development library +sudo apt install cyclonedds-dev + +# Create a compatibility directory structure +# (required because Ubuntu's multiarch layout doesn't match the expected CMake layout) +sudo mkdir -p /opt/cyclonedds/{lib,bin,include} +sudo ln -sf /usr/lib/x86_64-linux-gnu/libddsc.so* /opt/cyclonedds/lib/ +sudo ln -sf /usr/lib/x86_64-linux-gnu/libcycloneddsidl.so* /opt/cyclonedds/lib/ +sudo ln -sf /usr/bin/idlc /opt/cyclonedds/bin/ +sudo ln -sf /usr/bin/ddsperf /opt/cyclonedds/bin/ +sudo ln -sf /usr/include/dds /opt/cyclonedds/include/ + +# Install with the dds extra +CYCLONEDDS_HOME=/opt/cyclonedds uv pip install -e '.[dds]' +``` + +To install all extras including DDS: + +```bash +CYCLONEDDS_HOME=/opt/cyclonedds uv sync --extra dds +``` + +``` +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)* | Relative or Absolute path to the native binary | +| `extra_args` | `list[str]` | `[]` | Additional CLI arguments appended after auto-generated ones | +| `extra_env` | `dict[str, str]` | `{}` | Extra environment variables for the subprocess | +| `cwd` | `str \| None` | `None` | Working directory (defaults to executable's parent dir) | +| `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): + executable: str = str(Path(__file__).parent / "cpp" / "result" / "bin" / "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(), +) +``` From e0ab2dbbfdfc810721269ef822493c23c35af1b0 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Sat, 14 Feb 2026 20:47:27 +0800 Subject: [PATCH 39/48] added an automatic build system --- dimos/core/native_module.py | 53 ++++++++++++++++++- .../lidar/fastlio2/fastlio_blueprints.py | 2 +- .../hardware/sensors/lidar/fastlio2/module.py | 5 +- dimos/hardware/sensors/lidar/livox/module.py | 8 +-- docs/usage/native_modules.md | 52 ++++++++++++++---- 5 files changed, 101 insertions(+), 19 deletions(-) diff --git a/dimos/core/native_module.py b/dimos/core/native_module.py index b46ebf38f7..3d883c152e 100644 --- a/dimos/core/native_module.py +++ b/dimos/core/native_module.py @@ -42,6 +42,7 @@ class MyCppModule(NativeModule): from dataclasses import dataclass, field, fields import enum +import inspect import json import os from pathlib import Path @@ -67,9 +68,10 @@ 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) - cwd: str | None = None shutdown_timeout: float = 10.0 log_format: LogFormat = LogFormat.TEXT @@ -125,6 +127,7 @@ class NativeModule(Module[NativeModuleConfig]): def __init__(self, *args: Any, **kwargs: Any) -> None: super().__init__(*args, **kwargs) self._io_threads = [] + self._resolve_paths() @rpc def start(self) -> None: @@ -132,6 +135,8 @@ def start(self) -> None: logger.warning("Native process already running", pid=self._process.pid) return + self._maybe_build() + topics = self._collect_topics() cmd = [self.config.executable] @@ -223,6 +228,52 @@ def _read_log_stream(self, stream: IO[bytes] | None, level: str) -> None: 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_reader = self._start_reader(proc.stdout, "info") + stderr_reader = self._start_reader(proc.stderr, "warning") + proc.wait() + stdout_reader.join(timeout=2) + stderr_reader.join(timeout=2) + 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] = {} diff --git a/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py b/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py index 6ea286500d..05801729e3 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py +++ b/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py @@ -17,7 +17,7 @@ from dimos.mapping.voxels import VoxelGridMapper from dimos.visualization.rerun.bridge import rerun_bridge -voxel_size = 0.15 +voxel_size = 0.05 mid360_fastlio = autoconnect( FastLio2.blueprint(voxel_size=voxel_size, map_voxel_size=voxel_size, map_freq=-1), diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py index 8c641e75b8..ae147fd255 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -51,7 +51,6 @@ from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 # noqa: TC001 from dimos.spec import mapping, perception -_DEFAULT_EXECUTABLE = str(Path(__file__).parent / "cpp" / "result" / "bin" / "fastlio2_native") _CONFIG_DIR = Path(__file__).parent / "config" @@ -59,7 +58,9 @@ class FastLio2Config(NativeModuleConfig): """Config for the FAST-LIO2 + Livox Mid-360 native module.""" - executable: str = _DEFAULT_EXECUTABLE + 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" diff --git a/dimos/hardware/sensors/lidar/livox/module.py b/dimos/hardware/sensors/lidar/livox/module.py index 411ed76ba8..197731b1a6 100644 --- a/dimos/hardware/sensors/lidar/livox/module.py +++ b/dimos/hardware/sensors/lidar/livox/module.py @@ -27,7 +27,6 @@ from __future__ import annotations from dataclasses import dataclass -from pathlib import Path from dimos.core import Out # noqa: TC001 from dimos.core.native_module import NativeModule, NativeModuleConfig @@ -47,14 +46,15 @@ from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 # noqa: TC001 from dimos.spec import perception -_DEFAULT_EXECUTABLE = str(Path(__file__).parent / "cpp" / "result" / "bin" / "mid360_native") - @dataclass(kw_only=True) class Mid360Config(NativeModuleConfig): """Config for the C++ Mid-360 native module.""" - executable: str = _DEFAULT_EXECUTABLE + 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 diff --git a/docs/usage/native_modules.md b/docs/usage/native_modules.md index ea238f10be..8e169be808 100644 --- a/docs/usage/native_modules.md +++ b/docs/usage/native_modules.md @@ -1,18 +1,27 @@ # 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 handles blueprint wiring, lifecycle, and logging. Native binary handles the actual computation, publishing and subscribing directly on 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. ## Supported languages -We have examples of LCM integrations for **C++**, **TypeScript**, and **Lua** here in the repo - see [/examples/language-interop/](/examples/language-interop/README.md) +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) + +Types generated (but no examples yet) for: +- [**C#**](https://github.com/dimensionalOS/dimos-lcm/tree/main/generated/csharp) +- [**Java**](https://github.com/dimensionalOS/dimos-lcm/tree/main/generated/java) -We have our msg types generated for **C#** and **Java** as well — see [dimos-lcm/generated](https://github.com/dimensionalOS/dimos-lcm/tree/main/generated). For pubsub client implementation, look into the [LCM Project](https://github.com/lcm-proj/lcm). +For pubsub client implementation, look into the [LCM Project](https://github.com/lcm-proj/lcm). -LCM is a very simple protocol that's easy to implement with any language, and our types are simple and automatically generated, so you can theoretically add support for anything. +Generally LCM is a very simple protocol that's easy to implement in any language, and our types are [autogenerated](https://github.com/dimensionalOS/dimos-lcm/) ## Defining a native module @@ -49,10 +58,11 @@ That's it. `MyLidar` is a full DimOS module. You can use it with `autoconnect`, When `start()` is called, NativeModule: -1. **Collects topics** from blueprint-assigned transports on each declared port. -2. **Builds the command line**: ` -- ... -- ...` -3. **Launches the subprocess** with `Popen`, piping stdout/stderr. -4. **Starts a watchdog** thread that calls `stop()` if the process crashes. +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: @@ -86,10 +96,11 @@ When `stop()` is called, the process receives SIGTERM. If it doesn't exit within | Field | Type | Default | Description | |--------------------|------------------|---------------|-------------------------------------------------------------| -| `executable` | `str` | *(required)* | Relative or Absolute path to the native binary | +| `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 | -| `cwd` | `str \| None` | `None` | Working directory (defaults to executable's parent dir) | | `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 | @@ -239,7 +250,9 @@ from dimos.spec import perception @dataclass(kw_only=True) class Mid360Config(NativeModuleConfig): - executable: str = str(Path(__file__).parent / "cpp" / "result" / "bin" / "mid360_native") + 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 @@ -263,3 +276,20 @@ autoconnect( 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. From e5d1184a78356add748f7dde4d9221ecaae49819 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Sat, 14 Feb 2026 20:58:33 +0800 Subject: [PATCH 40/48] moved native echo, small readme updates --- dimos/core/test_native_module.py | 2 +- dimos/core/{ => tests}/native_echo.py | 0 docs/usage/native_modules.md | 9 +++------ 3 files changed, 4 insertions(+), 7 deletions(-) rename dimos/core/{ => tests}/native_echo.py (100%) diff --git a/dimos/core/test_native_module.py b/dimos/core/test_native_module.py index a6c55047e9..2d024cb451 100644 --- a/dimos/core/test_native_module.py +++ b/dimos/core/test_native_module.py @@ -37,7 +37,7 @@ from dimos.msgs.sensor_msgs.Imu import Imu from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -_ECHO = str(Path(__file__).parent / "native_echo.py") +_ECHO = str(Path(__file__).parent / "tests" / "native_echo.py") @pytest.fixture diff --git a/dimos/core/native_echo.py b/dimos/core/tests/native_echo.py similarity index 100% rename from dimos/core/native_echo.py rename to dimos/core/tests/native_echo.py diff --git a/docs/usage/native_modules.md b/docs/usage/native_modules.md index 8e169be808..751a2e9432 100644 --- a/docs/usage/native_modules.md +++ b/docs/usage/native_modules.md @@ -11,17 +11,14 @@ Python module **never touches the pubsub data**. It just passes configuration an ## Supported languages 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) +[**C++**](/examples/language-interop/cpp/README.md), [**TypeScript**](/examples/language-interop/ts/README.md) and [**Lua**](/examples/language-interop/lua/README.md) Types generated (but no examples yet) for: -- [**C#**](https://github.com/dimensionalOS/dimos-lcm/tree/main/generated/csharp) -- [**Java**](https://github.com/dimensionalOS/dimos-lcm/tree/main/generated/java) +[**C#**](https://github.com/dimensionalOS/dimos-lcm/tree/main/generated/csharp) and [**Java**](https://github.com/dimensionalOS/dimos-lcm/tree/main/generated/java) For pubsub client implementation, look into the [LCM Project](https://github.com/lcm-proj/lcm). -Generally LCM is a very simple protocol that's easy to implement in any language, and our types are [autogenerated](https://github.com/dimensionalOS/dimos-lcm/) +Generally LCM is a very simple protocol ([UDP multicast](https://lcm-proj.github.io/lcm/content/udp-multicast-protocol.html#lcm-udp-multicast-protocol-description)) with a simple [message definition language](https://lcm-proj.github.io/lcm/content/lcm-type-ref.html#lcm-type-specification-language), so it's easy to implement in any language, and our types are proted from ROS, autogenerated and defined at [dimos-lcm](https://github.com/dimensionalOS/dimos-lcm/) repository. ## Defining a native module From fdac8b898313f2c4632a517eaaa3d28b3b4356d2 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Sat, 14 Feb 2026 21:32:00 +0800 Subject: [PATCH 41/48] small readme correction --- docs/usage/native_modules.md | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/docs/usage/native_modules.md b/docs/usage/native_modules.md index 751a2e9432..3e2070b6d9 100644 --- a/docs/usage/native_modules.md +++ b/docs/usage/native_modules.md @@ -11,14 +11,16 @@ Python module **never touches the pubsub data**. It just passes configuration an ## Supported languages We have examples of LCM integrations for: -[**C++**](/examples/language-interop/cpp/README.md), [**TypeScript**](/examples/language-interop/ts/README.md) and [**Lua**](/examples/language-interop/lua/README.md) +- [**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) -For pubsub client implementation, look into the [LCM Project](https://github.com/lcm-proj/lcm). - -Generally LCM is a very simple protocol ([UDP multicast](https://lcm-proj.github.io/lcm/content/udp-multicast-protocol.html#lcm-udp-multicast-protocol-description)) with a simple [message definition language](https://lcm-proj.github.io/lcm/content/lcm-type-ref.html#lcm-type-specification-language), so it's easy to implement in any language, and our types are proted from ROS, autogenerated and defined at [dimos-lcm](https://github.com/dimensionalOS/dimos-lcm/) repository. +LCM is a simple [UDP multicast](https://lcm-proj.github.io/lcm/content/udp-multicast-protocol.html#lcm-udp-multicast-protocol-description) protocol with a straightforward [message definition language](https://lcm-proj.github.io/lcm/content/lcm-type-ref.html#lcm-type-specification-language), making it easy to implement in any language. Our types are ported from ROS and autogenerated from the [dimos-lcm](https://github.com/dimensionalOS/dimos-lcm/) repository. ## Defining a native module From 880b617884ca267ca0a96825b470aa7dc19e10ed Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Sat, 14 Feb 2026 21:41:39 +0800 Subject: [PATCH 42/48] moved lcm info to lcm doc --- docs/usage/lcm.md | 23 ++++++++++++++++++++--- docs/usage/native_modules.md | 14 +------------- 2 files changed, 21 insertions(+), 16 deletions(-) diff --git a/docs/usage/lcm.md b/docs/usage/lcm.md index f1b41cfc9f..f934b52b31 100644 --- a/docs/usage/lcm.md +++ b/docs/usage/lcm.md @@ -1,9 +1,26 @@ - # 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 real power of LCM is its message definition format — classes that encode themselves to a compact binary representation. This means LCM messages can be sent over any transport (WebSocket, SSH, shared memory, etc.), not just UDP multicast. + +Our types are ported from ROS (in order to make conversion of our messages to ROS messages easy) and autogenerated from the [dimos-lcm](https://github.com/dimensionalOS/dimos-lcm/) repository. + +## 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 index 3e2070b6d9..5a9362839f 100644 --- a/docs/usage/native_modules.md +++ b/docs/usage/native_modules.md @@ -8,19 +8,7 @@ Python will handle blueprint wiring, lifecycle, and logging. Native binary handl Python module **never touches the pubsub data**. It just passes configuration and LCM topic to use via CLI args to your executable. -## Supported languages - -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) - -LCM is a simple [UDP multicast](https://lcm-proj.github.io/lcm/content/udp-multicast-protocol.html#lcm-udp-multicast-protocol-description) protocol with a straightforward [message definition language](https://lcm-proj.github.io/lcm/content/lcm-type-ref.html#lcm-type-specification-language), making it easy to implement in any language. Our types are ported from ROS and autogenerated from the [dimos-lcm](https://github.com/dimensionalOS/dimos-lcm/) repository. +On how to speak LCM with the rest of dimos, you can read our [LCM intro](/docs/usage/lcm.md) ## Defining a native module From 3142b5bceb232222982d39d7960319f3d5ec3cf6 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Sat, 14 Feb 2026 21:50:16 +0800 Subject: [PATCH 43/48] lcm performance note --- docs/usage/lcm.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/docs/usage/lcm.md b/docs/usage/lcm.md index f934b52b31..764f47b6c2 100644 --- a/docs/usage/lcm.md +++ b/docs/usage/lcm.md @@ -6,6 +6,8 @@ The LCM project provides pubsub clients and code generators for many languages. Our types are ported from ROS (in order to make conversion of our messages to ROS messages easy) and autogenerated from the [dimos-lcm](https://github.com/dimensionalOS/dimos-lcm/) repository. +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: From 507651c798eaa8134cb9de338ed0c70e6b042a6a Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Sat, 14 Feb 2026 22:03:43 +0800 Subject: [PATCH 44/48] small readme fixes --- docs/usage/lcm.md | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/docs/usage/lcm.md b/docs/usage/lcm.md index 764f47b6c2..99437a2458 100644 --- a/docs/usage/lcm.md +++ b/docs/usage/lcm.md @@ -2,9 +2,10 @@ 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 real power of LCM is its message definition format — classes that encode themselves to a compact binary representation. This means LCM messages can be sent over any transport (WebSocket, SSH, shared memory, etc.), not just UDP multicast. +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 types are ported from ROS (in order to make conversion of our messages to ROS messages easy) and autogenerated from the [dimos-lcm](https://github.com/dimensionalOS/dimos-lcm/) repository. +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) From 7c41b6e570afa0dfa8ebe5ea0ba77e60f447ea55 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Mon, 16 Feb 2026 11:56:11 +0000 Subject: [PATCH 45/48] Replace assert_implements_protocol with mypy-level protocol checks Remove runtime assert_implements_protocol and test_spec files. Instead, add TYPE_CHECKING instantiation at the bottom of each module file so mypy catches missing or misnamed protocol ports statically. --- .../hardware/sensors/lidar/fastlio2/module.py | 5 +++ .../sensors/lidar/fastlio2/test_spec.py | 27 ------------ dimos/hardware/sensors/lidar/livox/module.py | 5 +++ .../hardware/sensors/lidar/livox/test_spec.py | 29 ------------- dimos/spec/utils.py | 42 ------------------- 5 files changed, 10 insertions(+), 98 deletions(-) delete mode 100644 dimos/hardware/sensors/lidar/fastlio2/test_spec.py delete mode 100644 dimos/hardware/sensors/lidar/livox/test_spec.py diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py index ae147fd255..ee9a0783a0 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -32,6 +32,7 @@ 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 @@ -141,3 +142,7 @@ class FastLio2(NativeModule, perception.Lidar, perception.Odometry, mapping.Glob "FastLio2Config", "fastlio2_module", ] + +# Verify protocol port compliance (mypy will flag missing ports) +if TYPE_CHECKING: + FastLio2() diff --git a/dimos/hardware/sensors/lidar/fastlio2/test_spec.py b/dimos/hardware/sensors/lidar/fastlio2/test_spec.py deleted file mode 100644 index fbc8a3a065..0000000000 --- a/dimos/hardware/sensors/lidar/fastlio2/test_spec.py +++ /dev/null @@ -1,27 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Spec compliance tests for the FAST-LIO2 module.""" - -from __future__ import annotations - -from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 -from dimos.spec import mapping, perception -from dimos.spec.utils import assert_implements_protocol - - -def test_fastlio2_implements_spec() -> None: - assert_implements_protocol(FastLio2, perception.Lidar) - assert_implements_protocol(FastLio2, perception.Odometry) - assert_implements_protocol(FastLio2, mapping.GlobalPointcloud) diff --git a/dimos/hardware/sensors/lidar/livox/module.py b/dimos/hardware/sensors/lidar/livox/module.py index 197731b1a6..672968a0eb 100644 --- a/dimos/hardware/sensors/lidar/livox/module.py +++ b/dimos/hardware/sensors/lidar/livox/module.py @@ -27,6 +27,7 @@ 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 @@ -97,3 +98,7 @@ class Mid360(NativeModule, perception.Lidar, perception.IMU): "Mid360Config", "mid360_module", ] + +# Verify protocol port compliance (mypy will flag missing ports) +if TYPE_CHECKING: + Mid360() diff --git a/dimos/hardware/sensors/lidar/livox/test_spec.py b/dimos/hardware/sensors/lidar/livox/test_spec.py deleted file mode 100644 index 6e73c8c6b9..0000000000 --- a/dimos/hardware/sensors/lidar/livox/test_spec.py +++ /dev/null @@ -1,29 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Spec compliance tests for the Livox LiDAR module.""" - -from __future__ import annotations - -from dimos.hardware.sensors.lidar.livox.module import Mid360 -from dimos.spec import perception -from dimos.spec.utils import assert_implements_protocol - - -def test_livox_module_implements_lidar_spec() -> None: - assert_implements_protocol(Mid360, perception.Lidar) - - -def test_livox_module_implements_imu_spec() -> None: - assert_implements_protocol(Mid360, perception.IMU) diff --git a/dimos/spec/utils.py b/dimos/spec/utils.py index 0050a0b0c1..b9786b91b5 100644 --- a/dimos/spec/utils.py +++ b/dimos/spec/utils.py @@ -13,7 +13,6 @@ # limitations under the License. import inspect -import typing from typing import Any, Protocol, runtime_checkable from annotation_protocol import AnnotationProtocol # type: ignore[import-not-found,import-untyped] @@ -100,47 +99,6 @@ def foo(self) -> int: ... return isinstance(obj, strict_proto) -def _own_hints(cls: type) -> dict[str, Any]: - """Collect type hints from cls and its non-protocol bases only.""" - hints: dict[str, Any] = {} - for base in reversed(cls.__mro__): - if base is object or is_protocol(base): - continue - base_hints = typing.get_type_hints(base, include_extras=True) - # Only include annotations defined directly on this base - for name in base.__annotations__: - if name in base_hints: - hints[name] = base_hints[name] - return hints - - -def assert_implements_protocol(cls: type, protocol: type) -> None: - """Assert that cls has all annotations required by a Protocol. - - Works with any Protocol (not just Spec subclasses). Checks that every - annotation defined by the protocol is present on cls with a matching type. - Ignores annotations inherited from protocol bases so that inheriting from - a protocol doesn't automatically satisfy the check. - - Example: - class MyProto(Protocol): - x: Out[int] - - class Good: - x: Out[int] - - assert_implements_protocol(Good, MyProto) # passes - """ - proto_hints = typing.get_type_hints(protocol, include_extras=True) - cls_hints = _own_hints(cls) - - for name, expected_type in proto_hints.items(): - assert name in cls_hints, f"{cls.__name__} missing '{name}' required by {protocol.__name__}" - assert cls_hints[name] == expected_type, ( - f"{cls.__name__}.{name}: expected {expected_type}, got {cls_hints[name]}" - ) - - def get_protocol_method_signatures(proto: type[object]) -> dict[str, inspect.Signature]: """ Return a mapping of method_name -> inspect.Signature From 7a755593102c72a46f6f05ffc1217db11e7edd2f Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Mon, 16 Feb 2026 20:15:22 +0800 Subject: [PATCH 46/48] Address PR review: simplify NativeModule threads and test fixture Move reader thread spawning into the watchdog thread so only one thread is externally managed. Use tmp_path pytest fixture instead of manual tempfile. Use communicate() for build subprocess. --- dimos/core/native_module.py | 39 ++++++++++++++++---------------- dimos/core/test_native_module.py | 17 ++++---------- dimos/core/tests/native_echo.py | 9 ++++---- 3 files changed, 28 insertions(+), 37 deletions(-) diff --git a/dimos/core/native_module.py b/dimos/core/native_module.py index 3d883c152e..c871392d57 100644 --- a/dimos/core/native_module.py +++ b/dimos/core/native_module.py @@ -120,13 +120,11 @@ class NativeModule(Module[NativeModuleConfig]): default_config: type[NativeModuleConfig] = NativeModuleConfig # type: ignore[assignment] _process: subprocess.Popen[bytes] | None = None - _io_threads: list[threading.Thread] _watchdog: threading.Thread | None = None _stopping: bool = False def __init__(self, *args: Any, **kwargs: Any) -> None: super().__init__(*args, **kwargs) - self._io_threads = [] self._resolve_paths() @rpc @@ -159,10 +157,6 @@ def start(self) -> None: logger.info("Native process started", pid=self._process.pid) self._stopping = False - self._io_threads = [ - self._start_reader(self._process.stdout, "info"), - self._start_reader(self._process.stderr, "warning"), - ] self._watchdog = threading.Thread(target=self._watch_process, daemon=True) self._watchdog.start() @@ -180,26 +174,23 @@ def stop(self) -> None: ) self._process.kill() self._process.wait(timeout=5) - for t in self._io_threads: - t.join(timeout=2) if self._watchdog is not None and self._watchdog is not threading.current_thread(): self._watchdog.join(timeout=2) self._watchdog = None - self._io_threads = [] self._process = None super().stop() - def _start_reader(self, stream: IO[bytes] | None, level: str) -> threading.Thread: - """Spawn a daemon thread that pipes a subprocess stream through the logger.""" - t = threading.Thread(target=self._read_log_stream, args=(stream, level), daemon=True) - t.start() - return t - def _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( @@ -209,6 +200,12 @@ def _watch_process(self) -> None: ) 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 @@ -260,11 +257,13 @@ def _maybe_build(self) -> None: stdout=subprocess.PIPE, stderr=subprocess.PIPE, ) - stdout_reader = self._start_reader(proc.stdout, "info") - stderr_reader = self._start_reader(proc.stderr, "warning") - proc.wait() - stdout_reader.join(timeout=2) - stderr_reader.join(timeout=2) + 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}" diff --git a/dimos/core/test_native_module.py b/dimos/core/test_native_module.py index 2d024cb451..8bf50ea7a3 100644 --- a/dimos/core/test_native_module.py +++ b/dimos/core/test_native_module.py @@ -20,9 +20,7 @@ from dataclasses import dataclass import json -import os from pathlib import Path -import tempfile import time import pytest @@ -41,16 +39,9 @@ @pytest.fixture -def args_file(): - """Temp file where native_echo.py writes the CLI args it received.""" - fd, path = tempfile.mkstemp(suffix=".json", prefix="native_echo_") - os.close(fd) - os.unlink(path) - try: - yield path - finally: - if os.path.exists(path): - os.unlink(path) +def 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]: @@ -70,7 +61,7 @@ def read_json_file(path: str) -> dict[str, str]: @dataclass(kw_only=True) class StubNativeConfig(NativeModuleConfig): executable: str = _ECHO - log_format: LogFormat = LogFormat.JSON + log_format: LogFormat = LogFormat.TEXT some_param: float = 1.5 diff --git a/dimos/core/tests/native_echo.py b/dimos/core/tests/native_echo.py index 982d269e3c..b011ff3b8c 100755 --- a/dimos/core/tests/native_echo.py +++ b/dimos/core/tests/native_echo.py @@ -28,16 +28,17 @@ import sys import time +print("this mesasage goes to stdout") +print("this message goes to stderr", file=sys.stderr) + signal.signal(signal.SIGTERM, lambda *_: sys.exit(0)) output_path = os.environ.get("NATIVE_ECHO_OUTPUT") if output_path: with open(output_path, "w") as f: json.dump(sys.argv[1:], f) -else: - json.dump({"event": "echo_args", "args": sys.argv[1:]}, sys.stdout) - sys.stdout.write("\n") - sys.stdout.flush() + +print("my args:", json.dumps(sys.argv[1:])) die_after = os.environ.get("NATIVE_ECHO_DIE_AFTER") if die_after: From f83642e7698d403e4cbaa3116f90cc653846098e Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Mon, 16 Feb 2026 20:25:58 +0800 Subject: [PATCH 47/48] Switch native_echo test helper from env vars to CLI args Use argparse in native_echo.py instead of env vars for output_file and die_after. Add these as config fields on StubNativeConfig so they flow through the normal to_cli_args() path. --- dimos/core/test_native_module.py | 10 +++++++--- dimos/core/tests/native_echo.py | 26 +++++++++++++------------- 2 files changed, 20 insertions(+), 16 deletions(-) diff --git a/dimos/core/test_native_module.py b/dimos/core/test_native_module.py index 8bf50ea7a3..1deaf819f4 100644 --- a/dimos/core/test_native_module.py +++ b/dimos/core/test_native_module.py @@ -62,6 +62,8 @@ def read_json_file(path: str) -> dict[str, str]: 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 @@ -91,7 +93,7 @@ def start(self) -> None: def test_process_crash_triggers_stop() -> None: """When the native process dies unexpectedly, the watchdog calls stop().""" - mod = StubNativeModule(extra_env={"NATIVE_ECHO_DIE_AFTER": "0.2"}) + mod = StubNativeModule(die_after=0.2) mod.pointcloud.transport = LCMTransport("/pc", PointCloud2) mod.start() @@ -111,7 +113,7 @@ def test_manual(dimos_cluster, args_file) -> None: native_module = dimos_cluster.deploy( StubNativeModule, some_param=2.5, - extra_env={"NATIVE_ECHO_OUTPUT": args_file}, + output_file=args_file, ) native_module.pointcloud.transport = LCMTransport("/my/custom/lidar", PointCloud2) @@ -123,6 +125,7 @@ def test_manual(dimos_cluster, args_file) -> None: 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", } @@ -133,7 +136,7 @@ def test_autoconnect(args_file) -> None: blueprint = autoconnect( StubNativeModule.blueprint( some_param=2.5, - extra_env={"NATIVE_ECHO_OUTPUT": args_file}, + output_file=args_file, ), StubConsumer.blueprint(), StubProducer.blueprint(), @@ -167,5 +170,6 @@ def test_autoconnect(args_file) -> None: "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 index b011ff3b8c..6723b0b0d1 100755 --- a/dimos/core/tests/native_echo.py +++ b/dimos/core/tests/native_echo.py @@ -15,34 +15,34 @@ """Echo binary for NativeModule tests. -Dumps CLI args as a JSON log line to stdout, then waits for SIGTERM. - -Env vars: - NATIVE_ECHO_OUTPUT: path to write CLI args as JSON - NATIVE_ECHO_DIE_AFTER: seconds to wait before exiting with code 42 +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 os import signal import sys import time -print("this mesasage goes to stdout") +print("this message goes to stdout") print("this message goes to stderr", file=sys.stderr) signal.signal(signal.SIGTERM, lambda *_: sys.exit(0)) -output_path = os.environ.get("NATIVE_ECHO_OUTPUT") -if output_path: - with open(output_path, "w") as f: +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:])) -die_after = os.environ.get("NATIVE_ECHO_DIE_AFTER") -if die_after: - time.sleep(float(die_after)) +if args.die_after is not None: + time.sleep(args.die_after) sys.exit(42) signal.pause() From 4e98ec22d76813d764715cd7fb8828771108d408 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Mon, 16 Feb 2026 20:52:15 +0800 Subject: [PATCH 48/48] Switch native_echo from env vars to CLI args, fix mypy annotations Replace NATIVE_ECHO_OUTPUT/NATIVE_ECHO_DIE_AFTER env vars with output_file/die_after config fields passed as CLI args. Add type annotations to test functions. Remove stale type: ignore on NativeModule.default_config. --- dimos/core/native_module.py | 2 +- dimos/core/test_native_module.py | 9 +++++---- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/dimos/core/native_module.py b/dimos/core/native_module.py index c871392d57..6a93e6453a 100644 --- a/dimos/core/native_module.py +++ b/dimos/core/native_module.py @@ -118,7 +118,7 @@ class NativeModule(Module[NativeModuleConfig]): LCM topics directly. On ``stop()``, the process receives SIGTERM. """ - default_config: type[NativeModuleConfig] = NativeModuleConfig # type: ignore[assignment] + default_config: type[NativeModuleConfig] = NativeModuleConfig _process: subprocess.Popen[bytes] | None = None _watchdog: threading.Thread | None = None _stopping: bool = False diff --git a/dimos/core/test_native_module.py b/dimos/core/test_native_module.py index 1deaf819f4..8af63b0bf4 100644 --- a/dimos/core/test_native_module.py +++ b/dimos/core/test_native_module.py @@ -25,6 +25,7 @@ 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 @@ -109,8 +110,8 @@ def test_process_crash_triggers_stop() -> None: assert mod._process is None, f"Watchdog did not clean up after process {pid} died" -def test_manual(dimos_cluster, args_file) -> None: - native_module = dimos_cluster.deploy( +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, @@ -131,7 +132,7 @@ def test_manual(dimos_cluster, args_file) -> None: @pytest.mark.heavy -def test_autoconnect(args_file) -> None: +def test_autoconnect(args_file: str) -> None: """autoconnect passes correct topic args to the native subprocess.""" blueprint = autoconnect( StubNativeModule.blueprint( @@ -149,7 +150,7 @@ def test_autoconnect(args_file) -> None: coordinator = blueprint.global_config(viewer_backend="none").build() try: # Validate blueprint wiring: all modules deployed - native = coordinator.get_instance(StubNativeModule) + native = coordinator.get_instance(StubNativeModule) # type: ignore[type-var] consumer = coordinator.get_instance(StubConsumer) producer = coordinator.get_instance(StubProducer) assert native is not None