From a547e7b7b6cb9f538fa5fb3af6afa6b060176c48 Mon Sep 17 00:00:00 2001 From: kahzmic Date: Mon, 16 Feb 2026 11:55:23 -0800 Subject: [PATCH 1/2] first commit. still need to clean everything up for PR --- dimos/core/testing.py | 2 +- .../fastlio2/test_fastlio_column_carving.py | 160 +++++++++++++++ .../blueprints/smart/unitree_go2_fastlio.py | 188 ++++++++++++++++++ docs/usage/sensors/mid360.md | 19 ++ examples/livox_nav/module.py | 134 +++++++++++++ 5 files changed, 502 insertions(+), 1 deletion(-) create mode 100644 dimos/hardware/sensors/lidar/fastlio2/test_fastlio_column_carving.py create mode 100644 dimos/robot/unitree/go2/blueprints/smart/unitree_go2_fastlio.py create mode 100644 docs/usage/sensors/mid360.md create mode 100644 examples/livox_nav/module.py diff --git a/dimos/core/testing.py b/dimos/core/testing.py index dee25aaa4..eb7ef6ed1 100644 --- a/dimos/core/testing.py +++ b/dimos/core/testing.py @@ -1,4 +1,4 @@ -# Copyright 2025-2026 Dimensional Inc. +_# 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. diff --git a/dimos/hardware/sensors/lidar/fastlio2/test_fastlio_column_carving.py b/dimos/hardware/sensors/lidar/fastlio2/test_fastlio_column_carving.py new file mode 100644 index 000000000..7785b06e3 --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/test_fastlio_column_carving.py @@ -0,0 +1,160 @@ +# 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 pickle +import time +from pathlib import Path +import math + +import pytest + +from dimos import core +from dimos.core import Module, Out, rpc +from dimos.msgs.sensor_msgs import PointCloud2 +from dimos.msgs.geometry_msgs import Transform, Vector3, Quaternion +from dimos.utils.logging_config import setup_logger + # Build the blueprint (same as replay_object_permanence_mid360 from examples/livox_nav/module.py) +from dimos.core.blueprints import autoconnect +from dimos.visualization.rerun.bridge import rerun_bridge +from dimos.mapping.voxels import voxel_mapper +import math + +logger = setup_logger() +voxel_size = 0.05 + +#TODO: add foot note about getting a failed test due to dask multithread?? +class ReplayMid360Module(Module): + """Module that replays Mid360 lidar data from pickle file.""" + + lidar: Out[PointCloud2] + + def __init__(self, lidar_path: str) -> None: + super().__init__() + self.lidar_path = lidar_path + self.file = None + self._running = False + self._thread = None + + @rpc + def start(self) -> None: + """Start replaying lidar data.""" + import threading + + self.file = open(self.lidar_path, "rb") + self._running = True + self._thread = threading.Thread(target=self._replay_loop, daemon=True) + self._thread.start() + logger.info(f"ReplayMid360Module started, replaying from {self.lidar_path}") + + def _replay_loop(self): + floor_orientation = Transform( + translation=Vector3(0, 0, 0), + rotation=Quaternion.from_euler(Vector3(0, math.radians(24), 0)), + ) + try: + logger.info(f"Starting replay from {self.lidar_path}") + frame_count = 0 + while self._running: + pcd: PointCloud2 = pickle.load(self.file) + logger.info(f"Replaying pointcloud at ts={pcd.ts}") + self.lidar.publish(pcd.transform(floor_orientation)) + frame_count += 1 + time.sleep(0.1) # Add small delay between frames + except EOFError: + logger.info(f"Replay finished - reached end of file after {frame_count} frames") + self._running = False + except Exception as e: + logger.error(f"Error during replay: {e}") + self._running = False + + @rpc + def stop(self) -> None: + """Stop replaying lidar data.""" + self._running = False + if self._thread and self._thread.is_alive(): + self._thread.join(timeout=2.0) + if self.file: + self.file.close() + logger.info("ReplayMid360Module stopped") + + +@pytest.mark.integration +def test_replay_column_carving(): + """Test FastLIO2 voxel mapper with column carving using replay data.""" + + # Get test data path + data_path = Path(__file__).parent.parent.parent.parent.parent.parent / "data" / "livox_nav_recording" + lidar_path = data_path / "lidar.pkl" + + if not lidar_path.exists(): + pytest.skip(f"Test data not found at {lidar_path}") + + logger.info(f"Using test data from {lidar_path}") + + # Create the blueprint with our custom replay module + test_blueprint = autoconnect( + ReplayMid360Module.blueprint(lidar_path=str(lidar_path)), + voxel_mapper(voxel_size=voxel_size), + rerun_bridge() + ) + + # Build and start the application + app = test_blueprint.build() + app.start() + logger.info("Started replay with voxel mapper and rerun bridge") + + try: + # Verify modules were deployed + from dimos.mapping.voxels import VoxelGridMapper + + voxel_mapper_module = app.get_instance(VoxelGridMapper) + replay_module_instance = app.get_instance(ReplayMid360Module) + + assert voxel_mapper_module is not None, "VoxelGridMapper module not deployed" + assert replay_module_instance is not None, "ReplayMid360Module not deployed" + logger.info("✓ All required modules deployed successfully") + + # Wait for data processing to begin + logger.info("Waiting for data processing to begin...") + time.sleep(3) + logger.info("✓ System started processing data") + + # Let it run to test column carving (voxel accumulation over time) + processing_duration = 30.0 + logger.info(f"Running column carving test for {processing_duration} seconds...") + + elapsed = 0 + check_interval = 2.0 + while elapsed < processing_duration: + time.sleep(check_interval) + elapsed += check_interval + logger.info(f"Processing... {elapsed:.1f}s / {processing_duration}s") + + logger.info("✓ Column carving test completed successfully") + + finally: + # Stop the application and wait for threads to terminate + app.stop() + logger.info("Stopped application, waiting for cleanup...") + + # Give threads time to fully terminate + time.sleep(5) + + logger.info("✓ All FastLIO2 column carving tests passed!") + + +__all__ = ["ReplayMid360Module"] + +if __name__ == "__main__": + pytest.main(["-v", "-s", __file__]) diff --git a/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_fastlio.py b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_fastlio.py new file mode 100644 index 000000000..479397b04 --- /dev/null +++ b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_fastlio.py @@ -0,0 +1,188 @@ +#!/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 reactivex.disposable import Disposable + +from dimos.core.blueprints import autoconnect +from dimos.mapping.costmapper import cost_mapper +from dimos.mapping.voxels import VoxelGridMapper +from dimos.navigation.frontier_exploration import wavefront_frontier_explorer +from dimos.navigation.replanning_a_star.module import replanning_a_star_planner +from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic import unitree_go2_basic +from dimos.utils.logging_config import setup_logger +from dimos.core import Module, In, Out, rpc +from dimos.msgs.sensor_msgs import PointCloud2 +from dimos.msgs.geometry_msgs import Vector3, Quaternion, Transform, PoseStamped +from dimos.msgs.nav_msgs import Odometry +from dimos.visualization.rerun.bridge import rerun_bridge +from dimos.mapping.pointclouds.occupancy import HeightCostConfig +from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 +from dimos.robot.unitree.go2.connection import GO2Connection + +import pickle +import math +import time +from pathlib import Path + + +logger = setup_logger() + +voxel_size = 0.05 + +# data_path = Path(__file__).parent.parent.parent.parent.parent.parent / "data" / "livox_nav_recording" +# lidar_path = data_path / "lidar.pkl" + + +class ReplayMid360Module(Module): + """Module that replays Mid360 lidar data from pickle file.""" + + lidar: Out[PointCloud2] + + def __init__(self) -> None: + super().__init__() + # TODO: clean up (use get_data function) + data_path = Path(__file__).parent.parent.parent.parent.parent.parent.parent / "data" / "livox_nav_recording" + lidar_path = data_path / "lidar.pkl" + + self.lidar_path = lidar_path + self.file = None + self._running = False + self._thread = None + + @rpc + def start(self) -> None: + """Start replaying lidar data.""" + import threading + + self.file = open(self.lidar_path, "rb") + self._running = True + self._thread = threading.Thread(target=self._replay_loop, daemon=True) + self._thread.start() + logger.info(f"ReplayMid360Module started, replaying from {self.lidar_path}") + + def _replay_loop(self): + floor_orientation = Transform( + translation=Vector3(0, 0, 0.5), + rotation=Quaternion.from_euler(Vector3(0, math.radians(24), 0)), + ) + try: + logger.info(f"Starting replay from {self.lidar_path}") + frame_count = 0 + while self._running: + pcd: PointCloud2 = pickle.load(self.file) + logger.info(f"Replaying pointcloud at ts={pcd.ts}") + self.lidar.publish(pcd.transform(floor_orientation).filter_by_height(max_height=2)) + frame_count += 1 + time.sleep(0.1) # Add small delay between frames + except EOFError: + logger.info(f"Replay finished - reached end of file after {frame_count} frames") + self._running = False + except Exception as e: + logger.error(f"Error during replay: {e}") + self._running = False + + @rpc + def stop(self) -> None: + """Stop replaying lidar data.""" + self._running = False + if self._thread and self._thread.is_alive(): + self._thread.join(timeout=2.0) + if self.file: + self.file.close() + logger.info("ReplayMid360Module stopped") + +ANGLE_OF_MID_360_ON_ROBOT = 24 # degre +class TransformToRobot(Module): + lidar_untrans: In[PointCloud2] + lidar_trans: Out[PointCloud2] + + def __init__(self) -> None: + super().__init__() + self.floor_orientation = Transform( + translation=Vector3(0, 0, 0.5), + rotation=Quaternion.from_euler(Vector3(0, math.radians(ANGLE_OF_MID_360_ON_ROBOT), 0)), + ) + + @rpc + def start(self): + unsub = self.lidar_untrans.subscribe(self._on_lidar) + self._disposables.add(Disposable(unsub)) + + def _on_lidar(self, pcd: PointCloud2): + #TODO: add straight to costmapper + """Apply transform and filter by height before publishing.""" + transformed = pcd.transform(self.floor_orientation).filter_by_height(max_height=2) + self.lidar_trans.publish(transformed) + + @rpc + def stop(self): + pass + +class OdometryToOdom(Module): + """Module that converts nav_msgs.Odometry to geometry_msgs.PoseStamped.""" + + odometry: In[Odometry] + odom: Out[PoseStamped] + + def __init__(self) -> None: + super().__init__() + + @rpc + def start(self): + unsub = self.odometry.subscribe(self._on_odom) + self._disposables.add(Disposable(unsub)) + + def _on_odom(self, odometry: Odometry): + """Convert Odometry to PoseStamped.""" + self.odom.publish(PoseStamped( + ts=odometry.ts, + position=odometry.pose.position, + orientation=odometry.pose.orientation, + )) + + @rpc + def stop(self): + pass + +unitree_go2_fastlio = autoconnect( + unitree_go2_basic, + FastLio2.blueprint(voxel_size=voxel_size, map_voxel_size=voxel_size, map_freq=-1), + TransformToRobot.blueprint(), + OdometryToOdom.blueprint(), + VoxelGridMapper.blueprint(voxel_size=voxel_size), + cost_mapper(), + replanning_a_star_planner(), + wavefront_frontier_explorer(), + rerun_bridge() +).remappings([ + # TODO: maybe call lidar in fastlio raw lidar + # turn off go2 lidar (basically) + (GO2Connection, 'lidar', 'lidar_onboard'), + (GO2Connection, 'odom', 'odom_null'), + # + # (FastLio2, 'odometry', 'odometry_nav'), + # (FastLio2, 'lidar', 'lidar_fastlio'), + (TransformToRobot, 'lidar_untrans', 'lidar'), + # Convert nav_msgs.Odometry to geometry_msgs.PoseStamped + # (OdometryToOdom, 'odometry_nav', 'odom'), + # (cost_mapper, 'odom', 'odometry'), + (VoxelGridMapper, 'lidar', 'lidar_trans'), +]).global_config(n_dask_workers=6, robot_model="unitree_go2") + + + +__all__ = ["unitree_go2_fastlio"] + +if __name__ == "__main__": + unitree_go2_fastlio.build().loop() \ No newline at end of file diff --git a/docs/usage/sensors/mid360.md b/docs/usage/sensors/mid360.md new file mode 100644 index 000000000..b25b0aa2c --- /dev/null +++ b/docs/usage/sensors/mid360.md @@ -0,0 +1,19 @@ + + +# Livox Mid-360 blueprints +- mid360 — raw LiDAR +- mid360-fastlio — SLAM with 0.15m voxel filtering +- mid360-fastlio-voxels — SLAM + downstream VoxelGridMapper +- mid360-fastlio-voxels-native — SLAM + C++ global voxel map at 3Hz + +# Using USB to Ethernet Adapter +If you are using an adapter make sure to use the following commands so that the host +can succesfully communicate with the Mid-360: +- run `ip -br link` to see if the adpater is recognized. You should see an ip similar to enx00e04c680125 +- then assign a static IP to the adapter using `sudo ip addr add 192.168.1.5/24 dev ` + +Make sure the adpater is fully connected with the ethernet cord. Now you should be able to use any +Mid-360 blueprint. \ No newline at end of file diff --git a/examples/livox_nav/module.py b/examples/livox_nav/module.py new file mode 100644 index 000000000..b3fe30db7 --- /dev/null +++ b/examples/livox_nav/module.py @@ -0,0 +1,134 @@ +from dimos.core import Module, rpc, In, Out +from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 +from dimos.robot.unitree.go2.connection import GO2Connection +from dimos.core.blueprints import autoconnect +from dimos.visualization.rerun.bridge import rerun_bridge +from dimos.msgs.sensor_msgs import PointCloud2 +from dimos.msgs.nav_msgs import Odometry +from dimos.msgs.geometry_msgs import Transform, Vector3, Quaternion + + +from dimos.core.introspection import to_svg +from dimos.mapping.costmapper import cost_mapper +from dimos.mapping.voxels import voxel_mapper +from dimos.navigation.frontier_exploration import wavefront_frontier_explorer +from dimos.navigation.replanning_a_star.module import replanning_a_star_planner +from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic import unitree_go2_basic + +from pathlib import Path +import time +import pickle +import math + +voxel_size = 0.05 + +class RecordMid360Module(Module): + lidar: In[PointCloud2] + odometry: In[Odometry] + + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + self.file = None + + @rpc + def start(self): + self.file = open("lidar.pkl", "wb") + self.file2 = open('odo.pkl', "wb") + print("Started recording lidar data to lidar.pkl") + + def save_lidar(msg): + pickle.dump(msg, self.file) + print(f"Saved pointcloud at ts={msg.ts}") + + def save_odom(msg): + pickle.dump(msg, self.file2) + print(f"Saved odometry at ts={msg.ts}") + + self.lidar.subscribe(save_lidar) + self.odometry.subscribe(save_odom) + + @rpc + def stop(self): + if self.file: + self.file.close() + print(f"Recording stopped.") + super().stop() + +class ReplayMid360Module(Module): + lidar: Out[PointCloud2] + + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + self.file = None + + @rpc + def start(self): + import threading + self.file = open("lidar.pkl", "rb") + self._running = True + self._thread = threading.Thread(target=self._replay_loop, daemon=True) + self._thread.start() + + def _replay_loop(self): + floor_orienation = Transform( + translation=Vector3(0, 0, 0), + rotation=Quaternion.from_euler(Vector3(0,math.radians(24),0)), + ) + try: + print('Starting replay from lidar.pkl') + while self._running: + pcd: PointCloud2 = pickle.load(self.file) + print(f"Replaying pointcloud at ts={pcd.ts}") + self.lidar.publish(pcd.transform(floor_orienation)) + time.sleep(0.1) # Add small delay between frames + except EOFError: + print("Replay finished - reached end of file") + self._running = False + + @rpc + def stop(self): + self._running = False + if self._thread and self._thread.is_alive(): + self._thread.join(timeout=2.0) + if self.file: + self.file.close() + +unitree_go2 = autoconnect( + unitree_go2_basic, # robot connection + visualization + voxel_mapper(voxel_size=0.05), # 3D voxel mapping + cost_mapper(), # 2D costmap generation + replanning_a_star_planner(), # path planning + wavefront_frontier_explorer(), # exploration +).global_config(n_dask_workers=6, robot_model="unitree_go2") + + + +record_mid360 = autoconnect( + FastLio2.blueprint(voxel_size=voxel_size, map_voxel_size=voxel_size, map_freq=-1), + rerun_bridge( + visual_override={ + "world/lidar": lambda grid: grid.to_rerun(voxel_size=voxel_size, mode="boxes"), + } + ), + RecordMid360Module.blueprint() +) + +replay_mid360 = autoconnect( + ReplayMid360Module.blueprint(), + rerun_bridge( + visual_override={ + "world/lidar": lambda grid: grid.to_rerun(voxel_size=voxel_size, mode="boxes"), + } + ), +) + +replay_object_permanence_mid360 = autoconnect( + ReplayMid360Module.blueprint(), + voxel_mapper(voxel_size=voxel_size), + rerun_bridge() +) + +if __name__ == "__main__": + # record_mid360.build().loop() + # replay_mid360.build().loop() + replay_object_permanence_mid360.build().loop() \ No newline at end of file From c473ef0cdc0f5dd6d2d9bc95a93a544793e79b31 Mon Sep 17 00:00:00 2001 From: kahzmic Date: Mon, 23 Feb 2026 20:59:57 -0600 Subject: [PATCH 2/2] Finished some todos --- dimos/core/testing.py | 2 +- .../blueprints/smart/unitree_go2_fastlio.py | 65 ++++++++++++------- docs/usage/sensors/mid360.md | 44 +++++++++---- examples/livox_nav/module.py | 24 ++++--- 4 files changed, 87 insertions(+), 48 deletions(-) diff --git a/dimos/core/testing.py b/dimos/core/testing.py index eb7ef6ed1..dee25aaa4 100644 --- a/dimos/core/testing.py +++ b/dimos/core/testing.py @@ -1,4 +1,4 @@ -_# Copyright 2025-2026 Dimensional Inc. +# 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. diff --git a/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_fastlio.py b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_fastlio.py index 479397b04..b04dbcc01 100644 --- a/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_fastlio.py +++ b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_fastlio.py @@ -29,6 +29,9 @@ from dimos.mapping.pointclouds.occupancy import HeightCostConfig from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 from dimos.robot.unitree.go2.connection import GO2Connection +from dimos.utils.data import get_data +from dimos.core.module import ModuleConfig +from dataclasses import dataclass import pickle import math @@ -38,10 +41,9 @@ logger = setup_logger() -voxel_size = 0.05 - -# data_path = Path(__file__).parent.parent.parent.parent.parent.parent / "data" / "livox_nav_recording" -# lidar_path = data_path / "lidar.pkl" +VOXEL_SIZE = 0.05 +ANGLE_OF_MID_360_ON_ROBOT = 24 # degree +HEIGHT_OF_MID_360_ON_ROBOT = 0.5 # meter class ReplayMid360Module(Module): @@ -51,11 +53,8 @@ class ReplayMid360Module(Module): def __init__(self) -> None: super().__init__() - # TODO: clean up (use get_data function) - data_path = Path(__file__).parent.parent.parent.parent.parent.parent.parent / "data" / "livox_nav_recording" - lidar_path = data_path / "lidar.pkl" - - self.lidar_path = lidar_path + # TODO: clean up (use get_data function) done + self.lidar_path = get_data("livox_nav_recording/lidar.pkl") self.file = None self._running = False self._thread = None @@ -102,16 +101,25 @@ def stop(self) -> None: self.file.close() logger.info("ReplayMid360Module stopped") -ANGLE_OF_MID_360_ON_ROBOT = 24 # degre + +@dataclass +class Config(ModuleConfig): + angle_of_mid_360_on_robot: float = 0 + height_of_mid_360_on_robot: float = 0.5 + lidar_max_height: float = 2.0 + + class TransformToRobot(Module): + default_config = Config + config: Config lidar_untrans: In[PointCloud2] lidar_trans: Out[PointCloud2] def __init__(self) -> None: super().__init__() self.floor_orientation = Transform( - translation=Vector3(0, 0, 0.5), - rotation=Quaternion.from_euler(Vector3(0, math.radians(ANGLE_OF_MID_360_ON_ROBOT), 0)), + translation=Vector3(0, 0, self.config.height_of_mid_360_on_robot), + rotation=Quaternion.from_euler(Vector3(0, math.radians(self.config.angle_of_mid_360_on_robot), 0)), ) @rpc @@ -122,7 +130,7 @@ def start(self): def _on_lidar(self, pcd: PointCloud2): #TODO: add straight to costmapper """Apply transform and filter by height before publishing.""" - transformed = pcd.transform(self.floor_orientation).filter_by_height(max_height=2) + transformed = pcd.transform(self.floor_orientation).filter_by_height(max_height=self.config.lidar_max_height) self.lidar_trans.publish(transformed) @rpc @@ -157,10 +165,14 @@ def stop(self): unitree_go2_fastlio = autoconnect( unitree_go2_basic, - FastLio2.blueprint(voxel_size=voxel_size, map_voxel_size=voxel_size, map_freq=-1), - TransformToRobot.blueprint(), + FastLio2.blueprint(voxel_size=VOXEL_SIZE, map_voxel_size=VOXEL_SIZE, map_freq=-1), + TransformToRobot.blueprint( + angle_of_mid_360_on_robot=ANGLE_OF_MID_360_ON_ROBOT, + height_of_mid_360_on_robot=HEIGHT_OF_MID_360_ON_ROBOT, + lidar_max_height=2.0 + ), OdometryToOdom.blueprint(), - VoxelGridMapper.blueprint(voxel_size=voxel_size), + VoxelGridMapper.blueprint(voxel_size=VOXEL_SIZE), cost_mapper(), replanning_a_star_planner(), wavefront_frontier_explorer(), @@ -168,21 +180,26 @@ def stop(self): ).remappings([ # TODO: maybe call lidar in fastlio raw lidar # turn off go2 lidar (basically) - (GO2Connection, 'lidar', 'lidar_onboard'), + (GO2Connection, 'lidar', 'lidar_null'), (GO2Connection, 'odom', 'odom_null'), - # - # (FastLio2, 'odometry', 'odometry_nav'), - # (FastLio2, 'lidar', 'lidar_fastlio'), (TransformToRobot, 'lidar_untrans', 'lidar'), - # Convert nav_msgs.Odometry to geometry_msgs.PoseStamped - # (OdometryToOdom, 'odometry_nav', 'odom'), - # (cost_mapper, 'odom', 'odometry'), (VoxelGridMapper, 'lidar', 'lidar_trans'), ]).global_config(n_dask_workers=6, robot_model="unitree_go2") +fastlio_livox = autoconnect( + ReplayMid360Module.blueprint(), + TransformToRobot.blueprint(), + VoxelGridMapper.blueprint(voxel_size=VOXEL_SIZE), + cost_mapper(), + replanning_a_star_planner(), + wavefront_frontier_explorer(), + rerun_bridge() +) + __all__ = ["unitree_go2_fastlio"] if __name__ == "__main__": - unitree_go2_fastlio.build().loop() \ No newline at end of file + # unitree_go2_fastlio.build().loop() + fastlio_livox.build().loop() \ No newline at end of file diff --git a/docs/usage/sensors/mid360.md b/docs/usage/sensors/mid360.md index b25b0aa2c..2087641f8 100644 --- a/docs/usage/sensors/mid360.md +++ b/docs/usage/sensors/mid360.md @@ -2,18 +2,36 @@ - How to get your mid36 connected and confirm that it is running (ping address) - How to get just the data and view in rerun (simple blueprint) --> +# How to Use and Setup Mid-360 +The Mid-360 uses an M12 Aviation Connector for both power and data. If you do not have the Livox 1-to-3 spliter cable please refer to their official documentation to find the pinout information. To run the Mid-360 blueprints the Mid-360 needs to be hooked up to power (supports 9V to 27V) and to an ethernet port on your host machine. -# Livox Mid-360 blueprints -- mid360 — raw LiDAR -- mid360-fastlio — SLAM with 0.15m voxel filtering -- mid360-fastlio-voxels — SLAM + downstream VoxelGridMapper -- mid360-fastlio-voxels-native — SLAM + C++ global voxel map at 3Hz - -# Using USB to Ethernet Adapter -If you are using an adapter make sure to use the following commands so that the host -can succesfully communicate with the Mid-360: -- run `ip -br link` to see if the adpater is recognized. You should see an ip similar to enx00e04c680125 -- then assign a static IP to the adapter using `sudo ip addr add 192.168.1.5/24 dev ` +## Setting up the Ethernet Connection +Whether you are connecting the Mid-360 directly to your computer's ethernet port or using a USB to Ethernet adapter, you need to assign a static IP address to that interface so it can communicate with the LiDAR. -Make sure the adpater is fully connected with the ethernet cord. Now you should be able to use any -Mid-360 blueprint. \ No newline at end of file +- Run `ip -br link` to find the name of your network interface. + - For a direct ethernet port, it will typically look like `eth0`, `eno1`, or `enp4s0`. + - For a USB to Ethernet adapter, it will typically look like a long MAC-address-based name such as `enx00e04c680125`. +- Assign a static IP to the interface using: + `sudo ip addr add 192.168.1.5/24 dev ` + +- Make sure the ethernet cable is fully connected and verify the connection by pinging the default Mid-360 IP address: + `ping 192.168.1.155` + +Now you should be able to use any Mid-360 blueprint. + +## Building Mid-360 Native Modules +The Mid-360 blueprints require using a native module. Use nix to build the native module: +``` +cd dimos/hardware/sensors/lidar/livox/cpp +nix build .#mid360_native +``` +``` +cd dimos/hardware/sensors/lidar/fastlio2/cpp +nix build .#fastlio2_native +``` + +## Livox Mid-360 dimos run +- `dimos run mid360` — raw LiDAR +- `dimos run mid360-fastlio` — SLAM with 0.15m voxel filtering +- `dimos run mid360-fastlio-voxels` — SLAM + downstream VoxelGridMapper +- `dimos run mid360-fastlio-voxels-native` — SLAM + C++ global voxel map at 3Hz \ No newline at end of file diff --git a/examples/livox_nav/module.py b/examples/livox_nav/module.py index b3fe30db7..efae53b66 100644 --- a/examples/livox_nav/module.py +++ b/examples/livox_nav/module.py @@ -1,3 +1,4 @@ +from logging import log from dimos.core import Module, rpc, In, Out from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 from dimos.robot.unitree.go2.connection import GO2Connection @@ -14,12 +15,15 @@ from dimos.navigation.frontier_exploration import wavefront_frontier_explorer from dimos.navigation.replanning_a_star.module import replanning_a_star_planner from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic import unitree_go2_basic +from dimos.utils.logging_config import setup_logger from pathlib import Path import time import pickle import math +logger = setup_logger() + voxel_size = 0.05 class RecordMid360Module(Module): @@ -34,15 +38,15 @@ def __init__(self, *args, **kwargs): def start(self): self.file = open("lidar.pkl", "wb") self.file2 = open('odo.pkl', "wb") - print("Started recording lidar data to lidar.pkl") + logger.info("Started recording lidar data to lidar.pkl") def save_lidar(msg): pickle.dump(msg, self.file) - print(f"Saved pointcloud at ts={msg.ts}") - + logger.info(f"Saved pointcloud at ts={msg.ts}") + def save_odom(msg): pickle.dump(msg, self.file2) - print(f"Saved odometry at ts={msg.ts}") + logger.info(f"Saved odometry at ts={msg.ts}") self.lidar.subscribe(save_lidar) self.odometry.subscribe(save_odom) @@ -51,7 +55,7 @@ def save_odom(msg): def stop(self): if self.file: self.file.close() - print(f"Recording stopped.") + logger.info(f"Recording stopped.") super().stop() class ReplayMid360Module(Module): @@ -75,14 +79,14 @@ def _replay_loop(self): rotation=Quaternion.from_euler(Vector3(0,math.radians(24),0)), ) try: - print('Starting replay from lidar.pkl') + logger.info('Starting replay from lidar.pkl') while self._running: pcd: PointCloud2 = pickle.load(self.file) - print(f"Replaying pointcloud at ts={pcd.ts}") + logger.info(f"Replaying pointcloud at ts={pcd.ts}") self.lidar.publish(pcd.transform(floor_orienation)) time.sleep(0.1) # Add small delay between frames except EOFError: - print("Replay finished - reached end of file") + logger.info("Replay finished - reached end of file") self._running = False @rpc @@ -122,7 +126,7 @@ def stop(self): ), ) -replay_object_permanence_mid360 = autoconnect( +replay_mid360_voxel_mapper = autoconnect( ReplayMid360Module.blueprint(), voxel_mapper(voxel_size=voxel_size), rerun_bridge() @@ -131,4 +135,4 @@ def stop(self): if __name__ == "__main__": # record_mid360.build().loop() # replay_mid360.build().loop() - replay_object_permanence_mid360.build().loop() \ No newline at end of file + replay_mid360_voxel_mapper.build().loop() \ No newline at end of file