PointCloud2 Efficient Decoder

assets/foxglove_dashboards/unitree.json

@@ -2,14 +2,14 @@
   "configById": {
     "3D!3ezwzdr": {
       "cameraState": {
-        "perspective": true,
-        "distance": 11.974738784563408,
-        "phi": 46.81551198543342,
-        "thetaOffset": 71.81370622323534,
+        "perspective": false,
+        "distance": 43.169390434335696,
+        "phi": 24.156826900904342,
+        "thetaOffset": 0.3206650831361572,
         "targetOffset": [
-          0.7911075559165568,
-          1.8116053369121197,
-          3.4025317970075274e-16
+          -1.497522603906827,
+          0.6788138652418518,
+          -1.8419947991856367e-20
         ],
         "target": [
           0,
@@ -52,7 +52,7 @@
           "visible": true
         },
         "frame:world": {
-          "visible": true
+          "visible": false
         }
       },
       "topics": {
@@ -63,7 +63,7 @@
           "colorMap": "turbo",
           "pointSize": 2.85,
           "decayTime": 6,
-          "pointShape": "cube"
+          "pointShape": "circle"
         },
         "/detectorDB/scene_update": {
           "visible": true
@@ -117,23 +117,24 @@
           "flatColor": "#00aaffff"
         },
         "/global_map": {
-          "visible": true,
+          "visible": false,
           "colorField": "z",
           "colorMode": "colormap",
           "colorMap": "turbo",
           "pointSize": 3.66,
-          "pointShape": "cube",
+          "pointShape": "square",
           "explicitAlpha": 1,
           "cubeSize": 0.05,
           "cubeOutline": false,
-          "flatColor": "#ed8080ff"
+          "flatColor": "#ed8080ff",
+          "minValue": -0.1
         },
         "/global_costmap": {
-          "visible": false,
+          "visible": true,
           "colorMode": "custom",
           "unknownColor": "#80808000",
-          "minColor": "#282975ff",
-          "maxColor": "#ff0000ff",
+          "minColor": "#36377cff",
+          "maxColor": "#000000ff",
           "frameLocked": false,
           "drawBehind": false
         },
@@ -203,6 +204,10 @@
           "maxColor": "#ff2222ff",
           "minColor": "#00006bff",
           "unknownColor": "#80808000"
+        },
+        "/debug_navigation": {
+          "visible": false,
+          "cameraInfoTopic": "/camera_info"
         }
       },
       "layers": {
@@ -242,6 +247,26 @@
       "imageMode": {},
       "followTf": "world"
     },
+    "command-center-extension.command-center!3xr2po0": {},
+    "Plot!3cog9zw": {
+      "paths": [
+        {
+          "timestampMethod": "receiveTime",
+          "value": "/metrics/_calculate_costmap.data",
+          "enabled": true,
+          "color": "#4e98e2",
+          "id": "a1ff9a80-7a45-48ff-bdb1-232bda7bd492"
+        }
+      ],
+      "showXAxisLabels": true,
+      "showYAxisLabels": true,
+      "showLegend": true,
+      "legendDisplay": "floating",
+      "showPlotValuesInLegend": false,
+      "isSynced": true,
+      "xAxisVal": "timestamp",
+      "sidebarDimension": 240
+    },
     "Image!3mnp456": {
       "cameraState": {
         "distance": 20,
@@ -308,7 +333,7 @@
           "visible": false
         },
         "/global_costmap": {
-          "visible": true,
+          "visible": false,
           "minColor": "#ffffff00",
           "maxColor": "#ff0000ff",
           "unknownColor": "#80808000"
@@ -351,7 +376,8 @@
           "colorMode": "colormap",
           "colorMap": "turbo",
           "pointShape": "cube",
-          "pointSize": 5
+          "pointSize": 5,
+          "cubeSize": 0.03
         },
         "/detected/pointcloud/1": {
           "visible": false,
@@ -467,6 +493,32 @@
         "calibrationTopic": "/camera_info"
       },
       "foxglovePanelTitle": ""
+    },
+    "Plot!47kna9v": {
+      "paths": [
+        {
+          "timestampMethod": "receiveTime",
+          "value": "/global_map.header.stamp.sec",
+          "enabled": true,
+          "color": "#4e98e2",
+          "id": "19f95865-4d9e-4d38-b9d7-d227319d8ebd"
+        },
+        {
+          "timestampMethod": "receiveTime",
+          "value": "/global_costmap.header.stamp.sec",
+          "enabled": true,
+          "color": "#f5774d",
+          "id": "86ddc0e2-8e9c-4d52-bd5a-d02cb0357efe"
+        }
+      ],
+      "showXAxisLabels": true,
+      "showYAxisLabels": true,
+      "showLegend": true,
+      "legendDisplay": "floating",
+      "showPlotValuesInLegend": false,
+      "isSynced": true,
+      "xAxisVal": "timestamp",
+      "sidebarDimension": 240
     }
   },
   "globalVariables": {},
@@ -480,7 +532,20 @@
   "layout": {
     "direction": "row",
     "first": "3D!3ezwzdr",
-    "second": "Image!3mnp456",
-    "splitPercentage": 70.55232558139535
+    "second": {
+      "first": "command-center-extension.command-center!3xr2po0",
+      "second": {
+        "first": {
+          "first": "Plot!3cog9zw",
+          "second": "Plot!47kna9v",
+          "direction": "row"
+        },
+        "second": "Image!3mnp456",
+        "direction": "column"
+      },
+      "direction": "column",
+      "splitPercentage": 50.116550116550115
+    },
+    "splitPercentage": 63.706720977596746
   }
 }

dimos/mapping/costmapper.py

@@ -22,10 +22,11 @@
     OCCUPANCY_ALGOS,
     HeightCostConfig,
     OccupancyConfig,
+    SimpleOccupancyConfig,
 )
 from dimos.msgs.nav_msgs import OccupancyGrid
 from dimos.robot.unitree_webrtc.type.lidar import LidarMessage
-from dimos.utils.reactive import backpressure
+from dimos.utils.metrics import timed
 
 
 @dataclass
@@ -46,9 +47,7 @@ def start(self) -> None:
         super().start()
 
         self._disposables.add(
-            backpressure(
-                self.global_map.observable()  # type: ignore[no-untyped-call]
-            )
+            self.global_map.observable()  # type: ignore[no-untyped-call]
             .pipe(ops.map(self._calculate_costmap))
             .subscribe(
                 self.global_costmap.publish,
@@ -59,6 +58,7 @@ def start(self) -> None:
     def stop(self) -> None:
         super().stop()
 
+    # @timed()  # TODO: fix thread leak in timed decorator
     def _calculate_costmap(self, msg: LidarMessage) -> OccupancyGrid:
         fn = OCCUPANCY_ALGOS[self.config.algo]
         return fn(msg, **asdict(self.config.config))

dimos/mapping/pointclouds/occupancy.py

@@ -83,7 +83,7 @@ def height_cost_occupancy(cloud: PointCloud2, **kwargs: Any) -> OccupancyGrid:
         OccupancyGrid with costs 0-100 based on terrain slope, -1 for unknown
     """
     cfg = HeightCostConfig(**kwargs)
-    points = cloud.as_numpy()
+    points = cloud.as_numpy().astype(np.float64)  # Upcast to avoid float32 rounding
     ts = cloud.ts if hasattr(cloud, "ts") and cloud.ts is not None else 0.0
 
     if len(points) == 0:
@@ -232,7 +232,7 @@ def general_occupancy(cloud: PointCloud2, **kwargs: Any) -> OccupancyGrid:
         OccupancyGrid with occupied cells where points were projected
     """
     cfg = GeneralOccupancyConfig(**kwargs)
-    points = cloud.as_numpy()
+    points = cloud.as_numpy().astype(np.float64)  # Upcast to avoid float32 rounding
 
     if len(points) == 0:
         return OccupancyGrid(
@@ -357,7 +357,7 @@ def simple_occupancy(cloud: PointCloud2, **kwargs: Any) -> OccupancyGrid:
         OccupancyGrid with occupied/free cells
     """
     cfg = SimpleOccupancyConfig(**kwargs)
-    points = cloud.as_numpy()
+    points = cloud.as_numpy().astype(np.float64)  # Upcast to avoid float32 rounding
 
     if len(points) == 0:
         return OccupancyGrid(

dimos/mapping/pointclouds/test_occupancy.py

@@ -20,7 +20,6 @@
 
 from dimos.core import LCMTransport
 from dimos.mapping.pointclouds.occupancy import (
-    general_occupancy,
     height_cost_occupancy,
     simple_occupancy,
 )
@@ -40,7 +39,6 @@ def apartment() -> PointCloud:
 @pytest.mark.parametrize(
     "occupancy_fn,output_name",
     [
-        (general_occupancy, "occupancy_general.png"),
         (simple_occupancy, "occupancy_simple.png"),
     ],
 )

dimos/mapping/pointclouds/test_occupancy_speed.py

@@ -0,0 +1,58 @@
+# Copyright 2025 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
+
+import pytest
+
+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.testing import TimedSensorReplay
+
+
+@pytest.mark.tool
+def test_build_map():
+    mapper = VoxelGridMapper(publish_interval=-1)
+
+    for ts, frame in TimedSensorReplay("unitree_go2_bigoffice/lidar").iterate_duration():
+        print(ts, frame)
+        mapper.add_frame(frame)
+
+    pickle_file = _get_data_dir() / "unitree_go2_bigoffice_map.pickle"
+    global_pcd = mapper.get_global_pointcloud2()
+
+    with open(pickle_file, "wb") as f:
+        pickle.dump(global_pcd, f)
+
+    mapper.stop()
+
+
+def test_costmap_calc():
+    path = get_data("unitree_go2_bigoffice_map.pickle")
+    pointcloud = pickle.loads(path.read_bytes())
+
+    names = []
+    times_ms = []
+    for name, algo in OCCUPANCY_ALGOS.items():
+        start = time.perf_counter()
+        result = algo(pointcloud)
+        elapsed = time.perf_counter() - start
+        names.append(name)
+        times_ms.append(elapsed * 1000)
+        print(f"{name}: {elapsed * 1000:.1f}ms - {result}")
+
+    bar(names, times_ms, title="Occupancy Algorithm Speed", ylabel="ms")