G1 raytracing navigation stack (unitree-g1-nav-simple)

.gitignore

@@ -72,6 +72,7 @@ yolo11n.pt
 /results
 results/
 **/cpp/result
+**/rust/result
 
 CLAUDE.MD
 /assets/teleop_certs/

dimos/hardware/sensors/lidar/fastlio2/module.py

@@ -171,8 +171,8 @@ def start(self) -> None:
     def _on_odom_for_tf(self, msg: Odometry) -> None:
         self.tf.publish(
             Transform(
-                frame_id=FRAME_ODOM,
-                child_frame_id=FRAME_BODY,
+                frame_id=self.frame_id,
+                child_frame_id=self.config.child_frame_id,
                 translation=Vector3(
                     msg.pose.position.x,
                     msg.pose.position.y,

dimos/mapping/costmapper.py

@@ -14,7 +14,9 @@
 
 from dataclasses import asdict
 import time
+from typing import Any
 
+import numpy as np
 from pydantic import Field
 from reactivex import combine_latest, operators as ops
 
@@ -32,10 +34,33 @@
 
 logger = setup_logger()
 
+_COLOR_UNKNOWN = (0, 0, 0, 0)
+_COLOR_FREE = (72, 73, 129, 255)
+_COLOR_OCCUPIED = (255, 140, 0, 255)
+_COLOR_LETHAL = (220, 30, 30, 255)
+
+# Indexed by grid value + 1: 0 = unknown, 1 = free, 2..101 = cost 1..100.
+_COSTMAP_COLOR_LOOKUP_TABLE = np.empty((102, 4), dtype=np.uint8)
+_COSTMAP_COLOR_LOOKUP_TABLE[0] = _COLOR_UNKNOWN
+_COSTMAP_COLOR_LOOKUP_TABLE[1] = _COLOR_FREE
+_COSTMAP_COLOR_LOOKUP_TABLE[2:101] = _COLOR_OCCUPIED
+_COSTMAP_COLOR_LOOKUP_TABLE[101] = _COLOR_LETHAL
+
+_COSTMAP_Z_OFFSET = 0.02
+
+
+def costmap_to_rerun(grid: OccupancyGrid) -> Any:
+    return grid.to_rerun(
+        color_lookup_table=_COSTMAP_COLOR_LOOKUP_TABLE,
+        z_offset=_COSTMAP_Z_OFFSET,
+    )
+
 
 class Config(ModuleConfig):
     algo: str = "height_cost"
     config: OccupancyConfig = Field(default_factory=HeightCostConfig)
+    # for robots that cant see directly below themself
+    initial_safe_radius_meters: float = 0.0
 
 
 class CostMapper(Module):
@@ -82,5 +107,27 @@ def stop(self) -> None:
 
     # @timed()  # TODO: fix thread leak in timed decorator
     def _calculate_costmap(self, msg: PointCloud2) -> OccupancyGrid:
-        fn = OCCUPANCY_ALGOS[self.config.algo]
-        return fn(msg, **asdict(self.config.config))
+        occupancy_function = OCCUPANCY_ALGOS[self.config.algo]
+        grid = occupancy_function(msg, **asdict(self.config.config))
+        self._apply_initial_safe_radius(grid)
+        return grid
+
+    def _apply_initial_safe_radius(self, grid: OccupancyGrid) -> None:
+        radius_meters = self.config.initial_safe_radius_meters
+        if radius_meters <= 0 or grid.grid.size == 0:
+            return
+
+        resolution = grid.resolution
+        origin_x = grid.origin.position.x
+        origin_y = grid.origin.position.y
+
+        rows, columns = np.ogrid[: grid.grid.shape[0], : grid.grid.shape[1]]
+        cell_world_x = columns * resolution + origin_x
+        cell_world_y = rows * resolution + origin_y
+        distance_squared_meters = cell_world_x**2 + cell_world_y**2
+
+        # Half-cell tolerance: a cell counts as inside if any part of it overlaps
+        # the disc. Avoids floating-point boundary flakiness from radius/resolution.
+        effective_radius_meters = radius_meters + resolution * 0.5
+        safe_mask = distance_squared_meters <= effective_radius_meters**2
+        grid.grid[safe_mask] = 0

dimos/mapping/ray_tracing/module.py

@@ -26,8 +26,8 @@
 
 class RayTracingVoxelMapConfig(NativeModuleConfig):
     cwd: str | None = "rust"
-    executable: str = "target/release/voxel_ray_tracing"
-    build_command: str | None = "cargo build --release"
+    executable: str = "result/bin/voxel_ray_tracing"
+    build_command: str | None = "nix build path:."
     stdin_config: bool = True
 
     voxel_size: float = 0.1

dimos/mapping/ray_tracing/rust/.gitignore

@@ -1 +1,2 @@
 target/
+result

dimos/mapping/ray_tracing/rust/flake.nix

@@ -0,0 +1,42 @@
+{
+  description = "Voxel ray tracing native module for DimOS";
+
+  inputs = {
+    nixpkgs.url = "github:NixOS/nixpkgs/nixos-unstable";
+    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; };
+  };
+
+  outputs = { self, nixpkgs, flake-utils, dimos-repo }:
+    flake-utils.lib.eachDefaultSystem (system:
+      let
+        pkgs = import nixpkgs { inherit system; };
+
+        src = pkgs.runCommand "voxel-ray-tracing-src" {} ''
+          mkdir -p $out/dimos/mapping/ray_tracing/rust
+          cp -r ${./src} $out/dimos/mapping/ray_tracing/rust/src
+          cp ${./Cargo.toml} $out/dimos/mapping/ray_tracing/rust/Cargo.toml
+          cp ${./Cargo.lock} $out/dimos/mapping/ray_tracing/rust/Cargo.lock
+
+          mkdir -p $out/native/rust
+          cp -r ${dimos-repo}/native/rust/dimos-module $out/native/rust/dimos-module
+          cp -r ${dimos-repo}/native/rust/dimos-module-macros $out/native/rust/dimos-module-macros
+        '';
+      in {
+        packages.default = pkgs.rustPlatform.buildRustPackage {
+          pname = "voxel-ray-tracing";
+          version = "0.1.0";
+
+          inherit src;
+          cargoRoot = "dimos/mapping/ray_tracing/rust";
+          buildAndTestSubdir = "dimos/mapping/ray_tracing/rust";
+
+          cargoHash = "sha256-g30NaoLdtWT5YBsEnE4Xv+EMnI5HHFtZAUtdEL/VbKQ=";
+
+          meta.mainProgram = "voxel_ray_tracing";
+        };
+      });
+}

dimos/msgs/nav_msgs/OccupancyGrid.py

@@ -485,6 +485,7 @@ def to_rerun(
         opacity: float = 1.0,
         cost_range: tuple[int, int] | None = None,
         background: str | None = None,
+        color_lookup_table: np.ndarray | None = None,
     ) -> Archetype:
         """Convert to 3D textured mesh overlay on floor plane.
 
@@ -504,8 +505,9 @@ def to_rerun(
             step_w = max(1, grid.shape[1] // max_tex)
             grid = grid[::step_h, ::step_w]
 
-        lut = _build_occupancy_lut(colormap, opacity, background)
-        rgba = np.ascontiguousarray(lut[np.clip(grid + 1, 0, 101)])
+        if color_lookup_table is None:
+            color_lookup_table = _build_occupancy_lut(colormap, opacity, background)
+        rgba = np.ascontiguousarray(color_lookup_table[np.clip(grid + 1, 0, 101)])
 
         # Apply cost_range filter on downsampled grid
         if cost_range is not None:

dimos/msgs/nav_msgs/Odometry.py

@@ -24,6 +24,7 @@
 import numpy as np
 
 from dimos.msgs.geometry_msgs.Pose import Pose
+from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped
 from dimos.msgs.geometry_msgs.PoseWithCovariance import PoseWithCovariance
 from dimos.msgs.geometry_msgs.Twist import Twist
 from dimos.msgs.geometry_msgs.TwistWithCovariance import TwistWithCovariance
@@ -131,6 +132,19 @@ def pitch(self) -> float:
     def yaw(self) -> float:
         return self.pose.yaw
 
+    def to_pose_stamped(self) -> PoseStamped:
+        return PoseStamped(
+            ts=self.ts,
+            frame_id=self.frame_id,
+            position=[self.position.x, self.position.y, self.position.z],
+            orientation=[
+                self.orientation.x,
+                self.orientation.y,
+                self.orientation.z,
+                self.orientation.w,
+            ],
+        )
+
     # -- Serialization --
 
     def lcm_encode(self) -> bytes:

dimos/navigation/replanning_a_star/min_cost_astar.py

@@ -68,11 +68,12 @@ def _reconstruct_path(
     start_tuple: tuple[int, int],
     goal_tuple: tuple[int, int],
 ) -> Path:
+    frame_id = costmap.frame_id
     waypoints: list[PoseStamped] = []
     while current in parents:
         world_point = costmap.grid_to_world(current)
         pose = PoseStamped(
-            frame_id="world",
+            frame_id=frame_id,
             position=[world_point.x, world_point.y, 0.0],
             orientation=Quaternion(0, 0, 0, 1),  # Identity quaternion
         )
@@ -81,7 +82,7 @@ def _reconstruct_path(
 
     start_world_point = costmap.grid_to_world(start_tuple)
     start_pose = PoseStamped(
-        frame_id="world",
+        frame_id=frame_id,
         position=[start_world_point.x, start_world_point.y, 0.0],
         orientation=Quaternion(0, 0, 0, 1),
     )
@@ -97,31 +98,32 @@ def _reconstruct_path(
         or (waypoints[-1].x - goal_point.x) ** 2 + (waypoints[-1].y - goal_point.y) ** 2 > 1e-10
     ):
         goal_pose = PoseStamped(
-            frame_id="world",
+            frame_id=frame_id,
             position=[goal_point.x, goal_point.y, 0.0],
             orientation=Quaternion(0, 0, 0, 1),
         )
         waypoints.append(goal_pose)
 
-    return Path(frame_id="world", poses=waypoints)
+    return Path(frame_id=frame_id, poses=waypoints)
 
 
 def _reconstruct_path_from_coords(
     path_coords: list[tuple[int, int]],
     costmap: OccupancyGrid,
 ) -> Path:
+    frame_id = costmap.frame_id
     waypoints: list[PoseStamped] = []
 
     for gx, gy in path_coords:
         world_point = costmap.grid_to_world((gx, gy))
         pose = PoseStamped(
-            frame_id="world",
+            frame_id=frame_id,
             position=[world_point.x, world_point.y, 0.0],
             orientation=Quaternion(0, 0, 0, 1),
         )
         waypoints.append(pose)
 
-    return Path(frame_id="world", poses=waypoints)
+    return Path(frame_id=frame_id, poses=waypoints)
 
 
 def min_cost_astar(