feat: multi level navigation

dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp

@@ -219,35 +219,6 @@ static void on_imu_data(const uint32_t /*handle*/, const uint8_t /*dev_type*/, L
     if (!g_running.load() || data == nullptr || !g_point_lio) { return; }
 
     uint64_t pkt_ts_ns = get_timestamp_ns(data);
-    // Live IMU-drop instrumentation: a dropped datagram shows as a sensor-ts
-    // jump; wall gaps exceeding sensor gaps mean callback starvation.
-    {
-        static std::atomic<uint64_t> last_pkt_ts_ns{0};
-        static std::atomic<uint64_t> imu_pkt_count{0};
-        static std::atomic<uint64_t> imu_gap_count{0};
-        static std::atomic<uint64_t> max_sensor_gap_us{0};
-        using clk = std::chrono::steady_clock;
-        static auto last_wall = clk::now();
-        auto now_wall = clk::now();
-        uint64_t prev = last_pkt_ts_ns.exchange(pkt_ts_ns);
-        uint64_t pkt_count = imu_pkt_count.fetch_add(1) + 1;
-        if (prev != 0 && pkt_ts_ns > prev) {
-            uint64_t sensor_gap_us = (pkt_ts_ns - prev) / 1000;
-            uint64_t wall_gap_us = std::chrono::duration_cast<std::chrono::microseconds>( now_wall - last_wall).count();
-            uint64_t cur_max = max_sensor_gap_us.load();
-            while (sensor_gap_us > cur_max &&
-                   !max_sensor_gap_us.compare_exchange_weak(cur_max, sensor_gap_us)) {}
-            if (sensor_gap_us > 15000) {
-                imu_gap_count.fetch_add(1);
-                fprintf(stderr, "[imu-gap] sensor_gap=%.1fms wall_gap=%.1fms pkt#%llu\n", sensor_gap_us / 1000.0, wall_gap_us / 1000.0, static_cast<unsigned long long>(pkt_count));
-            }
-        }
-        last_wall = now_wall;
-        if (pkt_count % 1000 == 0) {
-            fprintf(stderr, "[imu-stats] pkts=%llu gaps>15ms=%llu max_sensor_gap=%.1fms\n", static_cast<unsigned long long>(pkt_count), static_cast<unsigned long long>(imu_gap_count.load()), max_sensor_gap_us.load() / 1000.0);
-        }
-    }
-
     double ts = static_cast<double>(pkt_ts_ns) / 1e9;
     auto* imu_pts = reinterpret_cast<const LivoxLidarImuRawPoint*>(data->data);
     uint16_t dot_num = data->dot_num;

dimos/mapping/ray_tracing/module.py

@@ -19,6 +19,7 @@
 
 from dimos.core.native_module import NativeModule, NativeModuleConfig
 from dimos.core.stream import In, Out
+from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped
 from dimos.msgs.nav_msgs.Odometry import Odometry
 from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2
 from dimos.spec import mapping
@@ -43,10 +44,18 @@ class RayTracingVoxelMapConfig(NativeModuleConfig):
     # Bounds for the health of voxels. Positive health means voxel is occupied.
     min_health: int = -2
     max_health: int = 1
-    # Spare a clearing miss when |ray dot surface normal| is below this.
+    # Don't clear a miss when abs of ray dot normal is below this, clear it when above.
+    # Higher clears only on direct hits, lower clears on slight grazes too.
     graze_cos: float = 0.7
     # Only spare a voxel whose neighborhood was hit within this many frames.
+    # A stale voxel can be cleared, even if it's a grazing hit. Large disables it.
     recency_window: int = 15
+    # Publish the accumulated local map and region bounds every Nth frame. Zero disables them.
+    emit_every: int = 1
+    # Publish the global map every Nth frame. Zero disables it.
+    global_emit_every: int = 1
+    # Size the local region to this percentile of batch point distances.
+    region_percentile: float = 95.0
 
 
 class RayTracingVoxelMap(NativeModule, mapping.GlobalPointcloud):
@@ -58,8 +67,8 @@ class RayTracingVoxelMap(NativeModule, mapping.GlobalPointcloud):
     odometry: In[Odometry]
     global_map: Out[PointCloud2]
     local_map: Out[PointCloud2]
+    region_bounds: Out[PoseStamped]
 
 
-# Verify protocol port compliance (mypy will flag missing ports)
 if TYPE_CHECKING:
     RayTracingVoxelMap()

dimos/mapping/ray_tracing/rust/Cargo.toml

@@ -21,6 +21,8 @@ lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "r
 tokio = { version = "1", features = ["rt-multi-thread", "macros", "signal"] }
 serde = { version = "1", features = ["derive"] }
 ahash = "0.8"
+arrayvec = "0.7"
+rayon = "1"
 tracing = "0.1"
 pyo3 = { version = "0.25", features = ["extension-module", "abi3-py310"] }
 numpy = "0.25"

dimos/mapping/ray_tracing/rust/flake.nix

@@ -6,8 +6,8 @@
     flake-utils.url = "github:numtide/flake-utils";
     # Relative git+file: will be deprecated (nix#12281) but there's no
     # viable alternative for reaching local path deps outside the flake dir currently
-    # presumably an alternative will be added before this is removed
-    dimos-repo = { url = "git+file:../../../.."; flake = false; };
+    # presumably an alternative will be added before this is removed.
+    dimos-repo = { url = "git+file:../../../..?ref=main"; flake = false; };
   };
 
   outputs = { self, nixpkgs, flake-utils, dimos-repo }:
@@ -34,7 +34,7 @@
           cargoRoot = "dimos/mapping/ray_tracing/rust";
           buildAndTestSubdir = "dimos/mapping/ray_tracing/rust";
 
-          cargoHash = "sha256-g30NaoLdtWT5YBsEnE4Xv+EMnI5HHFtZAUtdEL/VbKQ=";
+          cargoHash = "sha256-0d0dlNDvDplA7oWTyUWOCOlS74Zie8uMQ+ps6lXntOI=";
 
           meta.mainProgram = "voxel_ray_tracing";
         };