refactor: generated CDR voxel map streams

.github/workflows/message-codegen.yml

@@ -20,6 +20,7 @@ on:
       - dimos/protocol/tf/**
       - dimos/memory/**
       - dimos/mapping/occupancy/**
+      - dimos/mapping/voxels/**
       - dimos/mapping/pointclouds/occupancy.py
       - dimos/mapping/odometry_hist.py
       - dimos/mapping/test_generated_odometry_hist.py
@@ -246,3 +247,11 @@ jobs:
         run: |
           .native-review/bin/python -I -c 'import dimos.mapping.odometry_hist as history; import dimos.visualization.rerun.cdr as viewer; import pytest; import sys; assert all("site-packages" in str(value.__file__) for value in (history, viewer)); raise SystemExit(pytest.main(sys.argv[1:]))' --noconftest --import-mode=importlib -o addopts='' -q "$PWD/dimos/mapping/test_generated_odometry_hist.py" "$PWD/dimos/visualization/rerun/test_cdr.py"
           MYPYPATH="$PWD/build/message-source/python" .native-review/bin/python -m mypy --follow-imports=silent dimos/mapping/odometry_hist.py dimos/visualization/rerun/cdr.py
+      - name: Verify generated voxel streams and CPU stores over memory CDR
+        if: runner.os == 'Linux'
+        env:
+          OMP_NUM_THREADS: '2'
+          NUMBA_NUM_THREADS: '2'
+        run: |
+          .native-review/bin/python -I -c 'from dimos.mapping.voxels import grid, module; from dimos.mapping.voxels.impl import packed, o3d; import pytest; import sys; assert all("site-packages" in str(value.__file__) for value in (grid, module, packed, o3d)); raise SystemExit(pytest.main(sys.argv[1:]))' --noconftest --import-mode=importlib -o addopts='' -q "$PWD/dimos/mapping/voxels/test_generated_stream.py" "$PWD/dimos/mapping/voxels/test_stamped.py"
+          MYPYPATH="$PWD/build/message-source/python" .native-review/bin/python -m mypy --follow-imports=silent dimos/mapping/voxels/grid.py dimos/mapping/voxels/module.py dimos/mapping/voxels/impl/packed.py dimos/mapping/voxels/impl/o3d.py

dimos/mapping/voxels/grid.py

@@ -18,6 +18,9 @@
 import time
 from typing import TYPE_CHECKING
 
+import numpy as np
+from numpy.typing import NDArray
+
 from dimos.mapping.voxels.impl.o3d import O3dVoxels
 from dimos.mapping.voxels.impl.packed import PackedVoxels
 from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2
@@ -87,6 +90,24 @@ def add_frame(self, frame: PointCloud2) -> None:
         self.get_global_pointcloud.invalidate_cache(self)
         self.get_global_pointcloud2.invalidate_cache(self)
 
+    def add_points(self, points: NDArray[np.float32], *, timestamp: float | None = None) -> None:
+        """Accumulate finite Nx3 coordinates without requiring a message wrapper.
+
+        The optional per-frame timestamp uses the existing store's seconds unit;
+        stream adapters keep their exact source Header separately.
+        """
+        self._check_disposed()
+        values = np.asarray(points, dtype=np.float32)
+        if values.ndim != 2 or values.shape[1] != 3:
+            raise ValueError("Voxel points must have shape Nx3")
+        if not np.isfinite(values).all():
+            raise ValueError("Voxel points must be finite")
+        if timestamp is not None:
+            self._latest_frame_ts = timestamp
+        self._impl.add_points(values, timestamp)
+        self.get_global_pointcloud.invalidate_cache(self)
+        self.get_global_pointcloud2.invalidate_cache(self)
+
     def reproject(self, place: Callable[[PointCloud2], PointCloud2]) -> None:
         """Move every voxel to where `place` puts its stamped original position."""
         self._check_disposed()
@@ -113,7 +134,7 @@ def get_global_pointcloud(self) -> o3d.t.geometry.PointCloud:
         self._check_disposed()
         cpu = o3c.Device("CPU:0")
         out = o3d.t.geometry.PointCloud(device=cpu)
-        out.point["positions"] = o3c.Tensor(self._impl.points(), device=cpu)
+        out.point["positions"] = o3c.Tensor(self._impl.points(), dtype=o3c.float32, device=cpu)
         return out
 
     def size(self) -> int:

dimos/mapping/voxels/impl/o3d.py

@@ -17,6 +17,7 @@
 from typing import TYPE_CHECKING
 
 import numpy as np
+from numpy.typing import NDArray
 
 from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2
 
@@ -74,7 +75,18 @@ def add_frame(self, frame: PointCloud2) -> None:
         if pcd.is_empty():
             return
 
-        pts = pcd.point["positions"].to(self._dev, o3c.float32)
+        self._insert_points(pcd.point["positions"].to(self._dev, o3c.float32))
+
+    def add_points(self, points: NDArray[np.float32], _timestamp: float | None = None) -> None:
+        import open3d.core as o3c  # type: ignore[import-untyped]
+
+        if not len(points):
+            return
+        self._insert_points(o3c.Tensor(points, dtype=o3c.float32, device=self._dev))
+
+    def _insert_points(self, pts: o3c.Tensor) -> None:
+        import open3d.core as o3c  # type: ignore[import-untyped]
+
         vox = (pts / self._voxel_size).floor().to(self._key_dtype)
         keys_Nx3 = vox.contiguous()
 
@@ -108,7 +120,7 @@ def _carve_and_insert(self, new_keys: o3c.Tensor) -> None:
             value_element_shapes=[o3c.SizeVector([1])],
             device=self._dev,
         )
-        dummy_vals = o3c.Tensor.zeros((xy_keys.shape[0], 1), o3c.uint8, self._dev)
+        dummy_vals = o3c.Tensor.zeros(o3c.SizeVector([xy_keys.shape[0], 1]), o3c.uint8, self._dev)
         xy_hashmap.insert(xy_keys, dummy_vals)
 
         active_indices = self._hashmap.active_buf_indices()
@@ -142,5 +154,6 @@ def size(self) -> int:
 
     def dispose(self) -> None:
         """Free GPU resources."""
-        self.vbg = None
-        self._hashmap = None
+        if self.vbg is not None:
+            self.vbg = None
+            del self._hashmap

dimos/mapping/voxels/impl/packed.py

@@ -17,6 +17,7 @@
 from collections.abc import Callable
 
 import numpy as np
+from numpy.typing import NDArray
 
 from dimos.mapping.voxels.keys import FIELD_BITS as _BITS, FIELD_MASK as _MASK, KEY_OFFSET as _BIAS
 from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2
@@ -56,7 +57,9 @@ def _centers(self, k: np.ndarray) -> np.ndarray:
         return (vox + np.float32(0.5)) * np.float32(self._voxel_size)
 
     def add_frame(self, frame: PointCloud2) -> None:
-        pts = frame.points_f32()
+        self.add_points(frame.points_f32(), frame.ts)
+
+    def add_points(self, pts: NDArray[np.float32], timestamp: float | None = None) -> None:
         if not len(pts):
             return
         new = np.unique(self._pack(pts))
@@ -84,7 +87,7 @@ def add_frame(self, frame: PointCloud2) -> None:
             pos, new = pos[fresh], new[fresh]
         self._keys = np.insert(keys, pos, new)
         if self.stamped:
-            self._ts = np.insert(self._ts, pos, frame.ts)
+            self._ts = np.insert(self._ts, pos, np.nan if timestamp is None else timestamp)
             self._raw = np.insert(self._raw, pos, new)
 
     def reproject(self, place: Callable[[PointCloud2], PointCloud2]) -> None:

dimos/mapping/voxels/module.py

@@ -14,16 +14,21 @@
 
 from __future__ import annotations
 
+from copy import deepcopy
 from typing import TYPE_CHECKING, Any
 
+import numpy as np
+
 from dimos.core.core import rpc
 from dimos.core.module import ModuleConfig
 from dimos.core.stream import In, Out
 from dimos.mapping.voxels.grid import VoxelGrid
 from dimos.memory.module import StreamModule
 from dimos.memory.stream import Stream
 from dimos.memory.transform import Transformer
-from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2
+from dimos.messages.sensor_msgs.msg import PointCloud2
+from dimos.msgs.pointcloud import pointcloud_from_xyz, pointcloud_xyz
+from dimos.msgs.time import to_seconds
 
 if TYPE_CHECKING:
     from collections.abc import Iterator
@@ -53,8 +58,11 @@ def _make_obs(
         self, grid: VoxelGrid, last_obs: Observation[PointCloud2], count: int
     ) -> Observation[PointCloud2]:
         # pose=None: the global map is in world frame, per-observation pose is meaningless
+        header = deepcopy(last_obs.data.header)
+        header.frame_id = self._grid_kwargs.get("frame_id", "world")
+        points = grid.get_global_pointcloud().point["positions"].numpy()
         return last_obs.derive(
-            data=grid.get_global_pointcloud2(),
+            data=pointcloud_from_xyz(points, header=header),
             pose=None,
             tags={**last_obs.tags, "frame_count": count},
         )
@@ -68,7 +76,9 @@ def __call__(
             count = 0
 
             for obs in upstream:
-                grid.add_frame(obs.data)
+                coordinates = pointcloud_xyz(obs.data)
+                points = coordinates[np.isfinite(coordinates).all(axis=1)].astype(np.float32)
+                grid.add_points(points, timestamp=to_seconds(obs.data.header.stamp))
                 last_obs = obs
                 count += 1