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

.gitignore

@@ -79,7 +79,6 @@ 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=self.frame_id,
-                child_frame_id=self.config.child_frame_id,
+                frame_id=FRAME_ODOM,
+                child_frame_id=FRAME_BODY,
                 translation=Vector3(
                     msg.pose.position.x,
                     msg.pose.position.y,

dimos/mapping/costmapper.py

@@ -14,9 +14,7 @@
 
 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
 
@@ -34,33 +32,10 @@
 
 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):
@@ -107,27 +82,5 @@ def stop(self) -> None:
 
     # @timed()  # TODO: fix thread leak in timed decorator
     def _calculate_costmap(self, msg: PointCloud2) -> OccupancyGrid:
-        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
+        fn = OCCUPANCY_ALGOS[self.config.algo]
+        return fn(msg, **asdict(self.config.config))

dimos/mapping/ray_tracing/module.py

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

dimos/msgs/nav_msgs/OccupancyGrid.py

@@ -485,7 +485,6 @@ 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.
 
@@ -505,9 +504,8 @@ def to_rerun(
             step_w = max(1, grid.shape[1] // max_tex)
             grid = grid[::step_h, ::step_w]
 
-        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)])
+        lut = _build_occupancy_lut(colormap, opacity, background)
+        rgba = np.ascontiguousarray(lut[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,7 +24,6 @@
 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
@@ -132,19 +131,6 @@ 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,12 +68,11 @@ 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=frame_id,
+            frame_id="world",
             position=[world_point.x, world_point.y, 0.0],
             orientation=Quaternion(0, 0, 0, 1),  # Identity quaternion
         )
@@ -82,7 +81,7 @@ def _reconstruct_path(
 
     start_world_point = costmap.grid_to_world(start_tuple)
     start_pose = PoseStamped(
-        frame_id=frame_id,
+        frame_id="world",
         position=[start_world_point.x, start_world_point.y, 0.0],
         orientation=Quaternion(0, 0, 0, 1),
     )
@@ -98,32 +97,31 @@ 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=frame_id,
+            frame_id="world",
             position=[goal_point.x, goal_point.y, 0.0],
             orientation=Quaternion(0, 0, 0, 1),
         )
         waypoints.append(goal_pose)
 
-    return Path(frame_id=frame_id, poses=waypoints)
+    return Path(frame_id="world", 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=frame_id,
+            frame_id="world",
             position=[world_point.x, world_point.y, 0.0],
             orientation=Quaternion(0, 0, 0, 1),
         )
         waypoints.append(pose)
 
-    return Path(frame_id=frame_id, poses=waypoints)
+    return Path(frame_id="world", poses=waypoints)
 
 
 def min_cost_astar(

dimos/navigation/replanning_a_star/module.py

@@ -19,13 +19,12 @@
 from reactivex.disposable import Disposable
 
 from dimos.core.core import rpc
-from dimos.core.module import Module, ModuleConfig
+from dimos.core.module import Module
 from dimos.core.stream import In, Out
 from dimos.msgs.geometry_msgs.PointStamped import PointStamped
 from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped
 from dimos.msgs.geometry_msgs.Twist import Twist
 from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid
-from dimos.msgs.nav_msgs.Odometry import Odometry
 from dimos.msgs.nav_msgs.Path import Path
 from dimos.navigation.base import NavigationInterface, NavigationState
 from dimos.navigation.replanning_a_star.global_planner import GlobalPlanner
@@ -34,16 +33,8 @@
 logger = setup_logger()
 
 
-class ReplanningAStarPlannerConfig(ModuleConfig):
-    robot_width: float | None = None
-    robot_rotation_diameter: float | None = None
-
-
 class ReplanningAStarPlanner(Module, NavigationInterface):
-    config: ReplanningAStarPlannerConfig
-
     odom: In[PoseStamped]  # TODO: Use TF.
-    odometry: In[Odometry]
     global_costmap: In[OccupancyGrid]
     goal_request: In[PoseStamped]
     clicked_point: In[PointStamped]
@@ -60,31 +51,13 @@ class ReplanningAStarPlanner(Module, NavigationInterface):
 
     def __init__(self, **kwargs: Any) -> None:
         super().__init__(**kwargs)
-        overrides = {
-            name: value
-            for name, value in (
-                ("robot_width", self.config.robot_width),
-                ("robot_rotation_diameter", self.config.robot_rotation_diameter),
-            )
-            if value is not None
-        }
-        effective_global_config = (
-            self.config.g.model_copy(update=overrides) if overrides else self.config.g
-        )
-        self._planner = GlobalPlanner(effective_global_config)
+        self._planner = GlobalPlanner(self.config.g)
 
     @rpc
     def start(self) -> None:
         super().start()
 
         self.register_disposable(Disposable(self.odom.subscribe(self._planner.handle_odom)))
-        self.register_disposable(
-            Disposable(
-                self.odometry.subscribe(
-                    lambda msg: self._planner.handle_odom(msg.to_pose_stamped())
-                )
-            )
-        )
         self.register_disposable(
             Disposable(self.global_costmap.subscribe(self._planner.handle_global_costmap))
         )