From 334433f6fea65078c2e7ade99787e2d34c25d1f2 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Fri, 22 May 2026 12:52:05 -0700 Subject: [PATCH 01/55] Basic structure --- .../nav_stack/evaluator/evaluator.py | 151 ++++++++++++++++++ dimos/navigation/nav_stack/evaluator/main.py | 41 +++++ .../nav_stack/evaluator/scenarios.py | 122 ++++++++++++++ .../evaluator/straight_line_planner.py | 95 +++++++++++ 4 files changed, 409 insertions(+) create mode 100644 dimos/navigation/nav_stack/evaluator/evaluator.py create mode 100644 dimos/navigation/nav_stack/evaluator/main.py create mode 100644 dimos/navigation/nav_stack/evaluator/scenarios.py create mode 100644 dimos/navigation/nav_stack/evaluator/straight_line_planner.py diff --git a/dimos/navigation/nav_stack/evaluator/evaluator.py b/dimos/navigation/nav_stack/evaluator/evaluator.py new file mode 100644 index 0000000000..01bb73af71 --- /dev/null +++ b/dimos/navigation/nav_stack/evaluator/evaluator.py @@ -0,0 +1,151 @@ +# Copyright 2026 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. + +"""Evaluate path planner modules with a set of loaded scenes. + +Sends out global map, start pose, goal pose, and listens for +paths. Then evaluate each path for various metrics. +""" + +from __future__ import annotations + +import asyncio +from collections.abc import AsyncGenerator +from dataclasses import dataclass +import time +from typing import Any + +from dimos.core.module import Module, ModuleConfig +from dimos.core.stream import In, Out +from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.nav_msgs.Path import Path +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.navigation.nav_stack.evaluator.scenarios import ( + PlannerScenario, + default_scenarios, +) +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +@dataclass +class ScenarioResult: + name: str + expected_path: bool + got_path: bool + passed: bool + + +class EvaluatorConfig(ModuleConfig): + # Pause between publishing each input (map → start → goal) so the planner + # has all three before its goal-trigger fires. LCM doesn't order topics. + input_publish_delay: float = 0.2 + # Max seconds to wait for the planner's path reply per scenario. + path_timeout: float = 2.0 + # Pause between scenarios so rerun has time to show each one distinctly. + scenario_dwell: float = 2.0 + + +class Evaluator(Module): + """Drives a fixed scenario sequence through a black-box planner.""" + + config: EvaluatorConfig + + global_map: Out[PointCloud2] + start_pose: Out[Odometry] + goal_pose: Out[Odometry] + path: In[Path] + + def __init__(self, **kwargs: Any) -> None: + super().__init__(**kwargs) + self._latest_path: Path | None = None + self._path_received: asyncio.Event | None = None + self._eval_task: asyncio.Task[None] | None = None + + async def main(self) -> AsyncGenerator[None, None]: + self._path_received = asyncio.Event() + self._eval_task = asyncio.create_task(self._run_eval()) + yield + if self._eval_task is not None and not self._eval_task.done(): + self._eval_task.cancel() + try: + await self._eval_task + except asyncio.CancelledError: + pass + + async def handle_path(self, msg: Path) -> None: + self._latest_path = msg + if self._path_received is not None: + self._path_received.set() + + async def _run_eval(self) -> None: + scenarios = default_scenarios() + results: list[ScenarioResult] = [] + logger.info("Evaluator starting", scenarios=len(scenarios)) + await asyncio.sleep(1.0) + for scenario in scenarios: + result = await self._run_one(scenario) + results.append(result) + await asyncio.sleep(self.config.scenario_dwell) + self._log_summary(results) + + async def _run_one(self, scenario: PlannerScenario) -> ScenarioResult: + logger.info("Scenario start", name=scenario.name, expect_path=scenario.expect_path) + assert self._path_received is not None + self._latest_path = None + self._path_received.clear() + + now = time.time() + scenario.global_map.ts = now + scenario.start_pose.ts = now + scenario.goal_pose.ts = now + + self.global_map.publish(scenario.global_map) + await asyncio.sleep(self.config.input_publish_delay) + self.start_pose.publish(scenario.start_pose) + await asyncio.sleep(self.config.input_publish_delay) + self.goal_pose.publish(scenario.goal_pose) + + try: + await asyncio.wait_for(self._path_received.wait(), timeout=self.config.path_timeout) + got_path = self._latest_path is not None and len(self._latest_path) > 0 + except asyncio.TimeoutError: + got_path = False + + passed = got_path == scenario.expect_path + logger.info( + "Scenario result", + name=scenario.name, + expected=scenario.expect_path, + got=got_path, + passed=passed, + ) + return ScenarioResult( + name=scenario.name, + expected_path=scenario.expect_path, + got_path=got_path, + passed=passed, + ) + + def _log_summary(self, results: list[ScenarioResult]) -> None: + n_pass = sum(1 for r in results if r.passed) + logger.info("Evaluation complete", passed=n_pass, total=len(results)) + for r in results: + logger.info( + " " + ("PASS" if r.passed else "FAIL"), + scenario=r.name, + expected=r.expected_path, + got=r.got_path, + ) diff --git a/dimos/navigation/nav_stack/evaluator/main.py b/dimos/navigation/nav_stack/evaluator/main.py new file mode 100644 index 0000000000..fec766fdcc --- /dev/null +++ b/dimos/navigation/nav_stack/evaluator/main.py @@ -0,0 +1,41 @@ +# Copyright 2026 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. + +"""Blueprint + entrypoint for the path-planner evaluator. + +Wires the Evaluator and StraightLinePlanner together and bridges all +streams to rerun. Run with:: + + python -m dimos.navigation.nav_stack.evaluator.main +""" + +from __future__ import annotations + +from dimos.core.coordination.blueprints import Blueprint, autoconnect +from dimos.core.coordination.module_coordinator import ModuleCoordinator +from dimos.navigation.nav_stack.evaluator.evaluator import Evaluator +from dimos.navigation.nav_stack.evaluator.straight_line_planner import StraightLinePlanner +from dimos.visualization.rerun.bridge import RerunBridgeModule + + +def create_evaluator_blueprint() -> Blueprint: + return autoconnect( + Evaluator.blueprint(), + StraightLinePlanner.blueprint(), + RerunBridgeModule.blueprint(), + ) + + +if __name__ == "__main__": + ModuleCoordinator.build(create_evaluator_blueprint()).loop() diff --git a/dimos/navigation/nav_stack/evaluator/scenarios.py b/dimos/navigation/nav_stack/evaluator/scenarios.py new file mode 100644 index 0000000000..44041202e9 --- /dev/null +++ b/dimos/navigation/nav_stack/evaluator/scenarios.py @@ -0,0 +1,122 @@ +# Copyright 2026 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. + +"""Hand-crafted synthetic scenarios for black-box planner evaluation. + +Each scenario is a self-contained (map, start, goal, expectation) bundle. +Maps are PointCloud2 obstacle clouds, start/goal are Odometry poses, and +``expect_path`` records whether a planner *should* be able to find a path +(used by the evaluator to score pass/fail). +""" + +from __future__ import annotations + +from dataclasses import dataclass + +import numpy as np + +from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + +WORLD_FRAME = "map" + + +@dataclass +class PlannerScenario: + name: str + global_map: PointCloud2 + start_pose: Odometry + goal_pose: Odometry + expect_path: bool + + +def _odom(x: float, y: float, z: float = 0.0, frame: str = WORLD_FRAME) -> Odometry: + pose = Pose(position=[x, y, z], orientation=[0.0, 0.0, 0.0, 1.0]) + return Odometry(frame_id=frame, child_frame_id="body", pose=pose) + + +def _cloud(points: np.ndarray, frame: str = WORLD_FRAME) -> PointCloud2: + if points.size == 0: + points = np.zeros((0, 3), dtype=np.float32) + return PointCloud2.from_numpy(points.astype(np.float32), frame_id=frame) + + +def _wall( + x0: float, y0: float, x1: float, y1: float, *, spacing: float = 0.1, height: float = 0.5 +) -> np.ndarray: + """Sample a vertical-wall obstacle as a line of points from (x0,y0) to (x1,y1).""" + length = float(np.hypot(x1 - x0, y1 - y0)) + n = max(2, int(np.ceil(length / spacing))) + xs = np.linspace(x0, x1, n) + ys = np.linspace(y0, y1, n) + zs = np.linspace(0.0, height, max(2, int(np.ceil(height / spacing)))) + grid_xs, grid_zs = np.meshgrid(xs, zs) + grid_ys, _ = np.meshgrid(ys, zs) + return np.column_stack([grid_xs.ravel(), grid_ys.ravel(), grid_zs.ravel()]) + + +def _floor( + x_min: float = -2.0, + x_max: float = 8.0, + y_min: float = -3.0, + y_max: float = 3.0, + spacing: float = 0.25, +) -> np.ndarray: + """Flat ground plane sampled as points at z=0.""" + xs = np.arange(x_min, x_max + spacing, spacing) + ys = np.arange(y_min, y_max + spacing, spacing) + grid_xs, grid_ys = np.meshgrid(xs, ys) + pts = np.column_stack([grid_xs.ravel(), grid_ys.ravel(), np.zeros(grid_xs.size)]) + return pts + + +def _empty_map() -> PointCloud2: + return _cloud(_floor()) + + +def _map_with_walls(*walls: np.ndarray) -> PointCloud2: + return _cloud(np.vstack([_floor(), *walls])) + + +def default_scenarios() -> list[PlannerScenario]: + """A small starter set covering reachable / unreachable / detour cases.""" + return [ + PlannerScenario( + name="open_field", + global_map=_empty_map(), + start_pose=_odom(0.0, 0.0), + goal_pose=_odom(5.0, 0.0), + expect_path=True, + ), + PlannerScenario( + name="wall_with_detour", + global_map=_map_with_walls(_wall(2.0, -1.5, 2.0, 1.5)), + start_pose=_odom(0.0, 0.0), + goal_pose=_odom(5.0, 0.0), + expect_path=True, + ), + PlannerScenario( + name="sealed_box", + global_map=_map_with_walls( + _wall(4.0, -1.0, 6.0, -1.0), + _wall(4.0, 1.0, 6.0, 1.0), + _wall(4.0, -1.0, 4.0, 1.0), + _wall(6.0, -1.0, 6.0, 1.0), + ), + start_pose=_odom(0.0, 0.0), + goal_pose=_odom(5.0, 0.0), + expect_path=False, + ), + ] diff --git a/dimos/navigation/nav_stack/evaluator/straight_line_planner.py b/dimos/navigation/nav_stack/evaluator/straight_line_planner.py new file mode 100644 index 0000000000..8b17a35101 --- /dev/null +++ b/dimos/navigation/nav_stack/evaluator/straight_line_planner.py @@ -0,0 +1,95 @@ +# Copyright 2026 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. + +"""Braindead path planner just for testing out the eval architecture. + +It just creates a straight line path from start to end. +""" + +from __future__ import annotations + +import time +from typing import Any + +import numpy as np + +from dimos.core.module import Module, ModuleConfig +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.nav_msgs.Path import Path +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +class StraightLinePlannerConfig(ModuleConfig): + world_frame: str = "map" + num_waypoints: int = 20 + + +class StraightLinePlanner(Module): + """Emits a straight-line Path from start to goal; ignores the map.""" + + config: StraightLinePlannerConfig + + global_map: In[PointCloud2] + start_pose: In[Odometry] + goal_pose: In[Odometry] + path: Out[Path] + + def __init__(self, **kwargs: Any) -> None: + super().__init__(**kwargs) + self._latest_start: Odometry | None = None + + async def handle_global_map(self, _msg: PointCloud2) -> None: + # Naive planner: map is intentionally ignored. + return + + async def handle_start_pose(self, msg: Odometry) -> None: + self._latest_start = msg + + async def handle_goal_pose(self, msg: Odometry) -> None: + start = self._latest_start + if start is None: + logger.warning("StraightLinePlanner received goal before start; skipping") + return + path = self._straight_line(start, msg) + self.path.publish(path) + + def _straight_line(self, start: Odometry, goal: Odometry) -> Path: + n = max(2, self.config.num_waypoints) + sx, sy, sz = start.x, start.y, start.z + gx, gy, gz = goal.x, goal.y, goal.z + xs = np.linspace(sx, gx, n) + ys = np.linspace(sy, gy, n) + zs = np.linspace(sz, gz, n) + orient = [ + start.orientation.x, + start.orientation.y, + start.orientation.z, + start.orientation.w, + ] + now = time.time() + poses = [ + PoseStamped( + ts=now, + frame_id=self.config.world_frame, + position=[float(x), float(y), float(z)], + orientation=orient, + ) + for x, y, z in zip(xs, ys, zs, strict=True) + ] + return Path(ts=now, frame_id=self.config.world_frame, poses=poses) From b1fe6921e0bdf9df4b2c2106fdc2f4927cc27673 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Fri, 22 May 2026 14:40:16 -0700 Subject: [PATCH 02/55] Mesh loader --- .../nav_stack/evaluator/evaluator.py | 2 +- .../nav_stack/evaluator/mesh_loader.py | 76 +++++++++++++++++++ .../nav_stack/evaluator/scenarios.py | 43 ++++------- 3 files changed, 91 insertions(+), 30 deletions(-) create mode 100644 dimos/navigation/nav_stack/evaluator/mesh_loader.py diff --git a/dimos/navigation/nav_stack/evaluator/evaluator.py b/dimos/navigation/nav_stack/evaluator/evaluator.py index 01bb73af71..e9bbe4425c 100644 --- a/dimos/navigation/nav_stack/evaluator/evaluator.py +++ b/dimos/navigation/nav_stack/evaluator/evaluator.py @@ -49,7 +49,7 @@ class ScenarioResult: class EvaluatorConfig(ModuleConfig): - # Pause between publishing each input (map → start → goal) so the planner + # Pause between publishing each input so the planner # has all three before its goal-trigger fires. LCM doesn't order topics. input_publish_delay: float = 0.2 # Max seconds to wait for the planner's path reply per scenario. diff --git a/dimos/navigation/nav_stack/evaluator/mesh_loader.py b/dimos/navigation/nav_stack/evaluator/mesh_loader.py new file mode 100644 index 0000000000..2e37c765d4 --- /dev/null +++ b/dimos/navigation/nav_stack/evaluator/mesh_loader.py @@ -0,0 +1,76 @@ +# Copyright 2026 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. + +"""Load a 3D mesh, sample points on its surfaces, voxel-downsample.""" + +from __future__ import annotations + +from pathlib import Path + +import numpy as np +import open3d as o3d # type: ignore[import-untyped] + +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +def load_voxelized_mesh( + path: str | Path, + voxel_size: float = 0.1, + num_sample_points: int = 2_000_000, + swap_y_up_to_z_up: bool = True, + recenter: bool = True, +) -> np.ndarray: + """Load a mesh file, sample its surface, voxel-downsample. + + GLB/glTF files use Y-up; we rotate to Z-up so the result drops into the + planner's frame. ``recenter`` translates so the XY bbox is centered on + the origin and the floor (minimum z) sits at z=0. + """ + mesh = o3d.io.read_triangle_mesh(str(path)) + if len(mesh.vertices) == 0: + raise ValueError(f"Mesh {path!r} has no vertices") + logger.info( + "Mesh loaded", + path=str(path), + vertices=len(mesh.vertices), + triangles=len(mesh.triangles), + ) + + pcd = mesh.sample_points_uniformly(number_of_points=num_sample_points) + points = np.asarray(pcd.points) + + if swap_y_up_to_z_up: + # 90 deg rotation around X: (x, y, z) to (x, -z, y). + points = np.column_stack([points[:, 0], -points[:, 2], points[:, 1]]) + + if recenter: + xy_center = (points[:, :2].max(axis=0) + points[:, :2].min(axis=0)) / 2 + points[:, :2] -= xy_center + points[:, 2] -= points[:, 2].min() + + # Snap each occupied cell to its voxel-grid center, with the grid + # anchored at world origin so cells line up cleanly across scenarios. + quantized = (np.floor(points / voxel_size) + 0.5) * voxel_size + centers = np.unique(quantized, axis=0).astype(np.float32) + + logger.info( + "Voxelized mesh ready", + voxels=len(centers), + voxel_size=voxel_size, + bbox_min=centers.min(axis=0).round(2).tolist(), + bbox_max=centers.max(axis=0).round(2).tolist(), + ) + return centers diff --git a/dimos/navigation/nav_stack/evaluator/scenarios.py b/dimos/navigation/nav_stack/evaluator/scenarios.py index 44041202e9..ee4331edc9 100644 --- a/dimos/navigation/nav_stack/evaluator/scenarios.py +++ b/dimos/navigation/nav_stack/evaluator/scenarios.py @@ -29,8 +29,10 @@ from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.navigation.nav_stack.evaluator.mesh_loader import load_voxelized_mesh WORLD_FRAME = "map" +MESH_PATH = "/home/andrew/Downloads/model.glb" @dataclass @@ -90,33 +92,16 @@ def _map_with_walls(*walls: np.ndarray) -> PointCloud2: return _cloud(np.vstack([_floor(), *walls])) +def _mesh_scene(name: str, mesh_path: str, expect_path: bool = True) -> PlannerScenario: + points = load_voxelized_mesh(mesh_path) + return PlannerScenario( + name=name, + global_map=_cloud(points), + start_pose=_odom(-5.0, 0.0), + goal_pose=_odom(5.0, 0.0), + expect_path=expect_path, + ) + + def default_scenarios() -> list[PlannerScenario]: - """A small starter set covering reachable / unreachable / detour cases.""" - return [ - PlannerScenario( - name="open_field", - global_map=_empty_map(), - start_pose=_odom(0.0, 0.0), - goal_pose=_odom(5.0, 0.0), - expect_path=True, - ), - PlannerScenario( - name="wall_with_detour", - global_map=_map_with_walls(_wall(2.0, -1.5, 2.0, 1.5)), - start_pose=_odom(0.0, 0.0), - goal_pose=_odom(5.0, 0.0), - expect_path=True, - ), - PlannerScenario( - name="sealed_box", - global_map=_map_with_walls( - _wall(4.0, -1.0, 6.0, -1.0), - _wall(4.0, 1.0, 6.0, 1.0), - _wall(4.0, -1.0, 4.0, 1.0), - _wall(6.0, -1.0, 6.0, 1.0), - ), - start_pose=_odom(0.0, 0.0), - goal_pose=_odom(5.0, 0.0), - expect_path=False, - ), - ] + return [_mesh_scene("loaded_mesh", MESH_PATH)] From 6d0ca547100f41d7307b3ef5773a10e9d0853c09 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Fri, 22 May 2026 15:03:28 -0700 Subject: [PATCH 03/55] Add scenarios and render start and goal --- dimos/navigation/nav_stack/evaluator/main.py | 31 ++++++++++++++++++- .../nav_stack/evaluator/scenarios.py | 29 ++++++++++------- 2 files changed, 47 insertions(+), 13 deletions(-) diff --git a/dimos/navigation/nav_stack/evaluator/main.py b/dimos/navigation/nav_stack/evaluator/main.py index fec766fdcc..a231b85953 100644 --- a/dimos/navigation/nav_stack/evaluator/main.py +++ b/dimos/navigation/nav_stack/evaluator/main.py @@ -22,18 +22,47 @@ from __future__ import annotations +from typing import Any + from dimos.core.coordination.blueprints import Blueprint, autoconnect from dimos.core.coordination.module_coordinator import ModuleCoordinator from dimos.navigation.nav_stack.evaluator.evaluator import Evaluator from dimos.navigation.nav_stack.evaluator.straight_line_planner import StraightLinePlanner from dimos.visualization.rerun.bridge import RerunBridgeModule +_POSE_MARKER_RADIUS = 0.4 + + +def _render_start_pose(msg: Any) -> Any: + import rerun as rr + + return rr.Points3D( + positions=[[msg.x, msg.y, msg.z]], + colors=[[0, 255, 0]], # green + radii=[_POSE_MARKER_RADIUS], + ) + + +def _render_goal_pose(msg: Any) -> Any: + import rerun as rr + + return rr.Points3D( + positions=[[msg.x, msg.y, msg.z]], + colors=[[255, 0, 0]], # red + radii=[_POSE_MARKER_RADIUS], + ) + def create_evaluator_blueprint() -> Blueprint: return autoconnect( Evaluator.blueprint(), StraightLinePlanner.blueprint(), - RerunBridgeModule.blueprint(), + RerunBridgeModule.blueprint( + visual_override={ + "world/start_pose": _render_start_pose, + "world/goal_pose": _render_goal_pose, + } + ), ) diff --git a/dimos/navigation/nav_stack/evaluator/scenarios.py b/dimos/navigation/nav_stack/evaluator/scenarios.py index ee4331edc9..f5b9a03e20 100644 --- a/dimos/navigation/nav_stack/evaluator/scenarios.py +++ b/dimos/navigation/nav_stack/evaluator/scenarios.py @@ -92,16 +92,21 @@ def _map_with_walls(*walls: np.ndarray) -> PointCloud2: return _cloud(np.vstack([_floor(), *walls])) -def _mesh_scene(name: str, mesh_path: str, expect_path: bool = True) -> PlannerScenario: - points = load_voxelized_mesh(mesh_path) - return PlannerScenario( - name=name, - global_map=_cloud(points), - start_pose=_odom(-5.0, 0.0), - goal_pose=_odom(5.0, 0.0), - expect_path=expect_path, - ) - - def default_scenarios() -> list[PlannerScenario]: - return [_mesh_scene("loaded_mesh", MESH_PATH)] + cloud = _cloud(load_voxelized_mesh(MESH_PATH)) + return [ + PlannerScenario( + name="outside", + global_map=cloud, + start_pose=_odom(-20.45, -19.85, 1.75), + goal_pose=_odom(21.95, -4.25, 1.75), + expect_path=True, + ), + PlannerScenario( + name="up the stairs", + global_map=cloud, + start_pose=_odom(7.15, -3.55, 2.05), + goal_pose=_odom(5.55, -2.05, 5.65), + expect_path=True, + ), + ] From 21110ac22141ec8791c230e1e3d020d164a74d00 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Fri, 22 May 2026 15:32:17 -0700 Subject: [PATCH 04/55] Setup --- dimos/navigation/nav_stack/evaluator/main.py | 10 +++ .../modules/mls_planner/mls_planner.py | 68 +++++++++++++++++++ 2 files changed, 78 insertions(+) create mode 100644 dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py diff --git a/dimos/navigation/nav_stack/evaluator/main.py b/dimos/navigation/nav_stack/evaluator/main.py index a231b85953..8fd70d8c0f 100644 --- a/dimos/navigation/nav_stack/evaluator/main.py +++ b/dimos/navigation/nav_stack/evaluator/main.py @@ -53,6 +53,14 @@ def _render_goal_pose(msg: Any) -> Any: ) +def _render_global_map(msg: Any) -> Any: + return msg.to_rerun(voxel_size=0.03) + + +def _render_surface_map(msg: Any) -> Any: + return msg.to_rerun(mode="boxes", size=0.1) + + def create_evaluator_blueprint() -> Blueprint: return autoconnect( Evaluator.blueprint(), @@ -61,6 +69,8 @@ def create_evaluator_blueprint() -> Blueprint: visual_override={ "world/start_pose": _render_start_pose, "world/goal_pose": _render_goal_pose, + "world/global_map": _render_global_map, + "world/surface_map": _render_surface_map, } ), ) diff --git a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py new file mode 100644 index 0000000000..f2d27744cf --- /dev/null +++ b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py @@ -0,0 +1,68 @@ +# Copyright 2026 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. + +"""Multi-Level Surface (MLS) path planner. + +Extracts walkable surfaces from a voxelized global map, builds a sparse +waypoint graph over those surfaces, and plans paths via local A* plus +shortest-path search on the graph. Skeleton — algorithm is filled in +piecewise. +""" + +from __future__ import annotations + +from typing import Any + +from dimos.core.module import Module, ModuleConfig +from dimos.core.stream import In, Out +from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.nav_msgs.Path import Path +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +class MLSPlannerConfig(ModuleConfig): + world_frame: str = "map" + + +class MLSPlanner(Module): + config: MLSPlannerConfig + + global_map: In[PointCloud2] + start_pose: In[Odometry] + goal_pose: In[Odometry] + path: Out[Path] + surface_map: Out[PointCloud2] + + def __init__(self, **kwargs: Any) -> None: + super().__init__(**kwargs) + self._latest_start: Odometry | None = None + + async def handle_global_map(self, _msg: PointCloud2) -> None: + return + + async def handle_start_pose(self, msg: Odometry) -> None: + self._latest_start = msg + + async def handle_goal_pose(self, msg: Odometry) -> None: + if self._latest_start is None: + logger.warning("MLSPlanner received goal before start; skipping") + return + logger.info( + "MLSPlanner goal received (not yet implemented)", + start=(self._latest_start.x, self._latest_start.y, self._latest_start.z), + goal=(msg.x, msg.y, msg.z), + ) From 7d0d2ce1b21dbf618ec92d016969b039f55cd3ae Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Fri, 22 May 2026 15:36:56 -0700 Subject: [PATCH 05/55] The floor is purp and assume cube --- dimos/navigation/nav_stack/evaluator/main.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/dimos/navigation/nav_stack/evaluator/main.py b/dimos/navigation/nav_stack/evaluator/main.py index 8fd70d8c0f..55832a3575 100644 --- a/dimos/navigation/nav_stack/evaluator/main.py +++ b/dimos/navigation/nav_stack/evaluator/main.py @@ -27,7 +27,7 @@ from dimos.core.coordination.blueprints import Blueprint, autoconnect from dimos.core.coordination.module_coordinator import ModuleCoordinator from dimos.navigation.nav_stack.evaluator.evaluator import Evaluator -from dimos.navigation.nav_stack.evaluator.straight_line_planner import StraightLinePlanner +from dimos.navigation.nav_stack.modules.mls_planner.mls_planner import MLSPlanner from dimos.visualization.rerun.bridge import RerunBridgeModule _POSE_MARKER_RADIUS = 0.4 @@ -58,13 +58,13 @@ def _render_global_map(msg: Any) -> Any: def _render_surface_map(msg: Any) -> Any: - return msg.to_rerun(mode="boxes", size=0.1) + return msg.to_rerun(mode="boxes", size=0.1, colors=[128, 0, 128]) # purple def create_evaluator_blueprint() -> Blueprint: return autoconnect( Evaluator.blueprint(), - StraightLinePlanner.blueprint(), + MLSPlanner.blueprint(), RerunBridgeModule.blueprint( visual_override={ "world/start_pose": _render_start_pose, From 2aba8e520cd261b4505f341f40bf59ad6c26c87d Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Fri, 22 May 2026 15:47:15 -0700 Subject: [PATCH 06/55] Basic surface detection --- .../modules/mls_planner/mls_planner.py | 57 ++++++++++++++++++- 1 file changed, 55 insertions(+), 2 deletions(-) diff --git a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py index f2d27744cf..13a5850a3c 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py +++ b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py @@ -22,8 +22,11 @@ from __future__ import annotations +import time from typing import Any +import numpy as np + from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out from dimos.msgs.nav_msgs.Odometry import Odometry @@ -36,6 +39,45 @@ class MLSPlannerConfig(ModuleConfig): world_frame: str = "map" + voxel_size: float = 0.1 + robot_height: float = 1.0 + + +def _extract_surfaces(points: np.ndarray, voxel_size: float, robot_height: float) -> np.ndarray: + """Find walkable surface tops in a voxelized point cloud. + + Iterate through all the columns, find continuous areas of + free space. If the free space column is at least robot height, + add the bottom of this range as a surface. + """ + if len(points) == 0: + return np.zeros((0, 3), dtype=np.float32) + + indices = np.floor(points / voxel_size).astype(np.int64) + ix, iy, iz = indices[:, 0], indices[:, 1], indices[:, 2] + + order = np.lexsort((iz, iy, ix)) + sx, sy, sz = ix[order], iy[order], iz[order] + + height_cells = int(np.ceil(robot_height / voxel_size)) + + next_same_col = np.zeros(len(sx), dtype=bool) + next_same_col[:-1] = (sx[:-1] == sx[1:]) & (sy[:-1] == sy[1:]) + + gap = np.empty(len(sx), dtype=np.int64) + gap[:-1] = sz[1:] - sz[:-1] + gap[-1] = 0 + + is_surface = (~next_same_col) | (gap > height_cells) + + surf_ix = sx[is_surface] + surf_iy = sy[is_surface] + surf_iz = sz[is_surface] + + x = (surf_ix.astype(np.float32) + 0.5) * voxel_size + y = (surf_iy.astype(np.float32) + 0.5) * voxel_size + z = (surf_iz.astype(np.float32) + 1.0) * voxel_size + return np.column_stack([x, y, z]).astype(np.float32) class MLSPlanner(Module): @@ -51,8 +93,19 @@ def __init__(self, **kwargs: Any) -> None: super().__init__(**kwargs) self._latest_start: Odometry | None = None - async def handle_global_map(self, _msg: PointCloud2) -> None: - return + async def handle_global_map(self, msg: PointCloud2) -> None: + points, _ = msg.as_numpy() + if points is None or len(points) == 0: + return + surface_points = _extract_surfaces(points, self.config.voxel_size, self.config.robot_height) + logger.info("Surfaces extracted", count=len(surface_points)) + self.surface_map.publish( + PointCloud2.from_numpy( + surface_points, + frame_id=self.config.world_frame, + timestamp=time.time(), + ) + ) async def handle_start_pose(self, msg: Odometry) -> None: self._latest_start = msg From d9d6c5b92f1f531cca2244d7efda75993674115c Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Fri, 22 May 2026 16:04:34 -0700 Subject: [PATCH 07/55] Dilation and erosion --- .../modules/mls_planner/mls_planner.py | 52 +++++++++++++++++++ 1 file changed, 52 insertions(+) diff --git a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py index 13a5850a3c..72a48426ae 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py +++ b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py @@ -26,6 +26,7 @@ from typing import Any import numpy as np +from scipy import ndimage from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out @@ -36,6 +37,9 @@ logger = setup_logger() +SURFACE_DILATION_PASSES = 3 +SURFACE_EROSION_PASSES = 3 + class MLSPlannerConfig(ModuleConfig): world_frame: str = "map" @@ -74,12 +78,60 @@ def _extract_surfaces(points: np.ndarray, voxel_size: float, robot_height: float surf_iy = sy[is_surface] surf_iz = sz[is_surface] + surf_ix, surf_iy, surf_iz = _close_surface_holes( + surf_ix, surf_iy, surf_iz, SURFACE_DILATION_PASSES, SURFACE_EROSION_PASSES + ) + x = (surf_ix.astype(np.float32) + 0.5) * voxel_size y = (surf_iy.astype(np.float32) + 0.5) * voxel_size z = (surf_iz.astype(np.float32) + 1.0) * voxel_size return np.column_stack([x, y, z]).astype(np.float32) +def _close_surface_holes( + surf_ix: np.ndarray, + surf_iy: np.ndarray, + surf_iz: np.ndarray, + dilation_passes: int, + erosion_passes: int, +) -> tuple[np.ndarray, np.ndarray, np.ndarray]: + """Do dilation then erosion on the surface map at each z level. + + Closes a lot of small holes that are artifacts missing lidar points. + """ + if len(surf_ix) == 0 or (dilation_passes <= 0 and erosion_passes <= 0): + return surf_ix, surf_iy, surf_iz + + pad = max(dilation_passes, 0) + new_ix: list[np.ndarray] = [] + new_iy: list[np.ndarray] = [] + new_iz: list[np.ndarray] = [] + for level_iz in np.unique(surf_iz): + sel = surf_iz == level_iz + lx = surf_ix[sel] + ly = surf_iy[sel] + x0, x1 = int(lx.min()), int(lx.max()) + y0, y1 = int(ly.min()), int(ly.max()) + w = x1 - x0 + 1 + 2 * pad + h = y1 - y0 + 1 + 2 * pad + mask = np.zeros((h, w), dtype=bool) + mask[ly - y0 + pad, lx - x0 + pad] = True + if dilation_passes > 0: + mask = ndimage.binary_dilation(mask, iterations=dilation_passes) + if erosion_passes > 0: + mask = ndimage.binary_erosion(mask, iterations=erosion_passes) + ys, xs = np.where(mask) + new_ix.append(xs.astype(np.int64) + x0 - pad) + new_iy.append(ys.astype(np.int64) + y0 - pad) + new_iz.append(np.full(len(xs), level_iz, dtype=np.int64)) + + return ( + np.concatenate(new_ix), + np.concatenate(new_iy), + np.concatenate(new_iz), + ) + + class MLSPlanner(Module): config: MLSPlannerConfig From 662bf5526b06a8ca39cb4196531002edea64b3de Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Sat, 23 May 2026 11:58:40 -0700 Subject: [PATCH 08/55] Way point graph --- dimos/navigation/nav_stack/evaluator/main.py | 23 +- .../nav_stack/evaluator/scenarios.py | 14 +- .../modules/mls_planner/mls_planner.py | 343 +++++++++++++++++- 3 files changed, 368 insertions(+), 12 deletions(-) diff --git a/dimos/navigation/nav_stack/evaluator/main.py b/dimos/navigation/nav_stack/evaluator/main.py index 55832a3575..c0412815f6 100644 --- a/dimos/navigation/nav_stack/evaluator/main.py +++ b/dimos/navigation/nav_stack/evaluator/main.py @@ -58,7 +58,26 @@ def _render_global_map(msg: Any) -> Any: def _render_surface_map(msg: Any) -> Any: - return msg.to_rerun(mode="boxes", size=0.1, colors=[128, 0, 128]) # purple + return msg.to_rerun(mode="spheres", voxel_size=0.05, colors=[128, 0, 128]) # purple + + +# raise the path and way points out of the surface +_GRAPH_Z_LIFT = 0.1 + + +def _render_waypoints(msg: Any) -> Any: + import rerun as rr + + pts, _ = msg.as_numpy() + if pts is None or len(pts) == 0: + return rr.Points3D([]) + pts = pts.copy() + pts[:, 2] += _GRAPH_Z_LIFT + return rr.Points3D(positions=pts, colors=[[75, 156, 211]], radii=[0.15]) # Carolina Blue + + +def _render_waypoint_edges(msg: Any) -> Any: + return msg.to_rerun(z_offset=_GRAPH_Z_LIFT, radii=0.04) def create_evaluator_blueprint() -> Blueprint: @@ -71,6 +90,8 @@ def create_evaluator_blueprint() -> Blueprint: "world/goal_pose": _render_goal_pose, "world/global_map": _render_global_map, "world/surface_map": _render_surface_map, + "world/waypoints": _render_waypoints, + "world/waypoint_edges": _render_waypoint_edges, } ), ) diff --git a/dimos/navigation/nav_stack/evaluator/scenarios.py b/dimos/navigation/nav_stack/evaluator/scenarios.py index f5b9a03e20..e6d243bda0 100644 --- a/dimos/navigation/nav_stack/evaluator/scenarios.py +++ b/dimos/navigation/nav_stack/evaluator/scenarios.py @@ -102,11 +102,11 @@ def default_scenarios() -> list[PlannerScenario]: goal_pose=_odom(21.95, -4.25, 1.75), expect_path=True, ), - PlannerScenario( - name="up the stairs", - global_map=cloud, - start_pose=_odom(7.15, -3.55, 2.05), - goal_pose=_odom(5.55, -2.05, 5.65), - expect_path=True, - ), + # PlannerScenario( + # name="up the stairs", + # global_map=cloud, + # start_pose=_odom(7.15, -3.55, 2.05), + # goal_pose=_odom(5.55, -2.05, 5.65), + # expect_path=True, + # ), ] diff --git a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py index 72a48426ae..2b3a310add 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py +++ b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py @@ -22,16 +22,28 @@ from __future__ import annotations +import heapq +import math import time from typing import Any +from dimos_lcm.geometry_msgs import ( + Point as LCMPoint, + Pose as LCMPose, + PoseStamped as LCMPoseStamped, + Quaternion as LCMQuaternion, +) +from dimos_lcm.nav_msgs import Path as LCMPath +from dimos_lcm.std_msgs import Header as LCMHeader, Time as LCMTime +import networkx as nx import numpy as np from scipy import ndimage from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out +from dimos.msgs.nav_msgs.LineSegments3D import LineSegments3D from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.nav_msgs.Path import Path +from dimos.msgs.nav_msgs.Path import Path, sec_nsec from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.utils.logging_config import setup_logger @@ -40,11 +52,17 @@ SURFACE_DILATION_PASSES = 3 SURFACE_EROSION_PASSES = 3 +WAYPOINT_SPACING_M = 2.0 +WAYPOINT_Z_TOLERANCE_M = 1.0 +WAYPOINT_STEP_THRESHOLD_M = 0.25 +WAYPOINT_MAX_EDGE_COST_M = 3.0 +WAYPOINT_SUB_SAMPLE_STRIDE = 20 + class MLSPlannerConfig(ModuleConfig): world_frame: str = "map" voxel_size: float = 0.1 - robot_height: float = 1.0 + robot_height: float = 1.5 def _extract_surfaces(points: np.ndarray, voxel_size: float, robot_height: float) -> np.ndarray: @@ -132,6 +150,276 @@ def _close_surface_holes( ) +class _GridHash: + """Sparse 2D bucket index over integer cell coordinates.""" + + def __init__(self, bucket_size_cells: int) -> None: + self._bucket_size = max(1, bucket_size_cells) + self._buckets: dict[tuple[int, int], list[int]] = {} + + def _key(self, ix: int, iy: int) -> tuple[int, int]: + return (ix // self._bucket_size, iy // self._bucket_size) + + def add(self, waypoint_id: int, ix: int, iy: int) -> None: + self._buckets.setdefault(self._key(ix, iy), []).append(waypoint_id) + + def nearby(self, ix: int, iy: int, radius_cells: int) -> list[int]: + bucket_radius = radius_cells // self._bucket_size + 1 + bx, by = self._key(ix, iy) + result: list[int] = [] + for dbx in range(-bucket_radius, bucket_radius + 1): + for dby in range(-bucket_radius, bucket_radius + 1): + ids = self._buckets.get((bx + dbx, by + dby)) + if ids: + result.extend(ids) + return result + + +def _build_surface_lookup( + sx: np.ndarray, sy: np.ndarray, sz: np.ndarray +) -> dict[tuple[int, int], np.ndarray]: + """Group surface cells by XY column for fast neighbor lookup in inner A*.""" + by_column: dict[tuple[int, int], list[int]] = {} + for ix_, iy_, iz_ in zip(sx.tolist(), sy.tolist(), sz.tolist(), strict=True): + by_column.setdefault((ix_, iy_), []).append(iz_) + return {key: np.array(sorted(vs), dtype=np.int64) for key, vs in by_column.items()} + + +def _surface_dijkstra( + surface_lookup: dict[tuple[int, int], np.ndarray], + start: tuple[int, int, int], + voxel_size: float, + step_threshold_cells: int, + max_cost: float, + targets: set[tuple[int, int, int]], +) -> tuple[dict[tuple[int, int, int], float], dict[tuple[int, int, int], tuple[int, int, int]]]: + """Single-source Dijkstra over surface cells with per-step delta-z cap. + + Explores until every cell in ``targets`` has been reached, the heap + drains, or every popped cell has g > ``max_cost``. Returns ``(g_score, + came_from)`` so callers can recover both costs and paths for any target + that's in ``g_score``. + """ + heap: list[tuple[float, int, tuple[int, int, int]]] = [(0.0, 0, start)] + g_score: dict[tuple[int, int, int], float] = {start: 0.0} + came_from: dict[tuple[int, int, int], tuple[int, int, int]] = {} + remaining_targets = set(targets) + remaining_targets.discard(start) + counter = 0 + + while heap: + cur_g, _, current = heapq.heappop(heap) + if cur_g > g_score[current]: + continue + remaining_targets.discard(current) + if not remaining_targets: + break + cx, cy, cz = current + for dx in (-1, 0, 1): + for dy in (-1, 0, 1): + if dx == 0 and dy == 0: + continue + nx_, ny_ = cx + dx, cy + dy + surfaces_here = surface_lookup.get((nx_, ny_)) + if surfaces_here is None: + continue + for nz_arr in surfaces_here: + nz = int(nz_arr) + dz = nz - cz + if abs(dz) > step_threshold_cells: + continue + step_cost = math.sqrt(dx * dx + dy * dy + dz * dz) * voxel_size + new_g = cur_g + step_cost + if new_g > max_cost: + continue + neighbor = (nx_, ny_, nz) + if new_g < g_score.get(neighbor, float("inf")): + came_from[neighbor] = current + g_score[neighbor] = new_g + counter += 1 + heapq.heappush(heap, (new_g, counter, neighbor)) + + return g_score, came_from + + +def _reconstruct_path( + came_from: dict[tuple[int, int, int], tuple[int, int, int]], + end: tuple[int, int, int], +) -> np.ndarray: + path = [end] + while end in came_from: + end = came_from[end] + path.append(end) + path.reverse() + return np.array(path, dtype=np.int64) + + +def build_waypoint_graph( + surface_points: np.ndarray, + voxel_size: float, + *, + waypoint_spacing: float, + waypoint_z_tolerance: float, + step_threshold: float, + max_edge_cost: float, + sub_sample_stride: int, +) -> nx.Graph: + """Build a sparse waypoint graph over the surface map. + + Iterates surface cells in deterministic lexicographic order, sub-sampled + by ``sub_sample_stride``. A waypoint is added when no existing waypoint + sits within a cylinder of XY radius ``waypoint_spacing`` and half-height + ``waypoint_z_tolerance``. Each new waypoint is connected to any existing + waypoint that the modified A* can reach within ``max_edge_cost`` subject + to the per-step delta-z cap. + """ + graph = nx.Graph() + if len(surface_points) == 0: + return graph + + sx = np.floor(surface_points[:, 0] / voxel_size).astype(np.int64) + sy = np.floor(surface_points[:, 1] / voxel_size).astype(np.int64) + sz = np.floor(surface_points[:, 2] / voxel_size).astype(np.int64) + + surface_lookup = _build_surface_lookup(sx, sy, sz) + + spacing_cells = max(1, int(waypoint_spacing / voxel_size)) + z_tol_cells = max(0, int(waypoint_z_tolerance / voxel_size)) + step_cells = max(0, int(step_threshold / voxel_size)) + edge_radius_cells = max(1, int(max_edge_cost / voxel_size)) + + grid = _GridHash(spacing_cells) + wp_ix: list[int] = [] + wp_iy: list[int] = [] + wp_iz: list[int] = [] + + order = np.lexsort((sz, sy, sx)) + spacing_sq = spacing_cells * spacing_cells + stride = max(1, sub_sample_stride) + + for idx in order[::stride]: + cix, ciy, ciz = int(sx[idx]), int(sy[idx]), int(sz[idx]) + + in_cylinder = False + for wid in grid.nearby(cix, ciy, spacing_cells): + dx = wp_ix[wid] - cix + dy = wp_iy[wid] - ciy + dz = wp_iz[wid] - ciz + if dx * dx + dy * dy < spacing_sq and abs(dz) < z_tol_cells: + in_cylinder = True + break + if in_cylinder: + continue + + new_id = len(wp_ix) + wp_ix.append(cix) + wp_iy.append(ciy) + wp_iz.append(ciz) + grid.add(new_id, cix, ciy) + graph.add_node( + new_id, + pos=( + (cix + 0.5) * voxel_size, + (ciy + 0.5) * voxel_size, + ciz * voxel_size, + ), + cell=(cix, ciy, ciz), + ) + + candidate_ids = [c for c in grid.nearby(cix, ciy, edge_radius_cells) if c != new_id] + candidate_cells: dict[tuple[int, int, int], int] = {} + for c in candidate_ids: + ox, oy, oz = wp_ix[c], wp_iy[c], wp_iz[c] + dx, dy, dz = ox - cix, oy - ciy, oz - ciz + if math.sqrt(dx * dx + dy * dy + dz * dz) * voxel_size > max_edge_cost: + continue + candidate_cells[(ox, oy, oz)] = c + if not candidate_cells: + continue + + g_score, came_from = _surface_dijkstra( + surface_lookup, + start=(cix, ciy, ciz), + voxel_size=voxel_size, + step_threshold_cells=step_cells, + max_cost=max_edge_cost, + targets=set(candidate_cells), + ) + for cell, other_id in candidate_cells.items(): + if cell in g_score: + graph.add_edge( + new_id, + other_id, + weight=float(g_score[cell]), + path=_reconstruct_path(came_from, cell), + ) + + return graph + + +class _PublishableLineSegments3D(LineSegments3D): + """LineSegments3D with a Python lcm_encode that matches the C++ wire format. + + Upstream only implements decode (encode raises NotImplementedError); this + subclass produces the same nav_msgs/Path wire layout, where consecutive + pose pairs are interpreted as segments and pose.orientation.w carries + traversability. + """ + + def lcm_encode(self) -> bytes: + lcm_msg = LCMPath() + sec, nsec = sec_nsec(self.ts) + lcm_poses = [] + for (p1, p2), trav in zip(self._segments, self._traversability, strict=False): + for pt in (p1, p2): + lp = LCMPoseStamped() + lp.pose = LCMPose() + lp.pose.position = LCMPoint() + lp.pose.orientation = LCMQuaternion() + lp.pose.position.x = pt[0] + lp.pose.position.y = pt[1] + lp.pose.position.z = pt[2] + lp.pose.orientation.w = trav + lp.header = LCMHeader() + lp.header.stamp = LCMTime() + lp.header.stamp.sec = sec + lp.header.stamp.nsec = nsec + lp.header.frame_id = self.frame_id + lcm_poses.append(lp) + lcm_msg.poses_length = len(lcm_poses) + lcm_msg.poses = lcm_poses + lcm_msg.header.stamp.sec = sec + lcm_msg.header.stamp.nsec = nsec + lcm_msg.header.frame_id = self.frame_id + return lcm_msg.lcm_encode() # type: ignore[no-any-return] + + +def _waypoints_to_cloud(graph: nx.Graph) -> np.ndarray: + if graph.number_of_nodes() == 0: + return np.zeros((0, 3), dtype=np.float32) + return np.array([graph.nodes[n]["pos"] for n in graph.nodes()], dtype=np.float32) + + +def _edges_to_segments( + graph: nx.Graph, voxel_size: float +) -> list[tuple[tuple[float, float, float], tuple[float, float, float]]]: + """Walk each edge's cached A* path and emit consecutive cell pairs as segments.""" + segments: list[tuple[tuple[float, float, float], tuple[float, float, float]]] = [] + for _, _, data in graph.edges(data=True): + path_cells: np.ndarray = data["path"] + for i in range(len(path_cells) - 1): + a = path_cells[i] + b = path_cells[i + 1] + ax = (float(a[0]) + 0.5) * voxel_size + ay = (float(a[1]) + 0.5) * voxel_size + az = float(a[2]) * voxel_size + bx = (float(b[0]) + 0.5) * voxel_size + by = (float(b[1]) + 0.5) * voxel_size + bz = float(b[2]) * voxel_size + segments.append(((ax, ay, az), (bx, by, bz))) + return segments + + class MLSPlanner(Module): config: MLSPlannerConfig @@ -140,24 +428,71 @@ class MLSPlanner(Module): goal_pose: In[Odometry] path: Out[Path] surface_map: Out[PointCloud2] + waypoints: Out[PointCloud2] + waypoint_edges: Out[LineSegments3D] def __init__(self, **kwargs: Any) -> None: super().__init__(**kwargs) self._latest_start: Odometry | None = None + self._graph: nx.Graph | None = None async def handle_global_map(self, msg: PointCloud2) -> None: points, _ = msg.as_numpy() if points is None or len(points) == 0: return + + t0 = time.perf_counter() surface_points = _extract_surfaces(points, self.config.voxel_size, self.config.robot_height) - logger.info("Surfaces extracted", count=len(surface_points)) + surfaces_ms = (time.perf_counter() - t0) * 1000 self.surface_map.publish( PointCloud2.from_numpy( - surface_points, + surface_points, frame_id=self.config.world_frame, timestamp=time.time() + ) + ) + logger.info( + "Surfaces ready", + surfaces=len(surface_points), + surface_ms=round(surfaces_ms, 1), + ) + + logger.info( + "Building waypoint graph", + spacing_m=WAYPOINT_SPACING_M, + max_edge_cost_m=WAYPOINT_MAX_EDGE_COST_M, + stride=WAYPOINT_SUB_SAMPLE_STRIDE, + ) + t1 = time.perf_counter() + graph = build_waypoint_graph( + surface_points, + self.config.voxel_size, + waypoint_spacing=WAYPOINT_SPACING_M, + waypoint_z_tolerance=WAYPOINT_Z_TOLERANCE_M, + step_threshold=WAYPOINT_STEP_THRESHOLD_M, + max_edge_cost=WAYPOINT_MAX_EDGE_COST_M, + sub_sample_stride=WAYPOINT_SUB_SAMPLE_STRIDE, + ) + graph_ms = (time.perf_counter() - t1) * 1000 + self._graph = graph + self.waypoints.publish( + PointCloud2.from_numpy( + _waypoints_to_cloud(graph), frame_id=self.config.world_frame, timestamp=time.time(), ) ) + logger.info( + "Waypoint graph done", + waypoints=graph.number_of_nodes(), + edges=graph.number_of_edges(), + graph_ms=round(graph_ms, 1), + ) + self.waypoint_edges.publish( + _PublishableLineSegments3D( + ts=time.time(), + frame_id=self.config.world_frame, + segments=_edges_to_segments(graph, self.config.voxel_size), + ) + ) async def handle_start_pose(self, msg: Odometry) -> None: self._latest_start = msg From 39cbf69c4e1510acba6d097d4604fd72e743044e Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Mon, 25 May 2026 11:04:01 -0700 Subject: [PATCH 09/55] Rename to nodes --- dimos/navigation/nav_stack/evaluator/main.py | 8 +- .../modules/mls_planner/mls_planner.py | 96 +++++++++---------- 2 files changed, 52 insertions(+), 52 deletions(-) diff --git a/dimos/navigation/nav_stack/evaluator/main.py b/dimos/navigation/nav_stack/evaluator/main.py index c0412815f6..b89eae827e 100644 --- a/dimos/navigation/nav_stack/evaluator/main.py +++ b/dimos/navigation/nav_stack/evaluator/main.py @@ -65,7 +65,7 @@ def _render_surface_map(msg: Any) -> Any: _GRAPH_Z_LIFT = 0.1 -def _render_waypoints(msg: Any) -> Any: +def _render_nodes(msg: Any) -> Any: import rerun as rr pts, _ = msg.as_numpy() @@ -76,7 +76,7 @@ def _render_waypoints(msg: Any) -> Any: return rr.Points3D(positions=pts, colors=[[75, 156, 211]], radii=[0.15]) # Carolina Blue -def _render_waypoint_edges(msg: Any) -> Any: +def _render_node_edges(msg: Any) -> Any: return msg.to_rerun(z_offset=_GRAPH_Z_LIFT, radii=0.04) @@ -90,8 +90,8 @@ def create_evaluator_blueprint() -> Blueprint: "world/goal_pose": _render_goal_pose, "world/global_map": _render_global_map, "world/surface_map": _render_surface_map, - "world/waypoints": _render_waypoints, - "world/waypoint_edges": _render_waypoint_edges, + "world/nodes": _render_nodes, + "world/node_edges": _render_node_edges, } ), ) diff --git a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py index 2b3a310add..a3c4b4b32f 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py +++ b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py @@ -15,7 +15,7 @@ """Multi-Level Surface (MLS) path planner. Extracts walkable surfaces from a voxelized global map, builds a sparse -waypoint graph over those surfaces, and plans paths via local A* plus +node graph over those surfaces, and plans paths via local A* plus shortest-path search on the graph. Skeleton — algorithm is filled in piecewise. """ @@ -52,11 +52,11 @@ SURFACE_DILATION_PASSES = 3 SURFACE_EROSION_PASSES = 3 -WAYPOINT_SPACING_M = 2.0 -WAYPOINT_Z_TOLERANCE_M = 1.0 -WAYPOINT_STEP_THRESHOLD_M = 0.25 -WAYPOINT_MAX_EDGE_COST_M = 3.0 -WAYPOINT_SUB_SAMPLE_STRIDE = 20 +NODE_SPACING_M = 2.0 +NODE_Z_TOLERANCE_M = 1.0 +NODE_STEP_THRESHOLD_M = 0.25 +NODE_MAX_EDGE_COST_M = 3.0 +NODE_SUB_SAMPLE_STRIDE = 20 class MLSPlannerConfig(ModuleConfig): @@ -160,8 +160,8 @@ def __init__(self, bucket_size_cells: int) -> None: def _key(self, ix: int, iy: int) -> tuple[int, int]: return (ix // self._bucket_size, iy // self._bucket_size) - def add(self, waypoint_id: int, ix: int, iy: int) -> None: - self._buckets.setdefault(self._key(ix, iy), []).append(waypoint_id) + def add(self, node_id: int, ix: int, iy: int) -> None: + self._buckets.setdefault(self._key(ix, iy), []).append(node_id) def nearby(self, ix: int, iy: int, radius_cells: int) -> list[int]: bucket_radius = radius_cells // self._bucket_size + 1 @@ -254,23 +254,23 @@ def _reconstruct_path( return np.array(path, dtype=np.int64) -def build_waypoint_graph( +def build_node_graph( surface_points: np.ndarray, voxel_size: float, *, - waypoint_spacing: float, - waypoint_z_tolerance: float, + node_spacing: float, + node_z_tolerance: float, step_threshold: float, max_edge_cost: float, sub_sample_stride: int, ) -> nx.Graph: - """Build a sparse waypoint graph over the surface map. + """Build a sparse node graph over the surface map. Iterates surface cells in deterministic lexicographic order, sub-sampled - by ``sub_sample_stride``. A waypoint is added when no existing waypoint - sits within a cylinder of XY radius ``waypoint_spacing`` and half-height - ``waypoint_z_tolerance``. Each new waypoint is connected to any existing - waypoint that the modified A* can reach within ``max_edge_cost`` subject + by ``sub_sample_stride``. A node is added when no existing node + sits within a cylinder of XY radius ``node_spacing`` and half-height + ``node_z_tolerance``. Each new node is connected to any existing + node that the modified A* can reach within ``max_edge_cost`` subject to the per-step delta-z cap. """ graph = nx.Graph() @@ -283,15 +283,15 @@ def build_waypoint_graph( surface_lookup = _build_surface_lookup(sx, sy, sz) - spacing_cells = max(1, int(waypoint_spacing / voxel_size)) - z_tol_cells = max(0, int(waypoint_z_tolerance / voxel_size)) + spacing_cells = max(1, int(node_spacing / voxel_size)) + z_tol_cells = max(0, int(node_z_tolerance / voxel_size)) step_cells = max(0, int(step_threshold / voxel_size)) edge_radius_cells = max(1, int(max_edge_cost / voxel_size)) grid = _GridHash(spacing_cells) - wp_ix: list[int] = [] - wp_iy: list[int] = [] - wp_iz: list[int] = [] + node_ix: list[int] = [] + node_iy: list[int] = [] + node_iz: list[int] = [] order = np.lexsort((sz, sy, sx)) spacing_sq = spacing_cells * spacing_cells @@ -301,20 +301,20 @@ def build_waypoint_graph( cix, ciy, ciz = int(sx[idx]), int(sy[idx]), int(sz[idx]) in_cylinder = False - for wid in grid.nearby(cix, ciy, spacing_cells): - dx = wp_ix[wid] - cix - dy = wp_iy[wid] - ciy - dz = wp_iz[wid] - ciz + for nid in grid.nearby(cix, ciy, spacing_cells): + dx = node_ix[nid] - cix + dy = node_iy[nid] - ciy + dz = node_iz[nid] - ciz if dx * dx + dy * dy < spacing_sq and abs(dz) < z_tol_cells: in_cylinder = True break if in_cylinder: continue - new_id = len(wp_ix) - wp_ix.append(cix) - wp_iy.append(ciy) - wp_iz.append(ciz) + new_id = len(node_ix) + node_ix.append(cix) + node_iy.append(ciy) + node_iz.append(ciz) grid.add(new_id, cix, ciy) graph.add_node( new_id, @@ -329,7 +329,7 @@ def build_waypoint_graph( candidate_ids = [c for c in grid.nearby(cix, ciy, edge_radius_cells) if c != new_id] candidate_cells: dict[tuple[int, int, int], int] = {} for c in candidate_ids: - ox, oy, oz = wp_ix[c], wp_iy[c], wp_iz[c] + ox, oy, oz = node_ix[c], node_iy[c], node_iz[c] dx, dy, dz = ox - cix, oy - ciy, oz - ciz if math.sqrt(dx * dx + dy * dy + dz * dz) * voxel_size > max_edge_cost: continue @@ -394,7 +394,7 @@ def lcm_encode(self) -> bytes: return lcm_msg.lcm_encode() # type: ignore[no-any-return] -def _waypoints_to_cloud(graph: nx.Graph) -> np.ndarray: +def _nodes_to_cloud(graph: nx.Graph) -> np.ndarray: if graph.number_of_nodes() == 0: return np.zeros((0, 3), dtype=np.float32) return np.array([graph.nodes[n]["pos"] for n in graph.nodes()], dtype=np.float32) @@ -428,8 +428,8 @@ class MLSPlanner(Module): goal_pose: In[Odometry] path: Out[Path] surface_map: Out[PointCloud2] - waypoints: Out[PointCloud2] - waypoint_edges: Out[LineSegments3D] + nodes: Out[PointCloud2] + node_edges: Out[LineSegments3D] def __init__(self, **kwargs: Any) -> None: super().__init__(**kwargs) @@ -456,37 +456,37 @@ async def handle_global_map(self, msg: PointCloud2) -> None: ) logger.info( - "Building waypoint graph", - spacing_m=WAYPOINT_SPACING_M, - max_edge_cost_m=WAYPOINT_MAX_EDGE_COST_M, - stride=WAYPOINT_SUB_SAMPLE_STRIDE, + "Building node graph", + spacing_m=NODE_SPACING_M, + max_edge_cost_m=NODE_MAX_EDGE_COST_M, + stride=NODE_SUB_SAMPLE_STRIDE, ) t1 = time.perf_counter() - graph = build_waypoint_graph( + graph = build_node_graph( surface_points, self.config.voxel_size, - waypoint_spacing=WAYPOINT_SPACING_M, - waypoint_z_tolerance=WAYPOINT_Z_TOLERANCE_M, - step_threshold=WAYPOINT_STEP_THRESHOLD_M, - max_edge_cost=WAYPOINT_MAX_EDGE_COST_M, - sub_sample_stride=WAYPOINT_SUB_SAMPLE_STRIDE, + node_spacing=NODE_SPACING_M, + node_z_tolerance=NODE_Z_TOLERANCE_M, + step_threshold=NODE_STEP_THRESHOLD_M, + max_edge_cost=NODE_MAX_EDGE_COST_M, + sub_sample_stride=NODE_SUB_SAMPLE_STRIDE, ) graph_ms = (time.perf_counter() - t1) * 1000 self._graph = graph - self.waypoints.publish( + self.nodes.publish( PointCloud2.from_numpy( - _waypoints_to_cloud(graph), + _nodes_to_cloud(graph), frame_id=self.config.world_frame, timestamp=time.time(), ) ) logger.info( - "Waypoint graph done", - waypoints=graph.number_of_nodes(), + "Node graph done", + nodes=graph.number_of_nodes(), edges=graph.number_of_edges(), graph_ms=round(graph_ms, 1), ) - self.waypoint_edges.publish( + self.node_edges.publish( _PublishableLineSegments3D( ts=time.time(), frame_id=self.config.world_frame, From 7014f79e40e4bd120c3c14fc9234e93e294754f2 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Mon, 25 May 2026 11:24:26 -0700 Subject: [PATCH 10/55] Refactor into node place and edge add --- .../modules/mls_planner/mls_planner.py | 101 ++++++++++++------ 1 file changed, 70 insertions(+), 31 deletions(-) diff --git a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py index a3c4b4b32f..2d747bf4d7 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py +++ b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py @@ -254,28 +254,25 @@ def _reconstruct_path( return np.array(path, dtype=np.int64) -def build_node_graph( +def place_nodes( surface_points: np.ndarray, voxel_size: float, *, node_spacing: float, node_z_tolerance: float, - step_threshold: float, - max_edge_cost: float, sub_sample_stride: int, -) -> nx.Graph: - """Build a sparse node graph over the surface map. - - Iterates surface cells in deterministic lexicographic order, sub-sampled - by ``sub_sample_stride``. A node is added when no existing node - sits within a cylinder of XY radius ``node_spacing`` and half-height - ``node_z_tolerance``. Each new node is connected to any existing - node that the modified A* can reach within ``max_edge_cost`` subject - to the per-step delta-z cap. +) -> tuple[nx.Graph, dict[tuple[int, int], np.ndarray]]: + """Place sparse nodes over the surface map; returns ``(graph, surface_lookup)``. + + Strides through surface cells in lex order and adds a node whenever no + existing node sits within a cylinder of XY radius ``node_spacing`` and + half-height ``node_z_tolerance``. Edges are added separately by + ``add_node_edges`` so callers can publish the node cloud as soon as + placement returns. """ graph = nx.Graph() if len(surface_points) == 0: - return graph + return graph, {} sx = np.floor(surface_points[:, 0] / voxel_size).astype(np.int64) sy = np.floor(surface_points[:, 1] / voxel_size).astype(np.int64) @@ -285,8 +282,6 @@ def build_node_graph( spacing_cells = max(1, int(node_spacing / voxel_size)) z_tol_cells = max(0, int(node_z_tolerance / voxel_size)) - step_cells = max(0, int(step_threshold / voxel_size)) - edge_radius_cells = max(1, int(max_edge_cost / voxel_size)) grid = _GridHash(spacing_cells) node_ix: list[int] = [] @@ -326,14 +321,48 @@ def build_node_graph( cell=(cix, ciy, ciz), ) - candidate_ids = [c for c in grid.nearby(cix, ciy, edge_radius_cells) if c != new_id] + return graph, surface_lookup + + +def add_node_edges( + graph: nx.Graph, + surface_lookup: dict[tuple[int, int], np.ndarray], + voxel_size: float, + *, + step_threshold: float, + max_edge_cost: float, +) -> None: + """Connect each node to nearby nodes the surface Dijkstra can reach. + + For every node, runs a multi-target Dijkstra over surface cells against + any higher-id node whose cell sits within ``max_edge_cost`` (euclidean) + and adds an edge with the cached A* path whenever the constrained + shortest-path cost is also within ``max_edge_cost``. + """ + if graph.number_of_nodes() == 0: + return + + step_cells = max(0, int(step_threshold / voxel_size)) + edge_radius_cells = max(1, int(max_edge_cost / voxel_size)) + + grid = _GridHash(edge_radius_cells) + cells: dict[int, tuple[int, int, int]] = {} + for node_id, data in graph.nodes(data=True): + cix, ciy, ciz = data["cell"] + cells[node_id] = (cix, ciy, ciz) + grid.add(node_id, cix, ciy) + + for node_id in sorted(graph.nodes()): + cix, ciy, ciz = cells[node_id] candidate_cells: dict[tuple[int, int, int], int] = {} - for c in candidate_ids: - ox, oy, oz = node_ix[c], node_iy[c], node_iz[c] + for other_id in grid.nearby(cix, ciy, edge_radius_cells): + if other_id <= node_id: + continue + ox, oy, oz = cells[other_id] dx, dy, dz = ox - cix, oy - ciy, oz - ciz if math.sqrt(dx * dx + dy * dy + dz * dz) * voxel_size > max_edge_cost: continue - candidate_cells[(ox, oy, oz)] = c + candidate_cells[(ox, oy, oz)] = other_id if not candidate_cells: continue @@ -348,14 +377,12 @@ def build_node_graph( for cell, other_id in candidate_cells.items(): if cell in g_score: graph.add_edge( - new_id, + node_id, other_id, weight=float(g_score[cell]), path=_reconstruct_path(came_from, cell), ) - return graph - class _PublishableLineSegments3D(LineSegments3D): """LineSegments3D with a Python lcm_encode that matches the C++ wire format. @@ -456,23 +483,19 @@ async def handle_global_map(self, msg: PointCloud2) -> None: ) logger.info( - "Building node graph", + "Placing nodes", spacing_m=NODE_SPACING_M, - max_edge_cost_m=NODE_MAX_EDGE_COST_M, stride=NODE_SUB_SAMPLE_STRIDE, ) t1 = time.perf_counter() - graph = build_node_graph( + graph, surface_lookup = place_nodes( surface_points, self.config.voxel_size, node_spacing=NODE_SPACING_M, node_z_tolerance=NODE_Z_TOLERANCE_M, - step_threshold=NODE_STEP_THRESHOLD_M, - max_edge_cost=NODE_MAX_EDGE_COST_M, sub_sample_stride=NODE_SUB_SAMPLE_STRIDE, ) - graph_ms = (time.perf_counter() - t1) * 1000 - self._graph = graph + place_ms = (time.perf_counter() - t1) * 1000 self.nodes.publish( PointCloud2.from_numpy( _nodes_to_cloud(graph), @@ -481,10 +504,26 @@ async def handle_global_map(self, msg: PointCloud2) -> None: ) ) logger.info( - "Node graph done", + "Nodes placed", nodes=graph.number_of_nodes(), + place_ms=round(place_ms, 1), + ) + + logger.info("Building edges", max_edge_cost_m=NODE_MAX_EDGE_COST_M) + t2 = time.perf_counter() + add_node_edges( + graph, + surface_lookup, + self.config.voxel_size, + step_threshold=NODE_STEP_THRESHOLD_M, + max_edge_cost=NODE_MAX_EDGE_COST_M, + ) + edges_ms = (time.perf_counter() - t2) * 1000 + self._graph = graph + logger.info( + "Edges built", edges=graph.number_of_edges(), - graph_ms=round(graph_ms, 1), + edges_ms=round(edges_ms, 1), ) self.node_edges.publish( _PublishableLineSegments3D( From 6bbf2946b1785df69f438f99a027a1c085f41b78 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Mon, 25 May 2026 11:59:43 -0700 Subject: [PATCH 11/55] Perf improvements --- .../modules/mls_planner/mls_planner.py | 202 +++++++++++------- 1 file changed, 122 insertions(+), 80 deletions(-) diff --git a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py index 2d747bf4d7..adf79758b4 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py +++ b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py @@ -22,7 +22,6 @@ from __future__ import annotations -import heapq import math import time from typing import Any @@ -38,6 +37,8 @@ import networkx as nx import numpy as np from scipy import ndimage +from scipy.sparse import csr_matrix +from scipy.sparse.csgraph import dijkstra from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out @@ -185,73 +186,105 @@ def _build_surface_lookup( return {key: np.array(sorted(vs), dtype=np.int64) for key, vs in by_column.items()} -def _surface_dijkstra( +def _build_surface_adjacency( surface_lookup: dict[tuple[int, int], np.ndarray], - start: tuple[int, int, int], voxel_size: float, step_threshold_cells: int, - max_cost: float, - targets: set[tuple[int, int, int]], -) -> tuple[dict[tuple[int, int, int], float], dict[tuple[int, int, int], tuple[int, int, int]]]: - """Single-source Dijkstra over surface cells with per-step delta-z cap. - - Explores until every cell in ``targets`` has been reached, the heap - drains, or every popped cell has g > ``max_cost``. Returns ``(g_score, - came_from)`` so callers can recover both costs and paths for any target - that's in ``g_score``. +) -> tuple[csr_matrix, dict[tuple[int, int, int], int], list[tuple[int, int, int]]]: + """Build a sparse CSR adjacency over surface cells for ``scipy.csgraph.dijkstra``. + + Each surface cell becomes a row index. Edges connect 8-XY-adjacent cells + whose ``iz`` differs by at most ``step_threshold_cells``, with weight + equal to the 3D step distance in metres. Returns ``(adj, cell_to_idx, + idx_to_cell)``. + + Fully vectorized over surface cells: for each of the eight ``(dx, dy)`` + offsets, ``np.searchsorted`` finds the range of cells in each source's + neighbor column at once, and the ``|dz|`` cap is applied as a numpy + mask. """ - heap: list[tuple[float, int, tuple[int, int, int]]] = [(0.0, 0, start)] - g_score: dict[tuple[int, int, int], float] = {start: 0.0} - came_from: dict[tuple[int, int, int], tuple[int, int, int]] = {} - remaining_targets = set(targets) - remaining_targets.discard(start) - counter = 0 - - while heap: - cur_g, _, current = heapq.heappop(heap) - if cur_g > g_score[current]: + n = sum(len(zs) for zs in surface_lookup.values()) + if n == 0: + return csr_matrix((0, 0), dtype=np.float64), {}, [] + + ix = np.empty(n, dtype=np.int64) + iy = np.empty(n, dtype=np.int64) + iz = np.empty(n, dtype=np.int64) + cursor = 0 + for (ix_col, iy_col), zs in surface_lookup.items(): + k = len(zs) + ix[cursor : cursor + k] = int(ix_col) + iy[cursor : cursor + k] = int(iy_col) + iz[cursor : cursor + k] = zs + cursor += k + + idx_to_cell: list[tuple[int, int, int]] = list( + zip(ix.tolist(), iy.tolist(), iz.tolist(), strict=True) + ) + cell_to_idx: dict[tuple[int, int, int], int] = {cell: i for i, cell in enumerate(idx_to_cell)} + + # Encode (ix, iy) → int64 column key. Padding keeps neighbor keys + # (with dx, dy ∈ {-1, 0, +1}) in non-colliding slots from each other. + ix_pos = ix - ix.min() + 1 + iy_pos = iy - iy.min() + 1 + y_range = int(iy_pos.max()) + 2 + col_key = ix_pos * y_range + iy_pos + + sort_order = np.lexsort((iz, col_key)) + sorted_col_key = col_key[sort_order] + sorted_iz = iz[sort_order] + + row_chunks: list[np.ndarray] = [] + col_chunks: list[np.ndarray] = [] + data_chunks: list[np.ndarray] = [] + for dx, dy in ((-1, -1), (-1, 0), (-1, 1), (0, -1), (0, 1), (1, -1), (1, 0), (1, 1)): + neighbor_key = (ix_pos + dx) * y_range + (iy_pos + dy) + lo = np.searchsorted(sorted_col_key, neighbor_key, side="left") + hi = np.searchsorted(sorted_col_key, neighbor_key, side="right") + n_per_src = hi - lo + total = int(n_per_src.sum()) + if total == 0: continue - remaining_targets.discard(current) - if not remaining_targets: - break - cx, cy, cz = current - for dx in (-1, 0, 1): - for dy in (-1, 0, 1): - if dx == 0 and dy == 0: - continue - nx_, ny_ = cx + dx, cy + dy - surfaces_here = surface_lookup.get((nx_, ny_)) - if surfaces_here is None: - continue - for nz_arr in surfaces_here: - nz = int(nz_arr) - dz = nz - cz - if abs(dz) > step_threshold_cells: - continue - step_cost = math.sqrt(dx * dx + dy * dy + dz * dz) * voxel_size - new_g = cur_g + step_cost - if new_g > max_cost: - continue - neighbor = (nx_, ny_, nz) - if new_g < g_score.get(neighbor, float("inf")): - came_from[neighbor] = current - g_score[neighbor] = new_g - counter += 1 - heapq.heappush(heap, (new_g, counter, neighbor)) - - return g_score, came_from - - -def _reconstruct_path( - came_from: dict[tuple[int, int, int], tuple[int, int, int]], - end: tuple[int, int, int], + src_flat = np.repeat(np.arange(n), n_per_src) + starts = np.zeros(n, dtype=np.int64) + starts[1:] = np.cumsum(n_per_src[:-1]) + candidate_sorted_idx = lo[src_flat] + (np.arange(total) - starts[src_flat]) + dz = sorted_iz[candidate_sorted_idx] - iz[src_flat] + valid = np.abs(dz) <= step_threshold_cells + if not valid.any(): + continue + src_valid = src_flat[valid] + dst_valid = sort_order[candidate_sorted_idx[valid]] + dz_valid = dz[valid] + step_cost = np.sqrt(dx * dx + dy * dy + dz_valid * dz_valid) * voxel_size + row_chunks.append(src_valid) + col_chunks.append(dst_valid) + data_chunks.append(step_cost.astype(np.float64)) + + if not row_chunks: + return csr_matrix((n, n), dtype=np.float64), cell_to_idx, idx_to_cell + + rows = np.concatenate(row_chunks) + cols = np.concatenate(col_chunks) + data = np.concatenate(data_chunks) + return csr_matrix((data, (rows, cols)), shape=(n, n)), cell_to_idx, idx_to_cell + + +def _reconstruct_path_from_predecessors( + predecessors: np.ndarray, + src_idx: int, + tgt_idx: int, + idx_to_cell: list[tuple[int, int, int]], ) -> np.ndarray: - path = [end] - while end in came_from: - end = came_from[end] - path.append(end) - path.reverse() - return np.array(path, dtype=np.int64) + path_indices = [tgt_idx] + cur = tgt_idx + while cur != src_idx: + cur = int(predecessors[cur]) + if cur < 0: + break + path_indices.append(cur) + path_indices.reverse() + return np.array([idx_to_cell[i] for i in path_indices], dtype=np.int64) def place_nodes( @@ -334,10 +367,10 @@ def add_node_edges( ) -> None: """Connect each node to nearby nodes the surface Dijkstra can reach. - For every node, runs a multi-target Dijkstra over surface cells against - any higher-id node whose cell sits within ``max_edge_cost`` (euclidean) - and adds an edge with the cached A* path whenever the constrained - shortest-path cost is also within ``max_edge_cost``. + Builds a sparse adjacency over surface cells once, then runs scipy's + native bounded Dijkstra from each node and looks up the cost to every + higher-id candidate within ``max_edge_cost`` (euclidean). Edges get the + reconstructed cell path stored under ``data["path"]``. """ if graph.number_of_nodes() == 0: return @@ -345,6 +378,8 @@ def add_node_edges( step_cells = max(0, int(step_threshold / voxel_size)) edge_radius_cells = max(1, int(max_edge_cost / voxel_size)) + adj, cell_to_idx, idx_to_cell = _build_surface_adjacency(surface_lookup, voxel_size, step_cells) + grid = _GridHash(edge_radius_cells) cells: dict[int, tuple[int, int, int]] = {} for node_id, data in graph.nodes(data=True): @@ -366,22 +401,29 @@ def add_node_edges( if not candidate_cells: continue - g_score, came_from = _surface_dijkstra( - surface_lookup, - start=(cix, ciy, ciz), - voxel_size=voxel_size, - step_threshold_cells=step_cells, - max_cost=max_edge_cost, - targets=set(candidate_cells), + src_idx = cell_to_idx.get((cix, ciy, ciz)) + if src_idx is None: + continue + + dist, predecessors = dijkstra( + adj, + indices=src_idx, + limit=max_edge_cost, + return_predecessors=True, ) + for cell, other_id in candidate_cells.items(): - if cell in g_score: - graph.add_edge( - node_id, - other_id, - weight=float(g_score[cell]), - path=_reconstruct_path(came_from, cell), - ) + tgt_idx = cell_to_idx.get(cell) + if tgt_idx is None or not math.isfinite(dist[tgt_idx]): + continue + graph.add_edge( + node_id, + other_id, + weight=float(dist[tgt_idx]), + path=_reconstruct_path_from_predecessors( + predecessors, src_idx, tgt_idx, idx_to_cell + ), + ) class _PublishableLineSegments3D(LineSegments3D): From 696e16358daa1c2693c0fb1445fe7e249946d568 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Mon, 25 May 2026 12:20:14 -0700 Subject: [PATCH 12/55] We live in a predetermined simulation --- dimos/navigation/nav_stack/evaluator/mesh_loader.py | 1 + 1 file changed, 1 insertion(+) diff --git a/dimos/navigation/nav_stack/evaluator/mesh_loader.py b/dimos/navigation/nav_stack/evaluator/mesh_loader.py index 2e37c765d4..5f2e5f76bb 100644 --- a/dimos/navigation/nav_stack/evaluator/mesh_loader.py +++ b/dimos/navigation/nav_stack/evaluator/mesh_loader.py @@ -49,6 +49,7 @@ def load_voxelized_mesh( triangles=len(mesh.triangles), ) + o3d.utility.random.seed(42) pcd = mesh.sample_points_uniformly(number_of_points=num_sample_points) points = np.asarray(pcd.points) From b266e1989cf98a958735541d49a5ae68d1d578c8 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Mon, 25 May 2026 12:46:36 -0700 Subject: [PATCH 13/55] Validate connected surface components --- dimos/navigation/nav_stack/evaluator/main.py | 49 +++++++- .../modules/mls_planner/mls_planner.py | 113 +++++++++++++++--- 2 files changed, 145 insertions(+), 17 deletions(-) diff --git a/dimos/navigation/nav_stack/evaluator/main.py b/dimos/navigation/nav_stack/evaluator/main.py index b89eae827e..ff0f3432ae 100644 --- a/dimos/navigation/nav_stack/evaluator/main.py +++ b/dimos/navigation/nav_stack/evaluator/main.py @@ -24,13 +24,37 @@ from typing import Any +import numpy as np +from scipy.sparse.csgraph import connected_components + from dimos.core.coordination.blueprints import Blueprint, autoconnect from dimos.core.coordination.module_coordinator import ModuleCoordinator from dimos.navigation.nav_stack.evaluator.evaluator import Evaluator -from dimos.navigation.nav_stack.modules.mls_planner.mls_planner import MLSPlanner +from dimos.navigation.nav_stack.modules.mls_planner.mls_planner import ( + NODE_STEP_THRESHOLD_M, + MLSPlanner, + _build_surface_adjacency, + _build_surface_lookup, +) from dimos.visualization.rerun.bridge import RerunBridgeModule _POSE_MARKER_RADIUS = 0.4 +_SURFACE_VOXEL = 0.1 +_SURFACE_COMPONENT_PALETTE = np.array( + [ + [220, 50, 50], + [50, 180, 50], + [50, 100, 220], + [240, 180, 50], + [180, 50, 220], + [50, 200, 200], + [240, 130, 50], + [120, 200, 100], + [200, 60, 130], + [80, 80, 200], + ], + dtype=np.uint8, +) def _render_start_pose(msg: Any) -> Any: @@ -54,11 +78,30 @@ def _render_goal_pose(msg: Any) -> Any: def _render_global_map(msg: Any) -> Any: - return msg.to_rerun(voxel_size=0.03) + return msg.to_rerun(voxel_size=0.03, colors=[128, 128, 128]) # gray def _render_surface_map(msg: Any) -> Any: - return msg.to_rerun(mode="spheres", voxel_size=0.05, colors=[128, 0, 128]) # purple + import rerun as rr + + pts, _ = msg.as_numpy() + if pts is None or len(pts) == 0: + return rr.Points3D([]) + indices = np.floor(pts / _SURFACE_VOXEL).astype(np.int64) + ix, iy, iz = indices[:, 0], indices[:, 1], indices[:, 2] + surface_lookup = _build_surface_lookup(ix, iy, iz) + step_cells = max(0, int(NODE_STEP_THRESHOLD_M / _SURFACE_VOXEL)) + adj, cell_to_idx, _ = _build_surface_adjacency(surface_lookup, _SURFACE_VOXEL, step_cells) + _, labels = connected_components(adj, directed=False) + point_labels = np.array( + [ + labels[cell_to_idx[cell]] + for cell in zip(ix.tolist(), iy.tolist(), iz.tolist(), strict=True) + ], + dtype=np.int64, + ) + colors = _SURFACE_COMPONENT_PALETTE[point_labels % len(_SURFACE_COMPONENT_PALETTE)] + return rr.Points3D(positions=pts, colors=colors, radii=[0.05]) # raise the path and way points out of the surface diff --git a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py index adf79758b4..bb63b77698 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py +++ b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py @@ -38,7 +38,7 @@ import numpy as np from scipy import ndimage from scipy.sparse import csr_matrix -from scipy.sparse.csgraph import dijkstra +from scipy.sparse.csgraph import connected_components, dijkstra from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out @@ -56,7 +56,7 @@ NODE_SPACING_M = 2.0 NODE_Z_TOLERANCE_M = 1.0 NODE_STEP_THRESHOLD_M = 0.25 -NODE_MAX_EDGE_COST_M = 3.0 +NODE_MAX_EDGE_COST_M = 2.5 NODE_SUB_SAMPLE_STRIDE = 20 @@ -359,27 +359,25 @@ def place_nodes( def add_node_edges( graph: nx.Graph, - surface_lookup: dict[tuple[int, int], np.ndarray], + adj: csr_matrix, + cell_to_idx: dict[tuple[int, int, int], int], + idx_to_cell: list[tuple[int, int, int]], voxel_size: float, *, - step_threshold: float, max_edge_cost: float, ) -> None: """Connect each node to nearby nodes the surface Dijkstra can reach. - Builds a sparse adjacency over surface cells once, then runs scipy's - native bounded Dijkstra from each node and looks up the cost to every - higher-id candidate within ``max_edge_cost`` (euclidean). Edges get the - reconstructed cell path stored under ``data["path"]``. + For each source node, runs scipy's bounded Dijkstra over the surface + adjacency and reads the cost to every higher-id candidate within + ``max_edge_cost`` (euclidean). Edges get the reconstructed cell path + stored under ``data["path"]``. """ if graph.number_of_nodes() == 0: return - step_cells = max(0, int(step_threshold / voxel_size)) edge_radius_cells = max(1, int(max_edge_cost / voxel_size)) - adj, cell_to_idx, idx_to_cell = _build_surface_adjacency(surface_lookup, voxel_size, step_cells) - grid = _GridHash(edge_radius_cells) cells: dict[int, tuple[int, int, int]] = {} for node_id, data in graph.nodes(data=True): @@ -426,6 +424,77 @@ def add_node_edges( ) +def add_connectivity_bridges( + graph: nx.Graph, + adj: csr_matrix, + cell_to_idx: dict[tuple[int, int, int], int], + idx_to_cell: list[tuple[int, int, int]], +) -> int: + """Bridge graph components that share a surface component but aren't connected. + + Flood-fills the surface adjacency to label every cell with its connected + component, then for each surface component containing more than one + graph component, iteratively connects the closest cell-distance pair of + nodes across two unmerged components with an uncapped surface Dijkstra + edge. Returns the number of bridges added. + """ + if graph.number_of_nodes() == 0: + return 0 + + _, surf_labels = connected_components(adj, directed=False) + + nodes_by_surf_cc: dict[int, list[int]] = {} + for node_id in graph.nodes(): + cell = graph.nodes[node_id]["cell"] + cell_idx = cell_to_idx.get(cell) + if cell_idx is None: + continue + nodes_by_surf_cc.setdefault(int(surf_labels[cell_idx]), []).append(node_id) + + bridges_added = 0 + for node_ids in nodes_by_surf_cc.values(): + components = [list(c) for c in nx.connected_components(graph.subgraph(node_ids))] + while len(components) > 1: + best_cost_sq = math.inf + best_i = best_j = best_ai = best_bi = -1 + for i in range(len(components)): + cells_i = np.array([graph.nodes[n]["cell"] for n in components[i]], dtype=np.int64) + for j in range(i + 1, len(components)): + cells_j = np.array( + [graph.nodes[n]["cell"] for n in components[j]], dtype=np.int64 + ) + diff = cells_i[:, None, :] - cells_j[None, :, :] + dist_sq = (diff * diff).sum(-1) + flat = int(dist_sq.argmin()) + ai, bi = divmod(flat, len(components[j])) + c = int(dist_sq[ai, bi]) + if c < best_cost_sq: + best_cost_sq = c + best_i, best_j, best_ai, best_bi = i, j, ai, bi + node_a = components[best_i][best_ai] + node_b = components[best_j][best_bi] + src_idx = cell_to_idx[graph.nodes[node_a]["cell"]] + tgt_idx = cell_to_idx[graph.nodes[node_b]["cell"]] + dist, predecessors = dijkstra(adj, indices=src_idx, return_predecessors=True) + if math.isfinite(dist[tgt_idx]): + graph.add_edge( + node_a, + node_b, + weight=float(dist[tgt_idx]), + path=_reconstruct_path_from_predecessors( + predecessors, src_idx, tgt_idx, idx_to_cell + ), + ) + bridges_added += 1 + merged = components[best_i] + components[best_j] + components = [ + components[k] for k in range(len(components)) if k not in (best_i, best_j) + ] + components.append(merged) + + return bridges_added + + class _PublishableLineSegments3D(LineSegments3D): """LineSegments3D with a Python lcm_encode that matches the C++ wire format. @@ -551,22 +620,38 @@ async def handle_global_map(self, msg: PointCloud2) -> None: place_ms=round(place_ms, 1), ) + step_cells = max(0, int(NODE_STEP_THRESHOLD_M / self.config.voxel_size)) + adj, cell_to_idx, idx_to_cell = _build_surface_adjacency( + surface_lookup, self.config.voxel_size, step_cells + ) + logger.info("Building edges", max_edge_cost_m=NODE_MAX_EDGE_COST_M) t2 = time.perf_counter() add_node_edges( graph, - surface_lookup, + adj, + cell_to_idx, + idx_to_cell, self.config.voxel_size, - step_threshold=NODE_STEP_THRESHOLD_M, max_edge_cost=NODE_MAX_EDGE_COST_M, ) edges_ms = (time.perf_counter() - t2) * 1000 - self._graph = graph logger.info( "Edges built", edges=graph.number_of_edges(), edges_ms=round(edges_ms, 1), ) + + t3 = time.perf_counter() + n_bridges = add_connectivity_bridges(graph, adj, cell_to_idx, idx_to_cell) + bridges_ms = (time.perf_counter() - t3) * 1000 + logger.info( + "Bridges added", + bridges=n_bridges, + bridges_ms=round(bridges_ms, 1), + ) + + self._graph = graph self.node_edges.publish( _PublishableLineSegments3D( ts=time.time(), From d3a530c5a8eda3ff26f6f9b2bb2c791179093f91 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Mon, 25 May 2026 13:59:24 -0700 Subject: [PATCH 14/55] Better node placement and edge connection --- .../modules/mls_planner/mls_planner.py | 405 ++++++------------ 1 file changed, 126 insertions(+), 279 deletions(-) diff --git a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py index bb63b77698..0211065e55 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py +++ b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py @@ -12,17 +12,10 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Multi-Level Surface (MLS) path planner. - -Extracts walkable surfaces from a voxelized global map, builds a sparse -node graph over those surfaces, and plans paths via local A* plus -shortest-path search on the graph. Skeleton — algorithm is filled in -piecewise. -""" +"""Multi-level surface path planner.""" from __future__ import annotations -import math import time from typing import Any @@ -38,7 +31,7 @@ import numpy as np from scipy import ndimage from scipy.sparse import csr_matrix -from scipy.sparse.csgraph import connected_components, dijkstra +from scipy.sparse.csgraph import dijkstra from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out @@ -54,10 +47,8 @@ SURFACE_EROSION_PASSES = 3 NODE_SPACING_M = 2.0 -NODE_Z_TOLERANCE_M = 1.0 +NODE_WALL_BUFFER_M = 0.3 NODE_STEP_THRESHOLD_M = 0.25 -NODE_MAX_EDGE_COST_M = 2.5 -NODE_SUB_SAMPLE_STRIDE = 20 class MLSPlannerConfig(ModuleConfig): @@ -67,12 +58,7 @@ class MLSPlannerConfig(ModuleConfig): def _extract_surfaces(points: np.ndarray, voxel_size: float, robot_height: float) -> np.ndarray: - """Find walkable surface tops in a voxelized point cloud. - - Iterate through all the columns, find continuous areas of - free space. If the free space column is at least robot height, - add the bottom of this range as a surface. - """ + """For each XY column, mark cells with at least robot_height of free space above as surfaces.""" if len(points) == 0: return np.zeros((0, 3), dtype=np.float32) @@ -151,35 +137,10 @@ def _close_surface_holes( ) -class _GridHash: - """Sparse 2D bucket index over integer cell coordinates.""" - - def __init__(self, bucket_size_cells: int) -> None: - self._bucket_size = max(1, bucket_size_cells) - self._buckets: dict[tuple[int, int], list[int]] = {} - - def _key(self, ix: int, iy: int) -> tuple[int, int]: - return (ix // self._bucket_size, iy // self._bucket_size) - - def add(self, node_id: int, ix: int, iy: int) -> None: - self._buckets.setdefault(self._key(ix, iy), []).append(node_id) - - def nearby(self, ix: int, iy: int, radius_cells: int) -> list[int]: - bucket_radius = radius_cells // self._bucket_size + 1 - bx, by = self._key(ix, iy) - result: list[int] = [] - for dbx in range(-bucket_radius, bucket_radius + 1): - for dby in range(-bucket_radius, bucket_radius + 1): - ids = self._buckets.get((bx + dbx, by + dby)) - if ids: - result.extend(ids) - return result - - def _build_surface_lookup( sx: np.ndarray, sy: np.ndarray, sz: np.ndarray ) -> dict[tuple[int, int], np.ndarray]: - """Group surface cells by XY column for fast neighbor lookup in inner A*.""" + """Group surface cells by XY column.""" by_column: dict[tuple[int, int], list[int]] = {} for ix_, iy_, iz_ in zip(sx.tolist(), sy.tolist(), sz.tolist(), strict=True): by_column.setdefault((ix_, iy_), []).append(iz_) @@ -191,18 +152,7 @@ def _build_surface_adjacency( voxel_size: float, step_threshold_cells: int, ) -> tuple[csr_matrix, dict[tuple[int, int, int], int], list[tuple[int, int, int]]]: - """Build a sparse CSR adjacency over surface cells for ``scipy.csgraph.dijkstra``. - - Each surface cell becomes a row index. Edges connect 8-XY-adjacent cells - whose ``iz`` differs by at most ``step_threshold_cells``, with weight - equal to the 3D step distance in metres. Returns ``(adj, cell_to_idx, - idx_to_cell)``. - - Fully vectorized over surface cells: for each of the eight ``(dx, dy)`` - offsets, ``np.searchsorted`` finds the range of cells in each source's - neighbor column at once, and the ``|dz|`` cap is applied as a numpy - mask. - """ + """Sparse 8-connected adjacency over surface cells, with a per-step dz cap.""" n = sum(len(zs) for zs in surface_lookup.values()) if n == 0: return csr_matrix((0, 0), dtype=np.float64), {}, [] @@ -223,8 +173,7 @@ def _build_surface_adjacency( ) cell_to_idx: dict[tuple[int, int, int], int] = {cell: i for i, cell in enumerate(idx_to_cell)} - # Encode (ix, iy) → int64 column key. Padding keeps neighbor keys - # (with dx, dy ∈ {-1, 0, +1}) in non-colliding slots from each other. + # Pack (ix, iy) into one int64 key; padding so dx, dy ∈ {-1, 0, +1} don't collide. ix_pos = ix - ix.min() + 1 iy_pos = iy - iy.min() + 1 y_range = int(iy_pos.max()) + 2 @@ -270,21 +219,19 @@ def _build_surface_adjacency( return csr_matrix((data, (rows, cols)), shape=(n, n)), cell_to_idx, idx_to_cell -def _reconstruct_path_from_predecessors( - predecessors: np.ndarray, - src_idx: int, - tgt_idx: int, - idx_to_cell: list[tuple[int, int, int]], -) -> np.ndarray: - path_indices = [tgt_idx] - cur = tgt_idx - while cur != src_idx: - cur = int(predecessors[cur]) - if cur < 0: +def _walk_predecessors( + predecessors: np.ndarray, end_idx: int, idx_to_cell: list[tuple[int, int, int]] +) -> list[tuple[int, int, int]]: + """Walk predecessors from end_idx back to its scipy Dijkstra source (neg predecessor).""" + cells: list[tuple[int, int, int]] = [idx_to_cell[end_idx]] + cur = end_idx + while True: + nxt = int(predecessors[cur]) + if nxt < 0: break - path_indices.append(cur) - path_indices.reverse() - return np.array([idx_to_cell[i] for i in path_indices], dtype=np.int64) + cells.append(idx_to_cell[nxt]) + cur = nxt + return cells def place_nodes( @@ -292,60 +239,60 @@ def place_nodes( voxel_size: float, *, node_spacing: float, - node_z_tolerance: float, - sub_sample_stride: int, -) -> tuple[nx.Graph, dict[tuple[int, int], np.ndarray]]: - """Place sparse nodes over the surface map; returns ``(graph, surface_lookup)``. - - Strides through surface cells in lex order and adds a node whenever no - existing node sits within a cylinder of XY radius ``node_spacing`` and - half-height ``node_z_tolerance``. Edges are added separately by - ``add_node_edges`` so callers can publish the node cloud as soon as - placement returns. - """ + wall_buffer: float, + step_threshold: float, +) -> tuple[nx.Graph, csr_matrix, dict[tuple[int, int, int], int], list[tuple[int, int, int]]]: + """Place nodes by greedy NMS on the dist-to-wall field from one Dijkstra. + + Run multisource Dijkstra with the surface edges as sources to find distance from wall. + Then place nodes spaced throughout based on that distance.""" graph = nx.Graph() if len(surface_points) == 0: - return graph, {} + empty_adj = csr_matrix((0, 0), dtype=np.float64) + return graph, empty_adj, {}, [] sx = np.floor(surface_points[:, 0] / voxel_size).astype(np.int64) sy = np.floor(surface_points[:, 1] / voxel_size).astype(np.int64) sz = np.floor(surface_points[:, 2] / voxel_size).astype(np.int64) - surface_lookup = _build_surface_lookup(sx, sy, sz) - spacing_cells = max(1, int(node_spacing / voxel_size)) - z_tol_cells = max(0, int(node_z_tolerance / voxel_size)) - - grid = _GridHash(spacing_cells) - node_ix: list[int] = [] - node_iy: list[int] = [] - node_iz: list[int] = [] - - order = np.lexsort((sz, sy, sx)) - spacing_sq = spacing_cells * spacing_cells - stride = max(1, sub_sample_stride) - - for idx in order[::stride]: - cix, ciy, ciz = int(sx[idx]), int(sy[idx]), int(sz[idx]) - - in_cylinder = False - for nid in grid.nearby(cix, ciy, spacing_cells): - dx = node_ix[nid] - cix - dy = node_iy[nid] - ciy - dz = node_iz[nid] - ciz - if dx * dx + dy * dy < spacing_sq and abs(dz) < z_tol_cells: - in_cylinder = True - break - if in_cylinder: - continue + step_cells = max(0, int(step_threshold / voxel_size)) + adj, cell_to_idx, idx_to_cell = _build_surface_adjacency(surface_lookup, voxel_size, step_cells) + + n_cells = adj.shape[0] + if n_cells == 0: + return graph, adj, cell_to_idx, idx_to_cell + + neighbor_counts = np.diff(adj.indptr) + boundary_indices = np.where(neighbor_counts < 8)[0] + if len(boundary_indices) == 0: + boundary_indices = np.array([0], dtype=np.int64) + + dist = dijkstra(adj, indices=boundary_indices, min_only=True) + + cells_arr = np.array(idx_to_cell, dtype=np.float64) + cell_positions = cells_arr * voxel_size + np.array([0.5 * voxel_size, 0.5 * voxel_size, 0.0]) + + candidate_mask = np.isfinite(dist) & (dist >= wall_buffer) + candidate_indices = np.where(candidate_mask)[0] + if len(candidate_indices) == 0: + return graph, adj, cell_to_idx, idx_to_cell + order = candidate_indices[np.argsort(-dist[candidate_indices])] + + placed_positions = np.empty((0, 3), dtype=np.float64) + spacing_sq = node_spacing * node_spacing - new_id = len(node_ix) - node_ix.append(cix) - node_iy.append(ciy) - node_iz.append(ciz) - grid.add(new_id, cix, ciy) + for cell_idx in order: + pos = cell_positions[cell_idx] + if placed_positions.shape[0] > 0: + diff = placed_positions - pos + if (diff * diff).sum(-1).min() < spacing_sq: + continue + placed_positions = np.vstack([placed_positions, pos[None, :]]) + cix, ciy, ciz = idx_to_cell[int(cell_idx)] + nid = graph.number_of_nodes() graph.add_node( - new_id, + nid, pos=( (cix + 0.5) * voxel_size, (ciy + 0.5) * voxel_size, @@ -354,7 +301,7 @@ def place_nodes( cell=(cix, ciy, ciz), ) - return graph, surface_lookup + return graph, adj, cell_to_idx, idx_to_cell def add_node_edges( @@ -362,147 +309,72 @@ def add_node_edges( adj: csr_matrix, cell_to_idx: dict[tuple[int, int, int], int], idx_to_cell: list[tuple[int, int, int]], - voxel_size: float, - *, - max_edge_cost: float, ) -> None: - """Connect each node to nearby nodes the surface Dijkstra can reach. + """Add Voronoi-adjacency edges between placed nodes. - For each source node, runs scipy's bounded Dijkstra over the surface - adjacency and reads the cost to every higher-id candidate within - ``max_edge_cost`` (euclidean). Edges get the reconstructed cell path - stored under ``data["path"]``. + One multi-source Dijkstra labels each cell with its closest node. Pairs + of adjacent cells with different labels mark Voronoi boundaries between + their owners. The cheapest crossing per node-pair becomes an edge with + the cell path stored on data["path"]. """ if graph.number_of_nodes() == 0: return - edge_radius_cells = max(1, int(max_edge_cost / voxel_size)) - - grid = _GridHash(edge_radius_cells) - cells: dict[int, tuple[int, int, int]] = {} - for node_id, data in graph.nodes(data=True): - cix, ciy, ciz = data["cell"] - cells[node_id] = (cix, ciy, ciz) - grid.add(node_id, cix, ciy) - - for node_id in sorted(graph.nodes()): - cix, ciy, ciz = cells[node_id] - candidate_cells: dict[tuple[int, int, int], int] = {} - for other_id in grid.nearby(cix, ciy, edge_radius_cells): - if other_id <= node_id: - continue - ox, oy, oz = cells[other_id] - dx, dy, dz = ox - cix, oy - ciy, oz - ciz - if math.sqrt(dx * dx + dy * dy + dz * dz) * voxel_size > max_edge_cost: - continue - candidate_cells[(ox, oy, oz)] = other_id - if not candidate_cells: - continue - - src_idx = cell_to_idx.get((cix, ciy, ciz)) - if src_idx is None: - continue - - dist, predecessors = dijkstra( - adj, - indices=src_idx, - limit=max_edge_cost, - return_predecessors=True, - ) - - for cell, other_id in candidate_cells.items(): - tgt_idx = cell_to_idx.get(cell) - if tgt_idx is None or not math.isfinite(dist[tgt_idx]): - continue - graph.add_edge( - node_id, - other_id, - weight=float(dist[tgt_idx]), - path=_reconstruct_path_from_predecessors( - predecessors, src_idx, tgt_idx, idx_to_cell - ), - ) - - -def add_connectivity_bridges( - graph: nx.Graph, - adj: csr_matrix, - cell_to_idx: dict[tuple[int, int, int], int], - idx_to_cell: list[tuple[int, int, int]], -) -> int: - """Bridge graph components that share a surface component but aren't connected. - - Flood-fills the surface adjacency to label every cell with its connected - component, then for each surface component containing more than one - graph component, iteratively connects the closest cell-distance pair of - nodes across two unmerged components with an uncapped surface Dijkstra - edge. Returns the number of bridges added. - """ - if graph.number_of_nodes() == 0: - return 0 + node_ids = list(graph.nodes()) + source_cell_indices = np.empty(len(node_ids), dtype=np.int64) + cell_idx_to_nid: dict[int, int] = {} + for nid in node_ids: + cell_idx = cell_to_idx[graph.nodes[nid]["cell"]] + source_cell_indices[nid] = cell_idx + cell_idx_to_nid[cell_idx] = nid + + dist, predecessors, source_cells = dijkstra( + adj, + indices=source_cell_indices, + min_only=True, + return_predecessors=True, + ) - _, surf_labels = connected_components(adj, directed=False) + rows = np.repeat(np.arange(adj.shape[0]), np.diff(adj.indptr)) + cols = adj.indices + weights = adj.data + src_u = source_cells[rows] + src_v = source_cells[cols] + boundary = (src_u != src_v) & (src_u >= 0) & (src_v >= 0) + if not boundary.any(): + return - nodes_by_surf_cc: dict[int, list[int]] = {} - for node_id in graph.nodes(): - cell = graph.nodes[node_id]["cell"] - cell_idx = cell_to_idx.get(cell) - if cell_idx is None: - continue - nodes_by_surf_cc.setdefault(int(surf_labels[cell_idx]), []).append(node_id) - - bridges_added = 0 - for node_ids in nodes_by_surf_cc.values(): - components = [list(c) for c in nx.connected_components(graph.subgraph(node_ids))] - while len(components) > 1: - best_cost_sq = math.inf - best_i = best_j = best_ai = best_bi = -1 - for i in range(len(components)): - cells_i = np.array([graph.nodes[n]["cell"] for n in components[i]], dtype=np.int64) - for j in range(i + 1, len(components)): - cells_j = np.array( - [graph.nodes[n]["cell"] for n in components[j]], dtype=np.int64 - ) - diff = cells_i[:, None, :] - cells_j[None, :, :] - dist_sq = (diff * diff).sum(-1) - flat = int(dist_sq.argmin()) - ai, bi = divmod(flat, len(components[j])) - c = int(dist_sq[ai, bi]) - if c < best_cost_sq: - best_cost_sq = c - best_i, best_j, best_ai, best_bi = i, j, ai, bi - node_a = components[best_i][best_ai] - node_b = components[best_j][best_bi] - src_idx = cell_to_idx[graph.nodes[node_a]["cell"]] - tgt_idx = cell_to_idx[graph.nodes[node_b]["cell"]] - dist, predecessors = dijkstra(adj, indices=src_idx, return_predecessors=True) - if math.isfinite(dist[tgt_idx]): - graph.add_edge( - node_a, - node_b, - weight=float(dist[tgt_idx]), - path=_reconstruct_path_from_predecessors( - predecessors, src_idx, tgt_idx, idx_to_cell - ), - ) - bridges_added += 1 - merged = components[best_i] + components[best_j] - components = [ - components[k] for k in range(len(components)) if k not in (best_i, best_j) - ] - components.append(merged) - - return bridges_added + b_rows = rows[boundary] + b_cols = cols[boundary] + b_costs = dist[b_rows] + weights[boundary] + dist[b_cols] + b_src_u = src_u[boundary] + b_src_v = src_v[boundary] + + # Keep the min-cost boundary crossing per (node_a, node_b) pair. + best: dict[tuple[int, int], tuple[float, int, int]] = {} + for i in range(len(b_costs)): + nid_a = cell_idx_to_nid[int(b_src_u[i])] + nid_b = cell_idx_to_nid[int(b_src_v[i])] + u_a = int(b_rows[i]) + u_b = int(b_cols[i]) + if nid_a > nid_b: + nid_a, nid_b = nid_b, nid_a + u_a, u_b = u_b, u_a + cost = float(b_costs[i]) + existing = best.get((nid_a, nid_b)) + if existing is None or existing[0] > cost: + best[(nid_a, nid_b)] = (cost, u_a, u_b) + + for (nid_a, nid_b), (cost, u_a, u_b) in best.items(): + path_a = _walk_predecessors(predecessors, u_a, idx_to_cell) + path_b = _walk_predecessors(predecessors, u_b, idx_to_cell) + path_a.reverse() + full_path = np.array(path_a + path_b, dtype=np.int64) + graph.add_edge(nid_a, nid_b, weight=cost, path=full_path) class _PublishableLineSegments3D(LineSegments3D): - """LineSegments3D with a Python lcm_encode that matches the C++ wire format. - - Upstream only implements decode (encode raises NotImplementedError); this - subclass produces the same nav_msgs/Path wire layout, where consecutive - pose pairs are interpreted as segments and pose.orientation.w carries - traversability. - """ + """LineSegments3D with a Python lcm_encode; upstream only implements decode.""" def lcm_encode(self) -> bytes: lcm_msg = LCMPath() @@ -541,7 +413,7 @@ def _nodes_to_cloud(graph: nx.Graph) -> np.ndarray: def _edges_to_segments( graph: nx.Graph, voxel_size: float ) -> list[tuple[tuple[float, float, float], tuple[float, float, float]]]: - """Walk each edge's cached A* path and emit consecutive cell pairs as segments.""" + """Emit one segment per consecutive cell pair along each edge's cached path.""" segments: list[tuple[tuple[float, float, float], tuple[float, float, float]]] = [] for _, _, data in graph.edges(data=True): path_cells: np.ndarray = data["path"] @@ -593,18 +465,14 @@ async def handle_global_map(self, msg: PointCloud2) -> None: surface_ms=round(surfaces_ms, 1), ) - logger.info( - "Placing nodes", - spacing_m=NODE_SPACING_M, - stride=NODE_SUB_SAMPLE_STRIDE, - ) + logger.info("Placing nodes", spacing_m=NODE_SPACING_M) t1 = time.perf_counter() - graph, surface_lookup = place_nodes( + graph, adj, cell_to_idx, idx_to_cell = place_nodes( surface_points, self.config.voxel_size, node_spacing=NODE_SPACING_M, - node_z_tolerance=NODE_Z_TOLERANCE_M, - sub_sample_stride=NODE_SUB_SAMPLE_STRIDE, + wall_buffer=NODE_WALL_BUFFER_M, + step_threshold=NODE_STEP_THRESHOLD_M, ) place_ms = (time.perf_counter() - t1) * 1000 self.nodes.publish( @@ -620,21 +488,9 @@ async def handle_global_map(self, msg: PointCloud2) -> None: place_ms=round(place_ms, 1), ) - step_cells = max(0, int(NODE_STEP_THRESHOLD_M / self.config.voxel_size)) - adj, cell_to_idx, idx_to_cell = _build_surface_adjacency( - surface_lookup, self.config.voxel_size, step_cells - ) - - logger.info("Building edges", max_edge_cost_m=NODE_MAX_EDGE_COST_M) + logger.info("Building edges") t2 = time.perf_counter() - add_node_edges( - graph, - adj, - cell_to_idx, - idx_to_cell, - self.config.voxel_size, - max_edge_cost=NODE_MAX_EDGE_COST_M, - ) + add_node_edges(graph, adj, cell_to_idx, idx_to_cell) edges_ms = (time.perf_counter() - t2) * 1000 logger.info( "Edges built", @@ -642,15 +498,6 @@ async def handle_global_map(self, msg: PointCloud2) -> None: edges_ms=round(edges_ms, 1), ) - t3 = time.perf_counter() - n_bridges = add_connectivity_bridges(graph, adj, cell_to_idx, idx_to_cell) - bridges_ms = (time.perf_counter() - t3) * 1000 - logger.info( - "Bridges added", - bridges=n_bridges, - bridges_ms=round(bridges_ms, 1), - ) - self._graph = graph self.node_edges.publish( _PublishableLineSegments3D( From 51e40d95df6403eb09991316f84bf5b53f435a37 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Mon, 25 May 2026 15:25:29 -0700 Subject: [PATCH 15/55] Perf improvement --- .../modules/mls_planner/mls_planner.py | 23 ++++++++++--------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py index 0211065e55..ba0056ea4d 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py +++ b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py @@ -32,6 +32,7 @@ from scipy import ndimage from scipy.sparse import csr_matrix from scipy.sparse.csgraph import dijkstra +from scipy.spatial import cKDTree from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out @@ -278,18 +279,16 @@ def place_nodes( if len(candidate_indices) == 0: return graph, adj, cell_to_idx, idx_to_cell order = candidate_indices[np.argsort(-dist[candidate_indices])] + positions = cell_positions[order] - placed_positions = np.empty((0, 3), dtype=np.float64) - spacing_sq = node_spacing * node_spacing - - for cell_idx in order: - pos = cell_positions[cell_idx] - if placed_positions.shape[0] > 0: - diff = placed_positions - pos - if (diff * diff).sum(-1).min() < spacing_sq: - continue - placed_positions = np.vstack([placed_positions, pos[None, :]]) - cix, ciy, ciz = idx_to_cell[int(cell_idx)] + # kill points in batches with kd tree + tree = cKDTree(positions) + killed = np.zeros(len(order), dtype=bool) + + for i in range(len(order)): + if killed[i]: + continue + cix, ciy, ciz = idx_to_cell[int(order[i])] nid = graph.number_of_nodes() graph.add_node( nid, @@ -300,6 +299,8 @@ def place_nodes( ), cell=(cix, ciy, ciz), ) + nearby = tree.query_ball_point(positions[i], r=node_spacing) + killed[np.asarray(nearby, dtype=np.int64)] = True return graph, adj, cell_to_idx, idx_to_cell From 8253b828f8bc144c0774371f8fe49fc158734735 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Mon, 25 May 2026 16:23:55 -0700 Subject: [PATCH 16/55] Path planning --- .../modules/mls_planner/mls_planner.py | 210 ++++++++++++++++-- 1 file changed, 191 insertions(+), 19 deletions(-) diff --git a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py index ba0056ea4d..2006901fd2 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py +++ b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py @@ -36,6 +36,7 @@ from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.nav_msgs.LineSegments3D import LineSegments3D from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.nav_msgs.Path import Path, sec_nsec @@ -174,7 +175,7 @@ def _build_surface_adjacency( ) cell_to_idx: dict[tuple[int, int, int], int] = {cell: i for i, cell in enumerate(idx_to_cell)} - # Pack (ix, iy) into one int64 key; padding so dx, dy ∈ {-1, 0, +1} don't collide. + # Pack (ix, iy) into one int64 key. Padding leaves room for dx, dy in -1, 0, 1. ix_pos = ix - ix.min() + 1 iy_pos = iy - iy.min() + 1 y_range = int(iy_pos.max()) + 2 @@ -242,7 +243,13 @@ def place_nodes( node_spacing: float, wall_buffer: float, step_threshold: float, -) -> tuple[nx.Graph, csr_matrix, dict[tuple[int, int, int], int], list[tuple[int, int, int]]]: +) -> tuple[ + nx.Graph, + csr_matrix, + dict[tuple[int, int, int], int], + list[tuple[int, int, int]], + dict[tuple[int, int], np.ndarray], +]: """Place nodes by greedy NMS on the dist-to-wall field from one Dijkstra. Run multisource Dijkstra with the surface edges as sources to find distance from wall. @@ -250,7 +257,7 @@ def place_nodes( graph = nx.Graph() if len(surface_points) == 0: empty_adj = csr_matrix((0, 0), dtype=np.float64) - return graph, empty_adj, {}, [] + return graph, empty_adj, {}, [], {} sx = np.floor(surface_points[:, 0] / voxel_size).astype(np.int64) sy = np.floor(surface_points[:, 1] / voxel_size).astype(np.int64) @@ -262,7 +269,7 @@ def place_nodes( n_cells = adj.shape[0] if n_cells == 0: - return graph, adj, cell_to_idx, idx_to_cell + return graph, adj, cell_to_idx, idx_to_cell, surface_lookup neighbor_counts = np.diff(adj.indptr) boundary_indices = np.where(neighbor_counts < 8)[0] @@ -277,7 +284,7 @@ def place_nodes( candidate_mask = np.isfinite(dist) & (dist >= wall_buffer) candidate_indices = np.where(candidate_mask)[0] if len(candidate_indices) == 0: - return graph, adj, cell_to_idx, idx_to_cell + return graph, adj, cell_to_idx, idx_to_cell, surface_lookup order = candidate_indices[np.argsort(-dist[candidate_indices])] positions = cell_positions[order] @@ -302,7 +309,7 @@ def place_nodes( nearby = tree.query_ball_point(positions[i], r=node_spacing) killed[np.asarray(nearby, dtype=np.int64)] = True - return graph, adj, cell_to_idx, idx_to_cell + return graph, adj, cell_to_idx, idx_to_cell, surface_lookup def add_node_edges( @@ -310,16 +317,15 @@ def add_node_edges( adj: csr_matrix, cell_to_idx: dict[tuple[int, int, int], int], idx_to_cell: list[tuple[int, int, int]], -) -> None: +) -> tuple[np.ndarray, dict[int, int]]: """Add Voronoi-adjacency edges between placed nodes. - One multi-source Dijkstra labels each cell with its closest node. Pairs - of adjacent cells with different labels mark Voronoi boundaries between - their owners. The cheapest crossing per node-pair becomes an edge with - the cell path stored on data["path"]. + One multi-source Dijkstra labels each cell with its nearest node. The cheapest + boundary crossing per node-pair becomes an edge with the cell path on data["path"]. + Returns source_cells (per-cell owner) and cell_idx_to_nid for pose-snapping. """ if graph.number_of_nodes() == 0: - return + return np.full(adj.shape[0], -9999, dtype=np.int64), {} node_ids = list(graph.nodes()) source_cell_indices = np.empty(len(node_ids), dtype=np.int64) @@ -343,7 +349,7 @@ def add_node_edges( src_v = source_cells[cols] boundary = (src_u != src_v) & (src_u >= 0) & (src_v >= 0) if not boundary.any(): - return + return source_cells, cell_idx_to_nid b_rows = rows[boundary] b_cols = cols[boundary] @@ -373,9 +379,11 @@ def add_node_edges( full_path = np.array(path_a + path_b, dtype=np.int64) graph.add_edge(nid_a, nid_b, weight=cost, path=full_path) + return source_cells, cell_idx_to_nid + class _PublishableLineSegments3D(LineSegments3D): - """LineSegments3D with a Python lcm_encode; upstream only implements decode.""" + """LineSegments3D with a Python lcm_encode. Upstream only implements decode.""" def lcm_encode(self) -> bytes: lcm_msg = LCMPath() @@ -446,6 +454,10 @@ def __init__(self, **kwargs: Any) -> None: super().__init__(**kwargs) self._latest_start: Odometry | None = None self._graph: nx.Graph | None = None + self._cell_to_idx: dict[tuple[int, int, int], int] | None = None + self._surface_lookup: dict[tuple[int, int], np.ndarray] | None = None + self._source_cells: np.ndarray | None = None + self._cell_idx_to_nid: dict[int, int] | None = None async def handle_global_map(self, msg: PointCloud2) -> None: points, _ = msg.as_numpy() @@ -468,7 +480,7 @@ async def handle_global_map(self, msg: PointCloud2) -> None: logger.info("Placing nodes", spacing_m=NODE_SPACING_M) t1 = time.perf_counter() - graph, adj, cell_to_idx, idx_to_cell = place_nodes( + graph, adj, cell_to_idx, idx_to_cell, surface_lookup = place_nodes( surface_points, self.config.voxel_size, node_spacing=NODE_SPACING_M, @@ -491,7 +503,7 @@ async def handle_global_map(self, msg: PointCloud2) -> None: logger.info("Building edges") t2 = time.perf_counter() - add_node_edges(graph, adj, cell_to_idx, idx_to_cell) + source_cells, cell_idx_to_nid = add_node_edges(graph, adj, cell_to_idx, idx_to_cell) edges_ms = (time.perf_counter() - t2) * 1000 logger.info( "Edges built", @@ -500,6 +512,11 @@ async def handle_global_map(self, msg: PointCloud2) -> None: ) self._graph = graph + self._cell_to_idx = cell_to_idx + self._surface_lookup = surface_lookup + self._source_cells = source_cells + self._cell_idx_to_nid = cell_idx_to_nid + self.node_edges.publish( _PublishableLineSegments3D( ts=time.time(), @@ -515,8 +532,163 @@ async def handle_goal_pose(self, msg: Odometry) -> None: if self._latest_start is None: logger.warning("MLSPlanner received goal before start; skipping") return + if self._graph is None or self._graph.number_of_nodes() == 0: + logger.warning("MLSPlanner received goal before graph was built; skipping") + return + + t0 = time.perf_counter() + start = (self._latest_start.x, self._latest_start.y, self._latest_start.z) + goal = (msg.x, msg.y, msg.z) + + start_node = self._snap_pose_to_node(start) + goal_node = self._snap_pose_to_node(goal) + if start_node is None or goal_node is None: + logger.warning( + "Could not snap pose to graph", + start=start, + goal=goal, + start_node=start_node, + goal_node=goal_node, + ) + return + assert self._graph is not None + logger.info( + "Snapped poses to graph nodes", + start_pose=start, + start_node=start_node, + start_node_pos=self._graph.nodes[start_node]["pos"], + goal_pose=goal, + goal_node=goal_node, + goal_node_pos=self._graph.nodes[goal_node]["pos"], + ) + + try: + node_seq = nx.shortest_path( + self._graph, source=start_node, target=goal_node, weight="weight" + ) + except nx.NetworkXNoPath: + logger.warning( + "No path between start and goal nodes", + start_node=start_node, + goal_node=goal_node, + ) + return + + waypoints = self._assemble_waypoints(node_seq, start, goal) + plan_ms = (time.perf_counter() - t0) * 1000 + + now = time.time() + path_msg = Path( + ts=now, + frame_id=self.config.world_frame, + poses=[ + PoseStamped( + ts=now, + frame_id=self.config.world_frame, + position=[float(x), float(y), float(z)], + orientation=[0.0, 0.0, 0.0, 1.0], + ) + for x, y, z in waypoints + ], + ) + self.path.publish(path_msg) logger.info( - "MLSPlanner goal received (not yet implemented)", - start=(self._latest_start.x, self._latest_start.y, self._latest_start.z), - goal=(msg.x, msg.y, msg.z), + "Path planned", + waypoints=len(waypoints), + nodes_traversed=len(node_seq), + plan_ms=round(plan_ms, 1), ) + + def _snap_pose_to_node(self, pose_xyz: tuple[float, float, float]) -> int | None: + """Snap pose to its owning node via the precomputed Voronoi labels. + + Finds the surface cell in the pose's column, looks up its owner in source_cells. + Falls back to nearby columns if the pose's own column has no surface. + """ + if ( + self._graph is None + or self._cell_to_idx is None + or self._surface_lookup is None + or self._source_cells is None + or self._cell_idx_to_nid is None + ): + return None + if self._graph.number_of_nodes() == 0: + return None + + voxel = self.config.voxel_size + x, y, z = pose_xyz + ix = int(np.floor(x / voxel)) + iy = int(np.floor(y / voxel)) + target_iz = int(np.floor(z / voxel)) - 1 + tolerance_cells = int(np.ceil(self.config.robot_height / voxel)) + + cell = self._best_iz_in_column(ix, iy, target_iz, tolerance_cells) + if cell is None: + # Pose's column has no surface. Try nearby columns. + search_radius = 5 + best_cell = None + best_d2 = -1 + for dix in range(-search_radius, search_radius + 1): + for diy in range(-search_radius, search_radius + 1): + if dix == 0 and diy == 0: + continue + c = self._best_iz_in_column(ix + dix, iy + diy, target_iz, tolerance_cells) + if c is None: + continue + d2 = dix * dix + diy * diy + if best_d2 < 0 or d2 < best_d2: + best_d2 = d2 + best_cell = c + cell = best_cell + if cell is None: + return None + + cell_idx = self._cell_to_idx.get(cell) + if cell_idx is None: + return None + owner_cell_idx = int(self._source_cells[cell_idx]) + if owner_cell_idx < 0: + return None + return self._cell_idx_to_nid.get(owner_cell_idx) + + def _best_iz_in_column( + self, ix: int, iy: int, target_iz: int, tolerance_cells: int + ) -> tuple[int, int, int] | None: + """Surface iz in column (ix, iy) closest to target_iz, within tolerance.""" + if self._surface_lookup is None: + return None + zs = self._surface_lookup.get((ix, iy)) + if zs is None or len(zs) == 0: + return None + distances = np.abs(zs - target_iz) + best = int(distances.argmin()) + if int(distances[best]) > tolerance_cells: + return None + return (ix, iy, int(zs[best])) + + def _assemble_waypoints( + self, + node_seq: list[int], + start_pose: tuple[float, float, float], + goal_pose: tuple[float, float, float], + ) -> list[tuple[float, float, float]]: + """Chain cached edge paths into a continuous waypoint list, bracketed by the actual poses.""" + assert self._graph is not None + voxel = self.config.voxel_size + cells: list[tuple[int, int, int]] = [] + for i in range(len(node_seq) - 1): + a, b = node_seq[i], node_seq[i + 1] + edge_path: np.ndarray = self._graph[a][b]["path"] + # Stored path runs from min(a, b) to max(a, b). Reverse if traversing the other way. + if a > b: + edge_path = edge_path[::-1] + tail = edge_path if i == 0 else edge_path[1:] + for c in tail: + cells.append((int(c[0]), int(c[1]), int(c[2]))) + + waypoints: list[tuple[float, float, float]] = [start_pose] + for cix, ciy, ciz in cells: + waypoints.append(((cix + 0.5) * voxel, (ciy + 0.5) * voxel, (ciz + 1.0) * voxel)) + waypoints.append(goal_pose) + return waypoints From f446cb912860307551bcccd98bd9e95e26ec8ac1 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Mon, 25 May 2026 17:07:33 -0700 Subject: [PATCH 17/55] Refactor and run tests --- dimos/navigation/nav_stack/evaluator/main.py | 39 +- .../nav_stack/evaluator/scenarios.py | 103 ++++-- .../evaluator/straight_line_planner.py | 95 ----- .../modules/mls_planner/mls_planner.py | 342 ++++++++++-------- 4 files changed, 290 insertions(+), 289 deletions(-) delete mode 100644 dimos/navigation/nav_stack/evaluator/straight_line_planner.py diff --git a/dimos/navigation/nav_stack/evaluator/main.py b/dimos/navigation/nav_stack/evaluator/main.py index ff0f3432ae..1f1fb132bc 100644 --- a/dimos/navigation/nav_stack/evaluator/main.py +++ b/dimos/navigation/nav_stack/evaluator/main.py @@ -14,14 +14,15 @@ """Blueprint + entrypoint for the path-planner evaluator. -Wires the Evaluator and StraightLinePlanner together and bridges all -streams to rerun. Run with:: +Wires the Evaluator and MLSPlanner together and bridges all streams to rerun. +Run with:: python -m dimos.navigation.nav_stack.evaluator.main """ from __future__ import annotations +from functools import partial from typing import Any import numpy as np @@ -33,13 +34,15 @@ from dimos.navigation.nav_stack.modules.mls_planner.mls_planner import ( NODE_STEP_THRESHOLD_M, MLSPlanner, - _build_surface_adjacency, - _build_surface_lookup, + MLSPlannerConfig, + build_surface_adjacency, + build_surface_lookup, ) from dimos.visualization.rerun.bridge import RerunBridgeModule _POSE_MARKER_RADIUS = 0.4 -_SURFACE_VOXEL = 0.1 +# Small lift so graph artifacts render visibly above the surface points instead of z-fighting. +_GRAPH_Z_LIFT = 0.05 _SURFACE_COMPONENT_PALETTE = np.array( [ [220, 50, 50], @@ -62,7 +65,7 @@ def _render_start_pose(msg: Any) -> Any: return rr.Points3D( positions=[[msg.x, msg.y, msg.z]], - colors=[[0, 255, 0]], # green + colors=[[0, 255, 0]], radii=[_POSE_MARKER_RADIUS], ) @@ -72,26 +75,27 @@ def _render_goal_pose(msg: Any) -> Any: return rr.Points3D( positions=[[msg.x, msg.y, msg.z]], - colors=[[255, 0, 0]], # red + colors=[[255, 0, 0]], radii=[_POSE_MARKER_RADIUS], ) def _render_global_map(msg: Any) -> Any: - return msg.to_rerun(voxel_size=0.03, colors=[128, 128, 128]) # gray + return msg.to_rerun(voxel_size=0.03, colors=[128, 128, 128]) -def _render_surface_map(msg: Any) -> Any: +def _render_surface_map(voxel_size: float, msg: Any) -> Any: + """Render surface points colored by connected component.""" import rerun as rr pts, _ = msg.as_numpy() if pts is None or len(pts) == 0: return rr.Points3D([]) - indices = np.floor(pts / _SURFACE_VOXEL).astype(np.int64) + indices = np.floor(pts / voxel_size).astype(np.int64) ix, iy, iz = indices[:, 0], indices[:, 1], indices[:, 2] - surface_lookup = _build_surface_lookup(ix, iy, iz) - step_cells = max(0, int(NODE_STEP_THRESHOLD_M / _SURFACE_VOXEL)) - adj, cell_to_idx, _ = _build_surface_adjacency(surface_lookup, _SURFACE_VOXEL, step_cells) + surface_lookup = build_surface_lookup(ix, iy, iz) + step_cells = max(0, int(NODE_STEP_THRESHOLD_M / voxel_size)) + adj, cell_to_idx, _ = build_surface_adjacency(surface_lookup, voxel_size, step_cells) _, labels = connected_components(adj, directed=False) point_labels = np.array( [ @@ -104,10 +108,6 @@ def _render_surface_map(msg: Any) -> Any: return rr.Points3D(positions=pts, colors=colors, radii=[0.05]) -# raise the path and way points out of the surface -_GRAPH_Z_LIFT = 0.1 - - def _render_nodes(msg: Any) -> Any: import rerun as rr @@ -116,7 +116,7 @@ def _render_nodes(msg: Any) -> Any: return rr.Points3D([]) pts = pts.copy() pts[:, 2] += _GRAPH_Z_LIFT - return rr.Points3D(positions=pts, colors=[[75, 156, 211]], radii=[0.15]) # Carolina Blue + return rr.Points3D(positions=pts, colors=[[75, 156, 211]], radii=[0.15]) def _render_node_edges(msg: Any) -> Any: @@ -124,6 +124,7 @@ def _render_node_edges(msg: Any) -> Any: def create_evaluator_blueprint() -> Blueprint: + planner_voxel = MLSPlannerConfig().voxel_size return autoconnect( Evaluator.blueprint(), MLSPlanner.blueprint(), @@ -132,7 +133,7 @@ def create_evaluator_blueprint() -> Blueprint: "world/start_pose": _render_start_pose, "world/goal_pose": _render_goal_pose, "world/global_map": _render_global_map, - "world/surface_map": _render_surface_map, + "world/surface_map": partial(_render_surface_map, planner_voxel), "world/nodes": _render_nodes, "world/node_edges": _render_node_edges, } diff --git a/dimos/navigation/nav_stack/evaluator/scenarios.py b/dimos/navigation/nav_stack/evaluator/scenarios.py index e6d243bda0..5a1acd7cc8 100644 --- a/dimos/navigation/nav_stack/evaluator/scenarios.py +++ b/dimos/navigation/nav_stack/evaluator/scenarios.py @@ -16,7 +16,7 @@ Each scenario is a self-contained (map, start, goal, expectation) bundle. Maps are PointCloud2 obstacle clouds, start/goal are Odometry poses, and -``expect_path`` records whether a planner *should* be able to find a path +``expect_path`` records whether a planner should be able to find a path (used by the evaluator to score pass/fail). """ @@ -32,6 +32,9 @@ from dimos.navigation.nav_stack.evaluator.mesh_loader import load_voxelized_mesh WORLD_FRAME = "map" +# Walls must reach above the planner's robot_height (default 1.5m) to block surface above. +_WALL_HEIGHT_M = 2.0 + MESH_PATH = "/home/andrew/Downloads/model.glb" @@ -55,18 +58,35 @@ def _cloud(points: np.ndarray, frame: str = WORLD_FRAME) -> PointCloud2: return PointCloud2.from_numpy(points.astype(np.float32), frame_id=frame) +_WALL_THICKNESS_M = 0.5 + + def _wall( - x0: float, y0: float, x1: float, y1: float, *, spacing: float = 0.1, height: float = 0.5 + x0: float, + y0: float, + x1: float, + y1: float, + *, + spacing: float = 0.1, + height: float = _WALL_HEIGHT_M, + thickness: float = _WALL_THICKNESS_M, ) -> np.ndarray: - """Sample a vertical-wall obstacle as a line of points from (x0,y0) to (x1,y1).""" - length = float(np.hypot(x1 - x0, y1 - y0)) - n = max(2, int(np.ceil(length / spacing))) - xs = np.linspace(x0, x1, n) - ys = np.linspace(y0, y1, n) + """Sample a vertical wall as a 3D box from (x0,y0) to (x1,y1). + + Thickness extends perpendicular to the wall line in the XY plane. + """ + dx, dy = x1 - x0, y1 - y0 + length = float(np.hypot(dx, dy)) + if length == 0: + return np.zeros((0, 3), dtype=np.float32) + perp_x, perp_y = -dy / length, dx / length + along = np.linspace(0.0, 1.0, max(2, int(np.ceil(length / spacing)))) + perp = np.linspace(-thickness / 2, thickness / 2, max(1, int(np.ceil(thickness / spacing)) + 1)) zs = np.linspace(0.0, height, max(2, int(np.ceil(height / spacing)))) - grid_xs, grid_zs = np.meshgrid(xs, zs) - grid_ys, _ = np.meshgrid(ys, zs) - return np.column_stack([grid_xs.ravel(), grid_ys.ravel(), grid_zs.ravel()]) + a, p, z = np.meshgrid(along, perp, zs, indexing="ij") + x = x0 + a.ravel() * dx + p.ravel() * perp_x + y = y0 + a.ravel() * dy + p.ravel() * perp_y + return np.column_stack([x, y, z.ravel()]) def _floor( @@ -80,33 +100,66 @@ def _floor( xs = np.arange(x_min, x_max + spacing, spacing) ys = np.arange(y_min, y_max + spacing, spacing) grid_xs, grid_ys = np.meshgrid(xs, ys) - pts = np.column_stack([grid_xs.ravel(), grid_ys.ravel(), np.zeros(grid_xs.size)]) - return pts - - -def _empty_map() -> PointCloud2: - return _cloud(_floor()) + return np.column_stack([grid_xs.ravel(), grid_ys.ravel(), np.zeros(grid_xs.size)]) def _map_with_walls(*walls: np.ndarray) -> PointCloud2: return _cloud(np.vstack([_floor(), *walls])) -def default_scenarios() -> list[PlannerScenario]: +def empty_floor() -> PlannerScenario: + return PlannerScenario( + name="empty_floor", + global_map=_cloud(_floor()), + start_pose=_odom(-1.0, 0.0, 0.2), + goal_pose=_odom(7.0, 0.0, 0.2), + expect_path=True, + ) + + +def blocked_wall() -> PlannerScenario: + return PlannerScenario( + name="blocked_wall", + global_map=_map_with_walls(_wall(3.0, -3.0, 3.0, 3.0)), + start_pose=_odom(-1.0, 0.0, 0.2), + goal_pose=_odom(6.0, 0.0, 0.2), + expect_path=False, + ) + + +def two_rooms_one_door() -> PlannerScenario: + return PlannerScenario( + name="two_rooms_one_door", + global_map=_map_with_walls( + _wall(3.0, -3.0, 3.0, -0.75), + _wall(3.0, 0.75, 3.0, 3.0), + ), + start_pose=_odom(-1.0, 0.0, 0.2), + goal_pose=_odom(6.0, 0.0, 0.2), + expect_path=True, + ) + + +def _mesh_scenarios() -> list[PlannerScenario]: + """Two scenarios on a real building mesh: ground-level traverse and a stair climb.""" cloud = _cloud(load_voxelized_mesh(MESH_PATH)) return [ PlannerScenario( - name="outside", + name="mesh_outside", global_map=cloud, start_pose=_odom(-20.45, -19.85, 1.75), goal_pose=_odom(21.95, -4.25, 1.75), expect_path=True, ), - # PlannerScenario( - # name="up the stairs", - # global_map=cloud, - # start_pose=_odom(7.15, -3.55, 2.05), - # goal_pose=_odom(5.55, -2.05, 5.65), - # expect_path=True, - # ), + PlannerScenario( + name="mesh_up_the_stairs", + global_map=cloud, + start_pose=_odom(7.15, -3.55, 2.05), + goal_pose=_odom(5.55, -2.05, 5.65), + expect_path=True, + ), ] + + +def default_scenarios() -> list[PlannerScenario]: + return [empty_floor(), blocked_wall(), two_rooms_one_door(), *_mesh_scenarios()] diff --git a/dimos/navigation/nav_stack/evaluator/straight_line_planner.py b/dimos/navigation/nav_stack/evaluator/straight_line_planner.py deleted file mode 100644 index 8b17a35101..0000000000 --- a/dimos/navigation/nav_stack/evaluator/straight_line_planner.py +++ /dev/null @@ -1,95 +0,0 @@ -# Copyright 2026 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. - -"""Braindead path planner just for testing out the eval architecture. - -It just creates a straight line path from start to end. -""" - -from __future__ import annotations - -import time -from typing import Any - -import numpy as np - -from dimos.core.module import Module, ModuleConfig -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.nav_msgs.Path import Path -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.utils.logging_config import setup_logger - -logger = setup_logger() - - -class StraightLinePlannerConfig(ModuleConfig): - world_frame: str = "map" - num_waypoints: int = 20 - - -class StraightLinePlanner(Module): - """Emits a straight-line Path from start to goal; ignores the map.""" - - config: StraightLinePlannerConfig - - global_map: In[PointCloud2] - start_pose: In[Odometry] - goal_pose: In[Odometry] - path: Out[Path] - - def __init__(self, **kwargs: Any) -> None: - super().__init__(**kwargs) - self._latest_start: Odometry | None = None - - async def handle_global_map(self, _msg: PointCloud2) -> None: - # Naive planner: map is intentionally ignored. - return - - async def handle_start_pose(self, msg: Odometry) -> None: - self._latest_start = msg - - async def handle_goal_pose(self, msg: Odometry) -> None: - start = self._latest_start - if start is None: - logger.warning("StraightLinePlanner received goal before start; skipping") - return - path = self._straight_line(start, msg) - self.path.publish(path) - - def _straight_line(self, start: Odometry, goal: Odometry) -> Path: - n = max(2, self.config.num_waypoints) - sx, sy, sz = start.x, start.y, start.z - gx, gy, gz = goal.x, goal.y, goal.z - xs = np.linspace(sx, gx, n) - ys = np.linspace(sy, gy, n) - zs = np.linspace(sz, gz, n) - orient = [ - start.orientation.x, - start.orientation.y, - start.orientation.z, - start.orientation.w, - ] - now = time.time() - poses = [ - PoseStamped( - ts=now, - frame_id=self.config.world_frame, - position=[float(x), float(y), float(z)], - orientation=orient, - ) - for x, y, z in zip(xs, ys, zs, strict=True) - ] - return Path(ts=now, frame_id=self.config.world_frame, poses=poses) diff --git a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py index 2006901fd2..d24ef75821 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py +++ b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py @@ -16,6 +16,7 @@ from __future__ import annotations +from dataclasses import dataclass, field import time from typing import Any @@ -45,8 +46,8 @@ logger = setup_logger() -SURFACE_DILATION_PASSES = 3 -SURFACE_EROSION_PASSES = 3 +SURFACE_DILATION_PASSES = 2 +SURFACE_EROSION_PASSES = 2 NODE_SPACING_M = 2.0 NODE_WALL_BUFFER_M = 0.3 @@ -59,6 +60,34 @@ class MLSPlannerConfig(ModuleConfig): robot_height: float = 1.5 +def surface_point_xyz(ix: int, iy: int, iz: int, voxel_size: float) -> tuple[float, float, float]: + """Standing-surface coord for one cell. XY centered in the cell, Z at the cell's top face.""" + return ((ix + 0.5) * voxel_size, (iy + 0.5) * voxel_size, (iz + 1.0) * voxel_size) + + +def surface_points_xyz(cells: np.ndarray, voxel_size: float) -> np.ndarray: + """Vectorized surface_point_xyz. (N, 3) int cell indices to (N, 3) world XYZ.""" + offset = np.array([0.5 * voxel_size, 0.5 * voxel_size, voxel_size]) + return cells * voxel_size + offset + + +@dataclass +class SurfaceGraph: + """Surface-cell grid plus its waypoint-node overlay. + + place_nodes populates the first five fields. add_node_edges fills source_cells + and cell_idx_to_nid and adds edges to graph. + """ + + graph: nx.Graph + adj: csr_matrix + cell_to_idx: dict[tuple[int, int, int], int] + idx_to_cell: list[tuple[int, int, int]] + surface_lookup: dict[tuple[int, int], np.ndarray] + source_cells: np.ndarray = field(default_factory=lambda: np.zeros(0, dtype=np.int64)) + cell_idx_to_nid: dict[int, int] = field(default_factory=dict) + + def _extract_surfaces(points: np.ndarray, voxel_size: float, robot_height: float) -> np.ndarray: """For each XY column, mark cells with at least robot_height of free space above as surfaces.""" if len(points) == 0: @@ -68,59 +97,58 @@ def _extract_surfaces(points: np.ndarray, voxel_size: float, robot_height: float ix, iy, iz = indices[:, 0], indices[:, 1], indices[:, 2] order = np.lexsort((iz, iy, ix)) - sx, sy, sz = ix[order], iy[order], iz[order] + ix_sorted, iy_sorted, iz_sorted = ix[order], iy[order], iz[order] height_cells = int(np.ceil(robot_height / voxel_size)) - next_same_col = np.zeros(len(sx), dtype=bool) - next_same_col[:-1] = (sx[:-1] == sx[1:]) & (sy[:-1] == sy[1:]) + next_same_col = np.zeros(len(ix_sorted), dtype=bool) + next_same_col[:-1] = (ix_sorted[:-1] == ix_sorted[1:]) & (iy_sorted[:-1] == iy_sorted[1:]) - gap = np.empty(len(sx), dtype=np.int64) - gap[:-1] = sz[1:] - sz[:-1] + gap = np.empty(len(ix_sorted), dtype=np.int64) + gap[:-1] = iz_sorted[1:] - iz_sorted[:-1] gap[-1] = 0 is_surface = (~next_same_col) | (gap > height_cells) - surf_ix = sx[is_surface] - surf_iy = sy[is_surface] - surf_iz = sz[is_surface] + surface_ix = ix_sorted[is_surface] + surface_iy = iy_sorted[is_surface] + surface_iz = iz_sorted[is_surface] - surf_ix, surf_iy, surf_iz = _close_surface_holes( - surf_ix, surf_iy, surf_iz, SURFACE_DILATION_PASSES, SURFACE_EROSION_PASSES + surface_ix, surface_iy, surface_iz = _close_surface_holes( + surface_ix, surface_iy, surface_iz, SURFACE_DILATION_PASSES, SURFACE_EROSION_PASSES ) - x = (surf_ix.astype(np.float32) + 0.5) * voxel_size - y = (surf_iy.astype(np.float32) + 0.5) * voxel_size - z = (surf_iz.astype(np.float32) + 1.0) * voxel_size - return np.column_stack([x, y, z]).astype(np.float32) + cells = np.column_stack([surface_ix, surface_iy, surface_iz]) + return surface_points_xyz(cells, voxel_size).astype(np.float32) def _close_surface_holes( - surf_ix: np.ndarray, - surf_iy: np.ndarray, - surf_iz: np.ndarray, + surface_ix: np.ndarray, + surface_iy: np.ndarray, + surface_iz: np.ndarray, dilation_passes: int, erosion_passes: int, ) -> tuple[np.ndarray, np.ndarray, np.ndarray]: - """Do dilation then erosion on the surface map at each z level. + """Dilate then erode the surface map at each z level. - Closes a lot of small holes that are artifacts missing lidar points. + Closes small holes from missing lidar points. """ - if len(surf_ix) == 0 or (dilation_passes <= 0 and erosion_passes <= 0): - return surf_ix, surf_iy, surf_iz + if len(surface_ix) == 0 or (dilation_passes <= 0 and erosion_passes <= 0): + return surface_ix, surface_iy, surface_iz pad = max(dilation_passes, 0) new_ix: list[np.ndarray] = [] new_iy: list[np.ndarray] = [] new_iz: list[np.ndarray] = [] - for level_iz in np.unique(surf_iz): - sel = surf_iz == level_iz - lx = surf_ix[sel] - ly = surf_iy[sel] + for level_iz in np.unique(surface_iz): + sel = surface_iz == level_iz + lx = surface_ix[sel] + ly = surface_iy[sel] x0, x1 = int(lx.min()), int(lx.max()) y0, y1 = int(ly.min()), int(ly.max()) w = x1 - x0 + 1 + 2 * pad h = y1 - y0 + 1 + 2 * pad + # numpy mask is indexed (row, col) = (y, x). mask = np.zeros((h, w), dtype=bool) mask[ly - y0 + pad, lx - x0 + pad] = True if dilation_passes > 0: @@ -139,22 +167,28 @@ def _close_surface_holes( ) -def _build_surface_lookup( - sx: np.ndarray, sy: np.ndarray, sz: np.ndarray +def build_surface_lookup( + ix: np.ndarray, iy: np.ndarray, iz: np.ndarray ) -> dict[tuple[int, int], np.ndarray]: - """Group surface cells by XY column.""" + """Group surface cells by XY column. + + Public so downstream code can recompute the same lookup the planner uses. + """ by_column: dict[tuple[int, int], list[int]] = {} - for ix_, iy_, iz_ in zip(sx.tolist(), sy.tolist(), sz.tolist(), strict=True): + for ix_, iy_, iz_ in zip(ix.tolist(), iy.tolist(), iz.tolist(), strict=True): by_column.setdefault((ix_, iy_), []).append(iz_) return {key: np.array(sorted(vs), dtype=np.int64) for key, vs in by_column.items()} -def _build_surface_adjacency( +def build_surface_adjacency( surface_lookup: dict[tuple[int, int], np.ndarray], voxel_size: float, step_threshold_cells: int, ) -> tuple[csr_matrix, dict[tuple[int, int, int], int], list[tuple[int, int, int]]]: - """Sparse 8-connected adjacency over surface cells, with a per-step dz cap.""" + """Sparse 8-connected adjacency over surface cells, with a per-step dz cap. + + Public so downstream code can recompute the same adjacency the planner uses. + """ n = sum(len(zs) for zs in surface_lookup.values()) if n == 0: return csr_matrix((0, 0), dtype=np.float64), {}, [] @@ -175,11 +209,11 @@ def _build_surface_adjacency( ) cell_to_idx: dict[tuple[int, int, int], int] = {cell: i for i, cell in enumerate(idx_to_cell)} - # Pack (ix, iy) into one int64 key. Padding leaves room for dx, dy in -1, 0, 1. - ix_pos = ix - ix.min() + 1 - iy_pos = iy - iy.min() + 1 - y_range = int(iy_pos.max()) + 2 - col_key = ix_pos * y_range + iy_pos + # Pack (ix, iy) into one int64 key. Offsets leave room for dx, dy in -1, 0, 1. + ix_offset = ix - ix.min() + 1 + iy_offset = iy - iy.min() + 1 + y_stride = int(iy_offset.max()) + 2 + col_key = ix_offset * y_stride + iy_offset sort_order = np.lexsort((iz, col_key)) sorted_col_key = col_key[sort_order] @@ -189,23 +223,25 @@ def _build_surface_adjacency( col_chunks: list[np.ndarray] = [] data_chunks: list[np.ndarray] = [] for dx, dy in ((-1, -1), (-1, 0), (-1, 1), (0, -1), (0, 1), (1, -1), (1, 0), (1, 1)): - neighbor_key = (ix_pos + dx) * y_range + (iy_pos + dy) + # For each source cell, range-scan the (dx, dy) neighbor column for its surface cells, + # then keep only the dz transitions that are within the per-step limit. + neighbor_key = (ix_offset + dx) * y_stride + (iy_offset + dy) lo = np.searchsorted(sorted_col_key, neighbor_key, side="left") hi = np.searchsorted(sorted_col_key, neighbor_key, side="right") n_per_src = hi - lo total = int(n_per_src.sum()) if total == 0: continue - src_flat = np.repeat(np.arange(n), n_per_src) + src_per_candidate = np.repeat(np.arange(n), n_per_src) starts = np.zeros(n, dtype=np.int64) starts[1:] = np.cumsum(n_per_src[:-1]) - candidate_sorted_idx = lo[src_flat] + (np.arange(total) - starts[src_flat]) - dz = sorted_iz[candidate_sorted_idx] - iz[src_flat] + candidate_in_sorted = lo[src_per_candidate] + (np.arange(total) - starts[src_per_candidate]) + dz = sorted_iz[candidate_in_sorted] - iz[src_per_candidate] valid = np.abs(dz) <= step_threshold_cells if not valid.any(): continue - src_valid = src_flat[valid] - dst_valid = sort_order[candidate_sorted_idx[valid]] + src_valid = src_per_candidate[valid] + dst_valid = sort_order[candidate_in_sorted[valid]] dz_valid = dz[valid] step_cost = np.sqrt(dx * dx + dy * dy + dz_valid * dz_valid) * voxel_size row_chunks.append(src_valid) @@ -243,52 +279,64 @@ def place_nodes( node_spacing: float, wall_buffer: float, step_threshold: float, -) -> tuple[ - nx.Graph, - csr_matrix, - dict[tuple[int, int, int], int], - list[tuple[int, int, int]], - dict[tuple[int, int], np.ndarray], -]: +) -> SurfaceGraph: """Place nodes by greedy NMS on the dist-to-wall field from one Dijkstra. Run multisource Dijkstra with the surface edges as sources to find distance from wall. - Then place nodes spaced throughout based on that distance.""" + Then place nodes spaced throughout based on that distance. + """ graph = nx.Graph() if len(surface_points) == 0: - empty_adj = csr_matrix((0, 0), dtype=np.float64) - return graph, empty_adj, {}, [], {} + return SurfaceGraph( + graph=graph, + adj=csr_matrix((0, 0), dtype=np.float64), + cell_to_idx={}, + idx_to_cell=[], + surface_lookup={}, + ) sx = np.floor(surface_points[:, 0] / voxel_size).astype(np.int64) sy = np.floor(surface_points[:, 1] / voxel_size).astype(np.int64) sz = np.floor(surface_points[:, 2] / voxel_size).astype(np.int64) - surface_lookup = _build_surface_lookup(sx, sy, sz) + surface_lookup = build_surface_lookup(sx, sy, sz) step_cells = max(0, int(step_threshold / voxel_size)) - adj, cell_to_idx, idx_to_cell = _build_surface_adjacency(surface_lookup, voxel_size, step_cells) + adj, cell_to_idx, idx_to_cell = build_surface_adjacency(surface_lookup, voxel_size, step_cells) n_cells = adj.shape[0] if n_cells == 0: - return graph, adj, cell_to_idx, idx_to_cell, surface_lookup + return SurfaceGraph( + graph=graph, + adj=adj, + cell_to_idx=cell_to_idx, + idx_to_cell=idx_to_cell, + surface_lookup=surface_lookup, + ) - neighbor_counts = np.diff(adj.indptr) - boundary_indices = np.where(neighbor_counts < 8)[0] - if len(boundary_indices) == 0: - boundary_indices = np.array([0], dtype=np.int64) + # Cells with fewer than 8 surface neighbors sit on a wall or hole edge. + surface_neighbor_count = np.diff(adj.indptr) + wall_adjacent_indices = np.where(surface_neighbor_count < 8)[0] + if len(wall_adjacent_indices) == 0: + wall_adjacent_indices = np.array([0], dtype=np.int64) - dist = dijkstra(adj, indices=boundary_indices, min_only=True) + dist = dijkstra(adj, indices=wall_adjacent_indices, min_only=True) cells_arr = np.array(idx_to_cell, dtype=np.float64) - cell_positions = cells_arr * voxel_size + np.array([0.5 * voxel_size, 0.5 * voxel_size, 0.0]) + cell_positions = surface_points_xyz(cells_arr, voxel_size) candidate_mask = np.isfinite(dist) & (dist >= wall_buffer) candidate_indices = np.where(candidate_mask)[0] if len(candidate_indices) == 0: - return graph, adj, cell_to_idx, idx_to_cell, surface_lookup + return SurfaceGraph( + graph=graph, + adj=adj, + cell_to_idx=cell_to_idx, + idx_to_cell=idx_to_cell, + surface_lookup=surface_lookup, + ) order = candidate_indices[np.argsort(-dist[candidate_indices])] positions = cell_positions[order] - # kill points in batches with kd tree tree = cKDTree(positions) killed = np.zeros(len(order), dtype=bool) @@ -299,57 +347,58 @@ def place_nodes( nid = graph.number_of_nodes() graph.add_node( nid, - pos=( - (cix + 0.5) * voxel_size, - (ciy + 0.5) * voxel_size, - ciz * voxel_size, - ), + pos=surface_point_xyz(cix, ciy, ciz, voxel_size), cell=(cix, ciy, ciz), ) nearby = tree.query_ball_point(positions[i], r=node_spacing) killed[np.asarray(nearby, dtype=np.int64)] = True - return graph, adj, cell_to_idx, idx_to_cell, surface_lookup + return SurfaceGraph( + graph=graph, + adj=adj, + cell_to_idx=cell_to_idx, + idx_to_cell=idx_to_cell, + surface_lookup=surface_lookup, + ) -def add_node_edges( - graph: nx.Graph, - adj: csr_matrix, - cell_to_idx: dict[tuple[int, int, int], int], - idx_to_cell: list[tuple[int, int, int]], -) -> tuple[np.ndarray, dict[int, int]]: - """Add Voronoi-adjacency edges between placed nodes. +def add_node_edges(sg: SurfaceGraph) -> None: + """Add Voronoi-adjacency edges between placed nodes, mutating sg in place. One multi-source Dijkstra labels each cell with its nearest node. The cheapest boundary crossing per node-pair becomes an edge with the cell path on data["path"]. - Returns source_cells (per-cell owner) and cell_idx_to_nid for pose-snapping. + Also fills sg.source_cells (per-cell owner) and sg.cell_idx_to_nid for pose-snapping. """ - if graph.number_of_nodes() == 0: - return np.full(adj.shape[0], -9999, dtype=np.int64), {} + if sg.graph.number_of_nodes() == 0: + sg.source_cells = np.full(sg.adj.shape[0], -9999, dtype=np.int64) + sg.cell_idx_to_nid = {} + return - node_ids = list(graph.nodes()) + node_ids = list(sg.graph.nodes()) source_cell_indices = np.empty(len(node_ids), dtype=np.int64) cell_idx_to_nid: dict[int, int] = {} for nid in node_ids: - cell_idx = cell_to_idx[graph.nodes[nid]["cell"]] + cell_idx = sg.cell_to_idx[sg.graph.nodes[nid]["cell"]] source_cell_indices[nid] = cell_idx cell_idx_to_nid[cell_idx] = nid dist, predecessors, source_cells = dijkstra( - adj, + sg.adj, indices=source_cell_indices, min_only=True, return_predecessors=True, ) - rows = np.repeat(np.arange(adj.shape[0]), np.diff(adj.indptr)) - cols = adj.indices - weights = adj.data + rows = np.repeat(np.arange(sg.adj.shape[0]), np.diff(sg.adj.indptr)) + cols = sg.adj.indices + weights = sg.adj.data src_u = source_cells[rows] src_v = source_cells[cols] boundary = (src_u != src_v) & (src_u >= 0) & (src_v >= 0) + sg.source_cells = source_cells + sg.cell_idx_to_nid = cell_idx_to_nid if not boundary.any(): - return source_cells, cell_idx_to_nid + return b_rows = rows[boundary] b_cols = cols[boundary] @@ -357,7 +406,7 @@ def add_node_edges( b_src_u = src_u[boundary] b_src_v = src_v[boundary] - # Keep the min-cost boundary crossing per (node_a, node_b) pair. + # Keep the min-cost boundary crossing per node pair. best: dict[tuple[int, int], tuple[float, int, int]] = {} for i in range(len(b_costs)): nid_a = cell_idx_to_nid[int(b_src_u[i])] @@ -373,17 +422,19 @@ def add_node_edges( best[(nid_a, nid_b)] = (cost, u_a, u_b) for (nid_a, nid_b), (cost, u_a, u_b) in best.items(): - path_a = _walk_predecessors(predecessors, u_a, idx_to_cell) - path_b = _walk_predecessors(predecessors, u_b, idx_to_cell) + path_a = _walk_predecessors(predecessors, u_a, sg.idx_to_cell) + path_b = _walk_predecessors(predecessors, u_b, sg.idx_to_cell) path_a.reverse() full_path = np.array(path_a + path_b, dtype=np.int64) - graph.add_edge(nid_a, nid_b, weight=cost, path=full_path) + sg.graph.add_edge(nid_a, nid_b, weight=cost, path=full_path) - return source_cells, cell_idx_to_nid +class _LineSegmentsAsPath(LineSegments3D): + """Pack LineSegments3D into nav_msgs/Path until a dedicated message exists. -class _PublishableLineSegments3D(LineSegments3D): - """LineSegments3D with a Python lcm_encode. Upstream only implements decode.""" + Segment endpoints alternate as poses (p1, p2, p1, p2, ...) and traversability rides + on each pose's orientation.w. + """ def lcm_encode(self) -> bytes: lcm_msg = LCMPath() @@ -429,13 +480,12 @@ def _edges_to_segments( for i in range(len(path_cells) - 1): a = path_cells[i] b = path_cells[i + 1] - ax = (float(a[0]) + 0.5) * voxel_size - ay = (float(a[1]) + 0.5) * voxel_size - az = float(a[2]) * voxel_size - bx = (float(b[0]) + 0.5) * voxel_size - by = (float(b[1]) + 0.5) * voxel_size - bz = float(b[2]) * voxel_size - segments.append(((ax, ay, az), (bx, by, bz))) + segments.append( + ( + surface_point_xyz(int(a[0]), int(a[1]), int(a[2]), voxel_size), + surface_point_xyz(int(b[0]), int(b[1]), int(b[2]), voxel_size), + ) + ) return segments @@ -453,17 +503,14 @@ class MLSPlanner(Module): def __init__(self, **kwargs: Any) -> None: super().__init__(**kwargs) self._latest_start: Odometry | None = None - self._graph: nx.Graph | None = None - self._cell_to_idx: dict[tuple[int, int, int], int] | None = None - self._surface_lookup: dict[tuple[int, int], np.ndarray] | None = None - self._source_cells: np.ndarray | None = None - self._cell_idx_to_nid: dict[int, int] | None = None + self._surface_graph: SurfaceGraph | None = None async def handle_global_map(self, msg: PointCloud2) -> None: points, _ = msg.as_numpy() if points is None or len(points) == 0: return + # 1. Surface extraction t0 = time.perf_counter() surface_points = _extract_surfaces(points, self.config.voxel_size, self.config.robot_height) surfaces_ms = (time.perf_counter() - t0) * 1000 @@ -478,9 +525,10 @@ async def handle_global_map(self, msg: PointCloud2) -> None: surface_ms=round(surfaces_ms, 1), ) + # 2. Node placement logger.info("Placing nodes", spacing_m=NODE_SPACING_M) t1 = time.perf_counter() - graph, adj, cell_to_idx, idx_to_cell, surface_lookup = place_nodes( + sg = place_nodes( surface_points, self.config.voxel_size, node_spacing=NODE_SPACING_M, @@ -490,49 +538,50 @@ async def handle_global_map(self, msg: PointCloud2) -> None: place_ms = (time.perf_counter() - t1) * 1000 self.nodes.publish( PointCloud2.from_numpy( - _nodes_to_cloud(graph), + _nodes_to_cloud(sg.graph), frame_id=self.config.world_frame, timestamp=time.time(), ) ) logger.info( "Nodes placed", - nodes=graph.number_of_nodes(), + nodes=sg.graph.number_of_nodes(), place_ms=round(place_ms, 1), ) + # 3. Edge construction logger.info("Building edges") t2 = time.perf_counter() - source_cells, cell_idx_to_nid = add_node_edges(graph, adj, cell_to_idx, idx_to_cell) + add_node_edges(sg) edges_ms = (time.perf_counter() - t2) * 1000 logger.info( "Edges built", - edges=graph.number_of_edges(), + edges=sg.graph.number_of_edges(), edges_ms=round(edges_ms, 1), ) - self._graph = graph - self._cell_to_idx = cell_to_idx - self._surface_lookup = surface_lookup - self._source_cells = source_cells - self._cell_idx_to_nid = cell_idx_to_nid - + self._surface_graph = sg self.node_edges.publish( - _PublishableLineSegments3D( + _LineSegmentsAsPath( ts=time.time(), frame_id=self.config.world_frame, - segments=_edges_to_segments(graph, self.config.voxel_size), + segments=_edges_to_segments(sg.graph, self.config.voxel_size), ) ) async def handle_start_pose(self, msg: Odometry) -> None: self._latest_start = msg + def _publish_empty_path(self) -> None: + """Clear any previously published path so the visualizer drops the stale plan.""" + self.path.publish(Path(ts=time.time(), frame_id=self.config.world_frame, poses=[])) + async def handle_goal_pose(self, msg: Odometry) -> None: if self._latest_start is None: logger.warning("MLSPlanner received goal before start; skipping") return - if self._graph is None or self._graph.number_of_nodes() == 0: + sg = self._surface_graph + if sg is None or sg.graph.number_of_nodes() == 0: logger.warning("MLSPlanner received goal before graph was built; skipping") return @@ -540,8 +589,8 @@ async def handle_goal_pose(self, msg: Odometry) -> None: start = (self._latest_start.x, self._latest_start.y, self._latest_start.z) goal = (msg.x, msg.y, msg.z) - start_node = self._snap_pose_to_node(start) - goal_node = self._snap_pose_to_node(goal) + start_node = self._snap_pose_to_node(sg, start) + goal_node = self._snap_pose_to_node(sg, goal) if start_node is None or goal_node is None: logger.warning( "Could not snap pose to graph", @@ -550,21 +599,21 @@ async def handle_goal_pose(self, msg: Odometry) -> None: start_node=start_node, goal_node=goal_node, ) + self._publish_empty_path() return - assert self._graph is not None logger.info( "Snapped poses to graph nodes", start_pose=start, start_node=start_node, - start_node_pos=self._graph.nodes[start_node]["pos"], + start_node_pos=sg.graph.nodes[start_node]["pos"], goal_pose=goal, goal_node=goal_node, - goal_node_pos=self._graph.nodes[goal_node]["pos"], + goal_node_pos=sg.graph.nodes[goal_node]["pos"], ) try: node_seq = nx.shortest_path( - self._graph, source=start_node, target=goal_node, weight="weight" + sg.graph, source=start_node, target=goal_node, weight="weight" ) except nx.NetworkXNoPath: logger.warning( @@ -572,9 +621,10 @@ async def handle_goal_pose(self, msg: Odometry) -> None: start_node=start_node, goal_node=goal_node, ) + self._publish_empty_path() return - waypoints = self._assemble_waypoints(node_seq, start, goal) + waypoints = self._assemble_waypoints(sg, node_seq, start, goal) plan_ms = (time.perf_counter() - t0) * 1000 now = time.time() @@ -599,21 +649,15 @@ async def handle_goal_pose(self, msg: Odometry) -> None: plan_ms=round(plan_ms, 1), ) - def _snap_pose_to_node(self, pose_xyz: tuple[float, float, float]) -> int | None: + def _snap_pose_to_node( + self, sg: SurfaceGraph, pose_xyz: tuple[float, float, float] + ) -> int | None: """Snap pose to its owning node via the precomputed Voronoi labels. Finds the surface cell in the pose's column, looks up its owner in source_cells. Falls back to nearby columns if the pose's own column has no surface. """ - if ( - self._graph is None - or self._cell_to_idx is None - or self._surface_lookup is None - or self._source_cells is None - or self._cell_idx_to_nid is None - ): - return None - if self._graph.number_of_nodes() == 0: + if sg.graph.number_of_nodes() == 0: return None voxel = self.config.voxel_size @@ -623,7 +667,7 @@ def _snap_pose_to_node(self, pose_xyz: tuple[float, float, float]) -> int | None target_iz = int(np.floor(z / voxel)) - 1 tolerance_cells = int(np.ceil(self.config.robot_height / voxel)) - cell = self._best_iz_in_column(ix, iy, target_iz, tolerance_cells) + cell = self._best_iz_in_column(sg, ix, iy, target_iz, tolerance_cells) if cell is None: # Pose's column has no surface. Try nearby columns. search_radius = 5 @@ -633,7 +677,7 @@ def _snap_pose_to_node(self, pose_xyz: tuple[float, float, float]) -> int | None for diy in range(-search_radius, search_radius + 1): if dix == 0 and diy == 0: continue - c = self._best_iz_in_column(ix + dix, iy + diy, target_iz, tolerance_cells) + c = self._best_iz_in_column(sg, ix + dix, iy + diy, target_iz, tolerance_cells) if c is None: continue d2 = dix * dix + diy * diy @@ -644,21 +688,19 @@ def _snap_pose_to_node(self, pose_xyz: tuple[float, float, float]) -> int | None if cell is None: return None - cell_idx = self._cell_to_idx.get(cell) + cell_idx = sg.cell_to_idx.get(cell) if cell_idx is None: return None - owner_cell_idx = int(self._source_cells[cell_idx]) + owner_cell_idx = int(sg.source_cells[cell_idx]) if owner_cell_idx < 0: return None - return self._cell_idx_to_nid.get(owner_cell_idx) + return sg.cell_idx_to_nid.get(owner_cell_idx) def _best_iz_in_column( - self, ix: int, iy: int, target_iz: int, tolerance_cells: int + self, sg: SurfaceGraph, ix: int, iy: int, target_iz: int, tolerance_cells: int ) -> tuple[int, int, int] | None: """Surface iz in column (ix, iy) closest to target_iz, within tolerance.""" - if self._surface_lookup is None: - return None - zs = self._surface_lookup.get((ix, iy)) + zs = sg.surface_lookup.get((ix, iy)) if zs is None or len(zs) == 0: return None distances = np.abs(zs - target_iz) @@ -669,17 +711,17 @@ def _best_iz_in_column( def _assemble_waypoints( self, + sg: SurfaceGraph, node_seq: list[int], start_pose: tuple[float, float, float], goal_pose: tuple[float, float, float], ) -> list[tuple[float, float, float]]: """Chain cached edge paths into a continuous waypoint list, bracketed by the actual poses.""" - assert self._graph is not None voxel = self.config.voxel_size cells: list[tuple[int, int, int]] = [] for i in range(len(node_seq) - 1): a, b = node_seq[i], node_seq[i + 1] - edge_path: np.ndarray = self._graph[a][b]["path"] + edge_path: np.ndarray = sg.graph[a][b]["path"] # Stored path runs from min(a, b) to max(a, b). Reverse if traversing the other way. if a > b: edge_path = edge_path[::-1] @@ -689,6 +731,6 @@ def _assemble_waypoints( waypoints: list[tuple[float, float, float]] = [start_pose] for cix, ciy, ciz in cells: - waypoints.append(((cix + 0.5) * voxel, (ciy + 0.5) * voxel, (ciz + 1.0) * voxel)) + waypoints.append(surface_point_xyz(cix, ciy, ciz, voxel)) waypoints.append(goal_pose) return waypoints From 07086ead9f70de4f4fdefb23d68ac013f0e914ab Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Tue, 26 May 2026 10:26:29 -0700 Subject: [PATCH 18/55] Avoid walls --- .../modules/mls_planner/mls_planner.py | 52 ++++++++++++++++--- 1 file changed, 44 insertions(+), 8 deletions(-) diff --git a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py index d24ef75821..fbf15ea7d2 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py +++ b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py @@ -170,14 +170,14 @@ def _close_surface_holes( def build_surface_lookup( ix: np.ndarray, iy: np.ndarray, iz: np.ndarray ) -> dict[tuple[int, int], np.ndarray]: - """Group surface cells by XY column. + """Group surface cells by XY column, deduping z values per column. Public so downstream code can recompute the same lookup the planner uses. """ by_column: dict[tuple[int, int], list[int]] = {} for ix_, iy_, iz_ in zip(ix.tolist(), iy.tolist(), iz.tolist(), strict=True): by_column.setdefault((ix_, iy_), []).append(iz_) - return {key: np.array(sorted(vs), dtype=np.int64) for key, vs in by_column.items()} + return {key: np.unique(np.array(vs, dtype=np.int64)) for key, vs in by_column.items()} def build_surface_adjacency( @@ -272,6 +272,35 @@ def _walk_predecessors( return cells +def _wall_safe_adjacency( + adj: csr_matrix, + dist_to_wall: np.ndarray, + missing_neighbors: np.ndarray, + buffer_m: float, + voxel_size: float, +) -> csr_matrix: + """Scale adjacency edge costs up for cells near walls or with high edge exposure. + + Two multiplicative penalties combine per cell: + - Distance penalty (buffer_m / dist)^4 clamped to >= 1, with a small dist + floor so cells exactly on an edge stay distinguishable from cells one + voxel in. + - Exposure penalty 1 + missing/2 from the count of missing same-z neighbors, + so the corner of a stair step (more sides exposed to cliffs) costs more + than the middle of the same step's edge. + + Per-edge cost averages the two endpoints' penalties. + """ + safe_dist = np.maximum(dist_to_wall, voxel_size / 10.0) + dist_penalty = np.maximum(1.0, (buffer_m / safe_dist) ** 4) + exposure_penalty = 1.0 + missing_neighbors / 2.0 + penalty = dist_penalty * exposure_penalty + src = np.repeat(np.arange(adj.shape[0]), np.diff(adj.indptr)) + dst = adj.indices + scaled = adj.data * (penalty[src] + penalty[dst]) / 2.0 + return csr_matrix((scaled, adj.indices.copy(), adj.indptr.copy()), shape=adj.shape) + + def place_nodes( surface_points: np.ndarray, voxel_size: float, @@ -283,7 +312,9 @@ def place_nodes( """Place nodes by greedy NMS on the dist-to-wall field from one Dijkstra. Run multisource Dijkstra with the surface edges as sources to find distance from wall. - Then place nodes spaced throughout based on that distance. + Then place nodes spaced throughout based on that distance. The returned adjacency + is wall-safe: edge costs scale up within wall_buffer of any wall so add_node_edges + routes through corridor centers. """ graph = nx.Graph() if len(surface_points) == 0: @@ -313,13 +344,16 @@ def place_nodes( surface_lookup=surface_lookup, ) - # Cells with fewer than 8 surface neighbors sit on a wall or hole edge. - surface_neighbor_count = np.diff(adj.indptr) - wall_adjacent_indices = np.where(surface_neighbor_count < 8)[0] + # Detect walls and cliffs using a same-z-only adjacency. The 3D adj would miss + # cliff edges by counting cross-z neighbors, letting a landing corner that + # overlooks a stair below appear "interior". + adj_xy, _, _ = build_surface_adjacency(surface_lookup, voxel_size, 0) + xy_neighbor_count = np.diff(adj_xy.indptr) + wall_adjacent_indices = np.where(xy_neighbor_count < 8)[0] if len(wall_adjacent_indices) == 0: wall_adjacent_indices = np.array([0], dtype=np.int64) - dist = dijkstra(adj, indices=wall_adjacent_indices, min_only=True) + dist = dijkstra(adj_xy, indices=wall_adjacent_indices, min_only=True) cells_arr = np.array(idx_to_cell, dtype=np.float64) cell_positions = surface_points_xyz(cells_arr, voxel_size) @@ -353,9 +387,11 @@ def place_nodes( nearby = tree.query_ball_point(positions[i], r=node_spacing) killed[np.asarray(nearby, dtype=np.int64)] = True + missing_neighbors = 8 - xy_neighbor_count + adj_safe = _wall_safe_adjacency(adj, dist, missing_neighbors, wall_buffer, voxel_size) return SurfaceGraph( graph=graph, - adj=adj, + adj=adj_safe, cell_to_idx=cell_to_idx, idx_to_cell=idx_to_cell, surface_lookup=surface_lookup, From d9270b297c9241e61d8b4de2296ea4d4d54c858f Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Tue, 26 May 2026 10:35:26 -0700 Subject: [PATCH 19/55] Don't dilate on walls --- .../modules/mls_planner/mls_planner.py | 38 +++++++++++++++++-- 1 file changed, 35 insertions(+), 3 deletions(-) diff --git a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py index fbf15ea7d2..b22f781f8b 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py +++ b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py @@ -115,7 +115,15 @@ def _extract_surfaces(points: np.ndarray, voxel_size: float, robot_height: float surface_iz = iz_sorted[is_surface] surface_ix, surface_iy, surface_iz = _close_surface_holes( - surface_ix, surface_iy, surface_iz, SURFACE_DILATION_PASSES, SURFACE_EROSION_PASSES + surface_ix, + surface_iy, + surface_iz, + SURFACE_DILATION_PASSES, + SURFACE_EROSION_PASSES, + ix, + iy, + iz, + height_cells, ) cells = np.column_stack([surface_ix, surface_iy, surface_iz]) @@ -128,10 +136,17 @@ def _close_surface_holes( surface_iz: np.ndarray, dilation_passes: int, erosion_passes: int, + obstacle_ix: np.ndarray, + obstacle_iy: np.ndarray, + obstacle_iz: np.ndarray, + height_cells: int, ) -> tuple[np.ndarray, np.ndarray, np.ndarray]: - """Dilate then erode the surface map at each z level. + """Dilate then erode the surface map at each z level, without bridging walls. - Closes small holes from missing lidar points. + Fills small holes from missing lidar points, then rejects dilated cells whose + column has an obstacle point at z' in (level_iz, level_iz + height_cells]. + That's the same clearance check the surface extractor uses, applied to the + dilated cells so morphology can't bridge across a wall column. """ if len(surface_ix) == 0 or (dilation_passes <= 0 and erosion_passes <= 0): return surface_ix, surface_iy, surface_iz @@ -155,6 +170,23 @@ def _close_surface_holes( mask = ndimage.binary_dilation(mask, iterations=dilation_passes) if erosion_passes > 0: mask = ndimage.binary_erosion(mask, iterations=erosion_passes) + + blocking = ( + (obstacle_iz > level_iz) + & (obstacle_iz <= level_iz + height_cells) + & (obstacle_ix >= x0 - pad) + & (obstacle_ix <= x1 + pad) + & (obstacle_iy >= y0 - pad) + & (obstacle_iy <= y1 + pad) + ) + if blocking.any(): + blocked = np.zeros((h, w), dtype=bool) + blocked[ + obstacle_iy[blocking] - y0 + pad, + obstacle_ix[blocking] - x0 + pad, + ] = True + mask = mask & ~blocked + ys, xs = np.where(mask) new_ix.append(xs.astype(np.int64) + x0 - pad) new_iy.append(ys.astype(np.int64) + y0 - pad) From 44b24b25142415da554b9f9c55fdcb47f7e51d7f Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Tue, 26 May 2026 10:39:38 -0700 Subject: [PATCH 20/55] Some nice pastel colors --- dimos/navigation/nav_stack/evaluator/main.py | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/dimos/navigation/nav_stack/evaluator/main.py b/dimos/navigation/nav_stack/evaluator/main.py index 1f1fb132bc..6652dc210d 100644 --- a/dimos/navigation/nav_stack/evaluator/main.py +++ b/dimos/navigation/nav_stack/evaluator/main.py @@ -45,16 +45,16 @@ _GRAPH_Z_LIFT = 0.05 _SURFACE_COMPONENT_PALETTE = np.array( [ - [220, 50, 50], - [50, 180, 50], - [50, 100, 220], - [240, 180, 50], - [180, 50, 220], - [50, 200, 200], - [240, 130, 50], - [120, 200, 100], - [200, 60, 130], - [80, 80, 200], + [245, 140, 150], + [245, 185, 120], + [245, 225, 125], + [170, 220, 135], + [125, 220, 195], + [130, 195, 230], + [170, 160, 230], + [210, 160, 230], + [230, 160, 195], + [225, 200, 145], ], dtype=np.uint8, ) From e6633c9cb470cf22c58151d8aacaf1d81bb759c9 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Tue, 26 May 2026 12:12:31 -0700 Subject: [PATCH 21/55] Point and click for start and goal --- dimos/navigation/nav_stack/evaluator/main.py | 2 ++ .../nav_stack/evaluator/scenarios.py | 2 +- .../modules/mls_planner/mls_planner.py | 28 +++++++++++++++---- 3 files changed, 26 insertions(+), 6 deletions(-) diff --git a/dimos/navigation/nav_stack/evaluator/main.py b/dimos/navigation/nav_stack/evaluator/main.py index 6652dc210d..aba94c268e 100644 --- a/dimos/navigation/nav_stack/evaluator/main.py +++ b/dimos/navigation/nav_stack/evaluator/main.py @@ -39,6 +39,7 @@ build_surface_lookup, ) from dimos.visualization.rerun.bridge import RerunBridgeModule +from dimos.visualization.rerun.websocket_server import RerunWebSocketServer _POSE_MARKER_RADIUS = 0.4 # Small lift so graph artifacts render visibly above the surface points instead of z-fighting. @@ -128,6 +129,7 @@ def create_evaluator_blueprint() -> Blueprint: return autoconnect( Evaluator.blueprint(), MLSPlanner.blueprint(), + RerunWebSocketServer.blueprint(), RerunBridgeModule.blueprint( visual_override={ "world/start_pose": _render_start_pose, diff --git a/dimos/navigation/nav_stack/evaluator/scenarios.py b/dimos/navigation/nav_stack/evaluator/scenarios.py index 5a1acd7cc8..aba11f8d6d 100644 --- a/dimos/navigation/nav_stack/evaluator/scenarios.py +++ b/dimos/navigation/nav_stack/evaluator/scenarios.py @@ -35,7 +35,7 @@ # Walls must reach above the planner's robot_height (default 1.5m) to block surface above. _WALL_HEIGHT_M = 2.0 -MESH_PATH = "/home/andrew/Downloads/model.glb" +MESH_PATH = "/home/andrew/Downloads/19_fairdale_ave_papakura.glb" @dataclass diff --git a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py index b22f781f8b..55bd1ce40c 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py +++ b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py @@ -37,6 +37,7 @@ from dimos.core.module import Module, ModuleConfig 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.nav_msgs.LineSegments3D import LineSegments3D from dimos.msgs.nav_msgs.Odometry import Odometry @@ -49,7 +50,7 @@ SURFACE_DILATION_PASSES = 2 SURFACE_EROSION_PASSES = 2 -NODE_SPACING_M = 2.0 +NODE_SPACING_M = 1.0 NODE_WALL_BUFFER_M = 0.3 NODE_STEP_THRESHOLD_M = 0.25 @@ -563,6 +564,7 @@ class MLSPlanner(Module): global_map: In[PointCloud2] start_pose: In[Odometry] goal_pose: In[Odometry] + clicked_point: In[PointStamped] path: Out[Path] surface_map: Out[PointCloud2] nodes: Out[PointCloud2] @@ -570,8 +572,10 @@ class MLSPlanner(Module): def __init__(self, **kwargs: Any) -> None: super().__init__(**kwargs) - self._latest_start: Odometry | None = None + self._latest_start: tuple[float, float, float] | None = None self._surface_graph: SurfaceGraph | None = None + # Clicks alternate between setting the start and setting the goal+planning. + self._next_click_sets_start: bool = True async def handle_global_map(self, msg: PointCloud2) -> None: points, _ = msg.as_numpy() @@ -638,13 +642,28 @@ async def handle_global_map(self, msg: PointCloud2) -> None: ) async def handle_start_pose(self, msg: Odometry) -> None: - self._latest_start = msg + self._latest_start = (msg.x, msg.y, msg.z) def _publish_empty_path(self) -> None: """Clear any previously published path so the visualizer drops the stale plan.""" self.path.publish(Path(ts=time.time(), frame_id=self.config.world_frame, poses=[])) async def handle_goal_pose(self, msg: Odometry) -> None: + self._plan_to((msg.x, msg.y, msg.z)) + + async def handle_clicked_point(self, msg: PointStamped) -> None: + pt = (msg.x, msg.y, msg.z) + if self._next_click_sets_start: + self._latest_start = pt + self._next_click_sets_start = False + self._publish_empty_path() + logger.info("Click set start; next click will set goal", start=pt) + return + self._next_click_sets_start = True + logger.info("Click set goal", goal=pt) + self._plan_to(pt) + + def _plan_to(self, goal: tuple[float, float, float]) -> None: if self._latest_start is None: logger.warning("MLSPlanner received goal before start; skipping") return @@ -654,8 +673,7 @@ async def handle_goal_pose(self, msg: Odometry) -> None: return t0 = time.perf_counter() - start = (self._latest_start.x, self._latest_start.y, self._latest_start.z) - goal = (msg.x, msg.y, msg.z) + start = self._latest_start start_node = self._snap_pose_to_node(sg, start) goal_node = self._snap_pose_to_node(sg, goal) From dce2be9b0a10d5091c462ca5175bb25a22bb5a18 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Tue, 26 May 2026 12:31:26 -0700 Subject: [PATCH 22/55] Bug fix --- dimos/navigation/nav_stack/evaluator/main.py | 1 + 1 file changed, 1 insertion(+) diff --git a/dimos/navigation/nav_stack/evaluator/main.py b/dimos/navigation/nav_stack/evaluator/main.py index aba94c268e..03aeb22438 100644 --- a/dimos/navigation/nav_stack/evaluator/main.py +++ b/dimos/navigation/nav_stack/evaluator/main.py @@ -92,6 +92,7 @@ def _render_surface_map(voxel_size: float, msg: Any) -> Any: pts, _ = msg.as_numpy() if pts is None or len(pts) == 0: return rr.Points3D([]) + pts = pts.astype(np.float32) indices = np.floor(pts / voxel_size).astype(np.int64) ix, iy, iz = indices[:, 0], indices[:, 1], indices[:, 2] surface_lookup = build_surface_lookup(ix, iy, iz) From 94c6697e3c653e0849efca933bd678d576ac7bf0 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Tue, 26 May 2026 13:42:05 -0700 Subject: [PATCH 23/55] Color edges by cost --- dimos/navigation/nav_stack/evaluator/main.py | 23 ++++++++- .../modules/mls_planner/mls_planner.py | 50 +++++++++++++------ 2 files changed, 58 insertions(+), 15 deletions(-) diff --git a/dimos/navigation/nav_stack/evaluator/main.py b/dimos/navigation/nav_stack/evaluator/main.py index 03aeb22438..7b5dfb2ca1 100644 --- a/dimos/navigation/nav_stack/evaluator/main.py +++ b/dimos/navigation/nav_stack/evaluator/main.py @@ -122,7 +122,28 @@ def _render_nodes(msg: Any) -> Any: def _render_node_edges(msg: Any) -> Any: - return msg.to_rerun(z_offset=_GRAPH_Z_LIFT, radii=0.04) + """Color each segment by its safe-adj weight on a log-scale green->red gradient.""" + import rerun as rr + + if not msg._segments: + return rr.LineStrips3D([]) + weights = np.asarray(msg._traversability, dtype=np.float64) + log_w = np.log10(np.maximum(weights, 1e-6)) + lo, hi = float(log_w.min()), float(log_w.max()) + norm = (log_w - lo) / (hi - lo) if hi > lo else np.zeros_like(log_w) + r = (255 * norm).astype(np.uint8) + g = (255 * (1.0 - norm)).astype(np.uint8) + b = np.full_like(r, 60) + a = np.full_like(r, 220) + colors = np.column_stack([r, g, b, a]) + strips = [ + [ + [p1[0], p1[1], p1[2] + _GRAPH_Z_LIFT], + [p2[0], p2[1], p2[2] + _GRAPH_Z_LIFT], + ] + for p1, p2 in msg._segments + ] + return rr.LineStrips3D(strips, colors=colors, radii=[0.04] * len(strips)) def create_evaluator_blueprint() -> Blueprint: diff --git a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py index 55bd1ce40c..7f38aa8071 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py +++ b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py @@ -47,8 +47,8 @@ logger = setup_logger() -SURFACE_DILATION_PASSES = 2 -SURFACE_EROSION_PASSES = 2 +SURFACE_DILATION_PASSES = 3 +SURFACE_EROSION_PASSES = 3 NODE_SPACING_M = 1.0 NODE_WALL_BUFFER_M = 0.3 @@ -319,14 +319,17 @@ def _wall_safe_adjacency( floor so cells exactly on an edge stay distinguishable from cells one voxel in. - Exposure penalty 1 + missing/2 from the count of missing same-z neighbors, - so the corner of a stair step (more sides exposed to cliffs) costs more - than the middle of the same step's edge. + gated by proximity to a wall. Far from walls, a missing neighbor is almost + always just a small hole in the surface map (mesh-sampling noise), not a + real wall edge -- so the exposure factor fades to 1 as dist exceeds buffer. + Near walls, the full corner-avoidance behavior kicks in. Per-edge cost averages the two endpoints' penalties. """ safe_dist = np.maximum(dist_to_wall, voxel_size / 10.0) dist_penalty = np.maximum(1.0, (buffer_m / safe_dist) ** 4) - exposure_penalty = 1.0 + missing_neighbors / 2.0 + near_wall = np.clip(1.0 - dist_to_wall / buffer_m, 0.0, 1.0) + exposure_penalty = 1.0 + (missing_neighbors / 2.0) * near_wall penalty = dist_penalty * exposure_penalty src = np.repeat(np.arange(adj.shape[0]), np.diff(adj.indptr)) dst = adj.indices @@ -540,22 +543,39 @@ def _nodes_to_cloud(graph: nx.Graph) -> np.ndarray: def _edges_to_segments( - graph: nx.Graph, voxel_size: float -) -> list[tuple[tuple[float, float, float], tuple[float, float, float]]]: - """Emit one segment per consecutive cell pair along each edge's cached path.""" + sg: SurfaceGraph, voxel_size: float +) -> tuple[ + list[tuple[tuple[float, float, float], tuple[float, float, float]]], + list[float], +]: + """Emit one segment per consecutive cell pair on each edge, paired with its safe-adj weight.""" segments: list[tuple[tuple[float, float, float], tuple[float, float, float]]] = [] - for _, _, data in graph.edges(data=True): - path_cells: np.ndarray = data["path"] + weights: list[float] = [] + indptr = sg.adj.indptr + indices = sg.adj.indices + data = sg.adj.data + for _, _, edge_data in sg.graph.edges(data=True): + path_cells: np.ndarray = edge_data["path"] for i in range(len(path_cells) - 1): a = path_cells[i] b = path_cells[i + 1] + a_cell = (int(a[0]), int(a[1]), int(a[2])) + b_cell = (int(b[0]), int(b[1]), int(b[2])) segments.append( ( - surface_point_xyz(int(a[0]), int(a[1]), int(a[2]), voxel_size), - surface_point_xyz(int(b[0]), int(b[1]), int(b[2]), voxel_size), + surface_point_xyz(*a_cell, voxel_size), + surface_point_xyz(*b_cell, voxel_size), ) ) - return segments + u = sg.cell_to_idx[a_cell] + v = sg.cell_to_idx[b_cell] + w = 0.0 + for k in range(int(indptr[u]), int(indptr[u + 1])): + if int(indices[k]) == v: + w = float(data[k]) + break + weights.append(w) + return segments, weights class MLSPlanner(Module): @@ -633,11 +653,13 @@ async def handle_global_map(self, msg: PointCloud2) -> None: ) self._surface_graph = sg + segments, segment_weights = _edges_to_segments(sg, self.config.voxel_size) self.node_edges.publish( _LineSegmentsAsPath( ts=time.time(), frame_id=self.config.world_frame, - segments=_edges_to_segments(sg.graph, self.config.voxel_size), + segments=segments, + traversability=segment_weights, ) ) From 143cdfc06972371b7fc325d35a1098892cf26c8e Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Tue, 26 May 2026 13:49:12 -0700 Subject: [PATCH 24/55] Bug fix --- .../modules/mls_planner/mls_planner.py | 25 +++++++------------ 1 file changed, 9 insertions(+), 16 deletions(-) diff --git a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py index 7f38aa8071..cb8ce53090 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py +++ b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py @@ -548,33 +548,26 @@ def _edges_to_segments( list[tuple[tuple[float, float, float], tuple[float, float, float]]], list[float], ]: - """Emit one segment per consecutive cell pair on each edge, paired with its safe-adj weight.""" + """Emit one segment per consecutive cell pair on each edge. + + All segments belonging to the same graph edge share the edge's total weight + so the renderer can color the whole edge uniformly. + """ segments: list[tuple[tuple[float, float, float], tuple[float, float, float]]] = [] weights: list[float] = [] - indptr = sg.adj.indptr - indices = sg.adj.indices - data = sg.adj.data for _, _, edge_data in sg.graph.edges(data=True): path_cells: np.ndarray = edge_data["path"] + edge_weight = float(edge_data.get("weight", 0.0)) for i in range(len(path_cells) - 1): a = path_cells[i] b = path_cells[i + 1] - a_cell = (int(a[0]), int(a[1]), int(a[2])) - b_cell = (int(b[0]), int(b[1]), int(b[2])) segments.append( ( - surface_point_xyz(*a_cell, voxel_size), - surface_point_xyz(*b_cell, voxel_size), + surface_point_xyz(int(a[0]), int(a[1]), int(a[2]), voxel_size), + surface_point_xyz(int(b[0]), int(b[1]), int(b[2]), voxel_size), ) ) - u = sg.cell_to_idx[a_cell] - v = sg.cell_to_idx[b_cell] - w = 0.0 - for k in range(int(indptr[u]), int(indptr[u + 1])): - if int(indices[k]) == v: - w = float(data[k]) - break - weights.append(w) + weights.append(edge_weight) return segments, weights From 3ac4a16c3f7a3b86120aced53b9f4dc080321c38 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Tue, 26 May 2026 15:51:55 -0700 Subject: [PATCH 25/55] Better adjacency criteria --- .../nav_stack/modules/mls_planner/mls_planner.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py index cb8ce53090..18b4ce2b1c 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py +++ b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py @@ -380,16 +380,16 @@ def place_nodes( surface_lookup=surface_lookup, ) - # Detect walls and cliffs using a same-z-only adjacency. The 3D adj would miss - # cliff edges by counting cross-z neighbors, letting a landing corner that - # overlooks a stair below appear "interior". - adj_xy, _, _ = build_surface_adjacency(surface_lookup, voxel_size, 0) - xy_neighbor_count = np.diff(adj_xy.indptr) - wall_adjacent_indices = np.where(xy_neighbor_count < 8)[0] + neighbor_count = np.diff(adj.indptr) + wall_adjacent_indices = np.where(neighbor_count < 8)[0] if len(wall_adjacent_indices) == 0: wall_adjacent_indices = np.array([0], dtype=np.int64) - dist = dijkstra(adj_xy, indices=wall_adjacent_indices, min_only=True) + dist = dijkstra(adj, indices=wall_adjacent_indices, min_only=True) + + # Same-z adjacency feeds the exposure penalty only. + adj_xy, _, _ = build_surface_adjacency(surface_lookup, voxel_size, 0) + xy_neighbor_count = np.diff(adj_xy.indptr) cells_arr = np.array(idx_to_cell, dtype=np.float64) cell_positions = surface_points_xyz(cells_arr, voxel_size) From 8122836d71413699cbe5265d1cb4199997eec932 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Thu, 28 May 2026 16:28:48 -0700 Subject: [PATCH 26/55] Merge changes --- .../evaluator/{main.py => blueprints.py} | 44 +++++++++---------- .../nav_stack/evaluator/evaluator.py | 9 ++-- .../nav_stack/evaluator/scenarios.py | 19 ++++++-- dimos/robot/all_blueprints.py | 3 ++ 4 files changed, 43 insertions(+), 32 deletions(-) rename dimos/navigation/nav_stack/evaluator/{main.py => blueprints.py} (81%) diff --git a/dimos/navigation/nav_stack/evaluator/main.py b/dimos/navigation/nav_stack/evaluator/blueprints.py similarity index 81% rename from dimos/navigation/nav_stack/evaluator/main.py rename to dimos/navigation/nav_stack/evaluator/blueprints.py index 7b5dfb2ca1..085ba4241e 100644 --- a/dimos/navigation/nav_stack/evaluator/main.py +++ b/dimos/navigation/nav_stack/evaluator/blueprints.py @@ -12,12 +12,12 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Blueprint + entrypoint for the path-planner evaluator. +"""Blueprint for the path-planner evaluator. Wires the Evaluator and MLSPlanner together and bridges all streams to rerun. Run with:: - python -m dimos.navigation.nav_stack.evaluator.main + dimos run path-planner-eval """ from __future__ import annotations @@ -28,8 +28,7 @@ import numpy as np from scipy.sparse.csgraph import connected_components -from dimos.core.coordination.blueprints import Blueprint, autoconnect -from dimos.core.coordination.module_coordinator import ModuleCoordinator +from dimos.core.coordination.blueprints import autoconnect from dimos.navigation.nav_stack.evaluator.evaluator import Evaluator from dimos.navigation.nav_stack.modules.mls_planner.mls_planner import ( NODE_STEP_THRESHOLD_M, @@ -146,24 +145,23 @@ def _render_node_edges(msg: Any) -> Any: return rr.LineStrips3D(strips, colors=colors, radii=[0.04] * len(strips)) -def create_evaluator_blueprint() -> Blueprint: - planner_voxel = MLSPlannerConfig().voxel_size - return autoconnect( - Evaluator.blueprint(), - MLSPlanner.blueprint(), - RerunWebSocketServer.blueprint(), - RerunBridgeModule.blueprint( - visual_override={ - "world/start_pose": _render_start_pose, - "world/goal_pose": _render_goal_pose, - "world/global_map": _render_global_map, - "world/surface_map": partial(_render_surface_map, planner_voxel), - "world/nodes": _render_nodes, - "world/node_edges": _render_node_edges, - } - ), - ) +_planner_voxel = MLSPlannerConfig().voxel_size + +path_planner_eval = autoconnect( + Evaluator.blueprint(), + MLSPlanner.blueprint(), + RerunWebSocketServer.blueprint(), + RerunBridgeModule.blueprint( + visual_override={ + "world/start_pose": _render_start_pose, + "world/goal_pose": _render_goal_pose, + "world/global_map": _render_global_map, + "world/surface_map": partial(_render_surface_map, _planner_voxel), + "world/nodes": _render_nodes, + "world/node_edges": _render_node_edges, + } + ), +) -if __name__ == "__main__": - ModuleCoordinator.build(create_evaluator_blueprint()).loop() +__all__ = ["path_planner_eval"] diff --git a/dimos/navigation/nav_stack/evaluator/evaluator.py b/dimos/navigation/nav_stack/evaluator/evaluator.py index e9bbe4425c..c3b7daccba 100644 --- a/dimos/navigation/nav_stack/evaluator/evaluator.py +++ b/dimos/navigation/nav_stack/evaluator/evaluator.py @@ -49,12 +49,10 @@ class ScenarioResult: class EvaluatorConfig(ModuleConfig): - # Pause between publishing each input so the planner - # has all three before its goal-trigger fires. LCM doesn't order topics. input_publish_delay: float = 0.2 # Max seconds to wait for the planner's path reply per scenario. path_timeout: float = 2.0 - # Pause between scenarios so rerun has time to show each one distinctly. + # Pause between scenes scenario_dwell: float = 2.0 @@ -104,8 +102,6 @@ async def _run_eval(self) -> None: async def _run_one(self, scenario: PlannerScenario) -> ScenarioResult: logger.info("Scenario start", name=scenario.name, expect_path=scenario.expect_path) assert self._path_received is not None - self._latest_path = None - self._path_received.clear() now = time.time() scenario.global_map.ts = now @@ -118,6 +114,9 @@ async def _run_one(self, scenario: PlannerScenario) -> ScenarioResult: await asyncio.sleep(self.config.input_publish_delay) self.goal_pose.publish(scenario.goal_pose) + self._latest_path = None + self._path_received.clear() + try: await asyncio.wait_for(self._path_received.wait(), timeout=self.config.path_timeout) got_path = self._latest_path is not None and len(self._latest_path) > 0 diff --git a/dimos/navigation/nav_stack/evaluator/scenarios.py b/dimos/navigation/nav_stack/evaluator/scenarios.py index aba11f8d6d..62cdd50e98 100644 --- a/dimos/navigation/nav_stack/evaluator/scenarios.py +++ b/dimos/navigation/nav_stack/evaluator/scenarios.py @@ -23,6 +23,8 @@ from __future__ import annotations from dataclasses import dataclass +import os +from pathlib import Path import numpy as np @@ -30,12 +32,15 @@ from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.navigation.nav_stack.evaluator.mesh_loader import load_voxelized_mesh +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() WORLD_FRAME = "map" # Walls must reach above the planner's robot_height (default 1.5m) to block surface above. _WALL_HEIGHT_M = 2.0 -MESH_PATH = "/home/andrew/Downloads/19_fairdale_ave_papakura.glb" +MESH_PATH = os.environ.get("MESH_PATH") @dataclass @@ -142,18 +147,24 @@ def two_rooms_one_door() -> PlannerScenario: def _mesh_scenarios() -> list[PlannerScenario]: """Two scenarios on a real building mesh: ground-level traverse and a stair climb.""" - cloud = _cloud(load_voxelized_mesh(MESH_PATH)) + if MESH_PATH is None: + logger.info("MESH_PATH not set, skipping mesh scenarios") + return [] + if not Path(MESH_PATH).is_file(): + logger.warning("Mesh file not found, skipping mesh scenarios", path=MESH_PATH) + return [] + points = load_voxelized_mesh(MESH_PATH) return [ PlannerScenario( name="mesh_outside", - global_map=cloud, + global_map=_cloud(points), start_pose=_odom(-20.45, -19.85, 1.75), goal_pose=_odom(21.95, -4.25, 1.75), expect_path=True, ), PlannerScenario( name="mesh_up_the_stairs", - global_map=cloud, + global_map=_cloud(points), start_pose=_odom(7.15, -3.55, 2.05), goal_pose=_odom(5.55, -2.05, 5.65), expect_path=True, diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 3d101cca79..2ca82cab72 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -70,6 +70,7 @@ "mid360-fastlio-voxels-native": "dimos.hardware.sensors.lidar.fastlio2.fastlio_blueprints:mid360_fastlio_voxels_native", "openarm-mock-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints:openarm_mock_planner_coordinator", "openarm-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints:openarm_planner_coordinator", + "path-planner-eval": "dimos.navigation.nav_stack.evaluator.blueprints:path_planner_eval", "teleop-phone": "dimos.teleop.phone.blueprints:teleop_phone", "teleop-phone-go2": "dimos.teleop.phone.blueprints:teleop_phone_go2", "teleop-phone-go2-fleet": "dimos.teleop.phone.blueprints:teleop_phone_go2_fleet", @@ -141,6 +142,7 @@ "drone-tracking-module": "dimos.robot.drone.drone_tracking_module.DroneTrackingModule", "embedding-memory": "dimos.memory.embedding.EmbeddingMemory", "emitter-module": "dimos.utils.demo_image_encoding.EmitterModule", + "evaluator": "dimos.navigation.nav_stack.evaluator.evaluator.Evaluator", "far-planner": "dimos.navigation.nav_stack.modules.far_planner.far_planner.FarPlanner", "fast-lio2": "dimos.hardware.sensors.lidar.fastlio2.module.FastLio2", "g1-connection": "dimos.robot.unitree.g1.connection.G1Connection", @@ -168,6 +170,7 @@ "mcp-client": "dimos.agents.mcp.mcp_client.McpClient", "mcp-server": "dimos.agents.mcp.mcp_server.McpServer", "memory-module": "dimos.memory2.module.MemoryModule", + "mls-planner": "dimos.navigation.nav_stack.modules.mls_planner.mls_planner.MLSPlanner", "mock-b1-connection-module": "dimos.robot.unitree.b1.connection.MockB1ConnectionModule", "module-a": "dimos.robot.unitree.demo_error_on_name_conflicts.ModuleA", "module-b": "dimos.robot.unitree.demo_error_on_name_conflicts.ModuleB", From e9674bfc37aa5dc1e057253a54537dc210c3ca1c Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Thu, 28 May 2026 17:47:39 -0700 Subject: [PATCH 27/55] Router for start and goal clicks --- .../nav_stack/evaluator/blueprints.py | 4 ++ .../click_start_goal_router.py | 58 +++++++++++++++++++ .../modules/mls_planner/mls_planner.py | 17 +----- dimos/robot/all_blueprints.py | 1 + 4 files changed, 64 insertions(+), 16 deletions(-) create mode 100644 dimos/navigation/nav_stack/modules/click_start_goal_router/click_start_goal_router.py diff --git a/dimos/navigation/nav_stack/evaluator/blueprints.py b/dimos/navigation/nav_stack/evaluator/blueprints.py index 085ba4241e..7ed7b832da 100644 --- a/dimos/navigation/nav_stack/evaluator/blueprints.py +++ b/dimos/navigation/nav_stack/evaluator/blueprints.py @@ -30,6 +30,9 @@ from dimos.core.coordination.blueprints import autoconnect from dimos.navigation.nav_stack.evaluator.evaluator import Evaluator +from dimos.navigation.nav_stack.modules.click_start_goal_router.click_start_goal_router import ( + ClickStartGoalRouter, +) from dimos.navigation.nav_stack.modules.mls_planner.mls_planner import ( NODE_STEP_THRESHOLD_M, MLSPlanner, @@ -150,6 +153,7 @@ def _render_node_edges(msg: Any) -> Any: path_planner_eval = autoconnect( Evaluator.blueprint(), MLSPlanner.blueprint(), + ClickStartGoalRouter.blueprint(), RerunWebSocketServer.blueprint(), RerunBridgeModule.blueprint( visual_override={ diff --git a/dimos/navigation/nav_stack/modules/click_start_goal_router/click_start_goal_router.py b/dimos/navigation/nav_stack/modules/click_start_goal_router/click_start_goal_router.py new file mode 100644 index 0000000000..146769492e --- /dev/null +++ b/dimos/navigation/nav_stack/modules/click_start_goal_router/click_start_goal_router.py @@ -0,0 +1,58 @@ +# Copyright 2026 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. + +"""Alternate clicks between start and goal pose streams for downstream planners.""" + +from __future__ import annotations + +from typing import Any + +from dimos.core.module import Module, ModuleConfig +from dimos.core.stream import In, Out +from dimos.msgs.geometry_msgs.PointStamped import PointStamped +from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +class ClickStartGoalRouterConfig(ModuleConfig): + world_frame: str = "map" + + +class ClickStartGoalRouter(Module): + """Alternates between sending start and goal poses on clicks.""" + + config: ClickStartGoalRouterConfig + + clicked_point: In[PointStamped] + start_pose: Out[Odometry] + goal_pose: Out[Odometry] + + def __init__(self, **kwargs: Any) -> None: + super().__init__(**kwargs) + self._next_is_start: bool = True + + async def handle_clicked_point(self, msg: PointStamped) -> None: + pose = Pose(position=[msg.x, msg.y, msg.z], orientation=[0.0, 0.0, 0.0, 1.0]) + odom = Odometry(ts=msg.ts, frame_id=self.config.world_frame, pose=pose) + if self._next_is_start: + self._next_is_start = False + logger.info("Click set start; next click will set goal", x=msg.x, y=msg.y, z=msg.z) + self.start_pose.publish(odom) + return + self._next_is_start = True + logger.info("Click set goal", x=msg.x, y=msg.y, z=msg.z) + self.goal_pose.publish(odom) diff --git a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py index 18b4ce2b1c..88727a6bce 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py +++ b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py @@ -37,7 +37,6 @@ from dimos.core.module import Module, ModuleConfig 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.nav_msgs.LineSegments3D import LineSegments3D from dimos.msgs.nav_msgs.Odometry import Odometry @@ -577,7 +576,6 @@ class MLSPlanner(Module): global_map: In[PointCloud2] start_pose: In[Odometry] goal_pose: In[Odometry] - clicked_point: In[PointStamped] path: Out[Path] surface_map: Out[PointCloud2] nodes: Out[PointCloud2] @@ -587,8 +585,6 @@ def __init__(self, **kwargs: Any) -> None: super().__init__(**kwargs) self._latest_start: tuple[float, float, float] | None = None self._surface_graph: SurfaceGraph | None = None - # Clicks alternate between setting the start and setting the goal+planning. - self._next_click_sets_start: bool = True async def handle_global_map(self, msg: PointCloud2) -> None: points, _ = msg.as_numpy() @@ -658,6 +654,7 @@ async def handle_global_map(self, msg: PointCloud2) -> None: async def handle_start_pose(self, msg: Odometry) -> None: self._latest_start = (msg.x, msg.y, msg.z) + self._publish_empty_path() def _publish_empty_path(self) -> None: """Clear any previously published path so the visualizer drops the stale plan.""" @@ -666,18 +663,6 @@ def _publish_empty_path(self) -> None: async def handle_goal_pose(self, msg: Odometry) -> None: self._plan_to((msg.x, msg.y, msg.z)) - async def handle_clicked_point(self, msg: PointStamped) -> None: - pt = (msg.x, msg.y, msg.z) - if self._next_click_sets_start: - self._latest_start = pt - self._next_click_sets_start = False - self._publish_empty_path() - logger.info("Click set start; next click will set goal", start=pt) - return - self._next_click_sets_start = True - logger.info("Click set goal", goal=pt) - self._plan_to(pt) - def _plan_to(self, goal: tuple[float, float, float]) -> None: if self._latest_start is None: logger.warning("MLSPlanner received goal before start; skipping") diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 2ca82cab72..fe5e76c94c 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -130,6 +130,7 @@ "b1-connection-module": "dimos.robot.unitree.b1.connection.B1ConnectionModule", "camera-module": "dimos.hardware.sensors.camera.module.CameraModule", "cartesian-motion-controller": "dimos.manipulation.control.servo_control.cartesian_motion_controller.CartesianMotionController", + "click-start-goal-router": "dimos.navigation.nav_stack.modules.click_start_goal_router.click_start_goal_router.ClickStartGoalRouter", "control-coordinator": "dimos.control.coordinator.ControlCoordinator", "cost-mapper": "dimos.mapping.costmapper.CostMapper", "demo-calculator-skill": "dimos.agents.skills.demo_calculator_skill.DemoCalculatorSkill", From 125d4ee94727cb6ce1f6976ab35bb6ea306fdbf0 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Thu, 28 May 2026 18:00:18 -0700 Subject: [PATCH 28/55] Rust framework --- .gitignore | 4 + .../modules/mls_planner/mls_planner_native.py | 56 ++ .../modules/mls_planner/rust/Cargo.lock | 591 ++++++++++++++++++ .../modules/mls_planner/rust/Cargo.toml | 22 + .../modules/mls_planner/rust/src/main.rs | 137 ++++ 5 files changed, 810 insertions(+) create mode 100644 dimos/navigation/nav_stack/modules/mls_planner/mls_planner_native.py create mode 100644 dimos/navigation/nav_stack/modules/mls_planner/rust/Cargo.lock create mode 100644 dimos/navigation/nav_stack/modules/mls_planner/rust/Cargo.toml create mode 100644 dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs diff --git a/.gitignore b/.gitignore index aedee04af7..230df688aa 100644 --- a/.gitignore +++ b/.gitignore @@ -48,6 +48,10 @@ package-lock.json dist/ build/ +# Rust build artifacts +**/target/ +**/*.rs.bk + # Ignore data directory but keep .lfs subdirectory data/* !data/.lfs/ diff --git a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner_native.py b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner_native.py new file mode 100644 index 0000000000..da61e3706d --- /dev/null +++ b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner_native.py @@ -0,0 +1,56 @@ +# Copyright 2026 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. + +"""Rust multi-level surface path planner.""" + +from __future__ import annotations + +from dimos.core.native_module import NativeModule, NativeModuleConfig +from dimos.core.stream import In, Out +from dimos.msgs.nav_msgs.LineSegments3D import LineSegments3D +from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.nav_msgs.Path import Path +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + + +class MLSPlannerNativeConfig(NativeModuleConfig): + cwd: str | None = "rust" + executable: str = "target/release/mls_planner" + build_command: str | None = "cargo build --release" + stdin_config: bool = True + + world_frame: str = "map" + voxel_size: float = 0.1 + robot_height: float = 1.5 + + surface_dilation_passes: int = 3 + surface_erosion_passes: int = 3 + node_spacing_m: float = 1.0 + node_wall_buffer_m: float = 0.3 + node_step_threshold_m: float = 0.25 + + +class MLSPlannerNative(NativeModule): + """Rust-backed MLS planner.""" + + config: MLSPlannerNativeConfig + + global_map: In[PointCloud2] + start_pose: In[Odometry] + goal_pose: In[Odometry] + + path: Out[Path] + surface_map: Out[PointCloud2] + nodes: Out[PointCloud2] + node_edges: Out[LineSegments3D] diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/Cargo.lock b/dimos/navigation/nav_stack/modules/mls_planner/rust/Cargo.lock new file mode 100644 index 0000000000..81cf7183b9 --- /dev/null +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/Cargo.lock @@ -0,0 +1,591 @@ +# This file is automatically @generated by Cargo. +# It is not intended for manual editing. +version = 4 + +[[package]] +name = "ahash" +version = "0.8.12" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5a15f179cd60c4584b8a8c596927aadc462e27f2ca70c04e0071964a73ba7a75" +dependencies = [ + "cfg-if", + "getrandom", + "once_cell", + "version_check", + "zerocopy", +] + +[[package]] +name = "aho-corasick" +version = "1.1.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ddd31a130427c27518df266943a5308ed92d4b226cc639f5a8f1002816174301" +dependencies = [ + "memchr", +] + +[[package]] +name = "byteorder" +version = "1.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1fd0f2584146f6f2ef48085050886acf353beff7305ebd1ae69500e27c67f64b" + +[[package]] +name = "bytes" +version = "1.11.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1e748733b7cbc798e1434b6ac524f0c1ff2ab456fe201501e6497c8417a4fc33" + +[[package]] +name = "cfg-if" +version = "1.0.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9330f8b2ff13f34540b44e946ef35111825727b38d33286ef986142615121801" + +[[package]] +name = "dimos-lcm" +version = "0.1.0" +source = "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#e7c9428b7201cdfeadecd181c77c9e2d60a14503" +dependencies = [ + "byteorder", + "socket2 0.5.10", + "tokio", +] + +[[package]] +name = "dimos-mls-planner" +version = "0.1.0" +dependencies = [ + "ahash", + "dimos-module", + "lcm-msgs", + "serde", + "tokio", + "tracing", +] + +[[package]] +name = "dimos-module" +version = "0.1.0" +dependencies = [ + "dimos-lcm", + "dimos-module-macros", + "serde", + "serde_json", + "tokio", + "tracing", + "tracing-subscriber", +] + +[[package]] +name = "dimos-module-macros" +version = "0.1.0" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "errno" +version = "0.3.14" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "39cab71617ae0d63f51a36d69f866391735b51691dbda63cf6f96d042b63efeb" +dependencies = [ + "libc", + "windows-sys 0.61.2", +] + +[[package]] +name = "getrandom" +version = "0.3.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "899def5c37c4fd7b2664648c28120ecec138e4d395b459e5ca34f9cce2dd77fd" +dependencies = [ + "cfg-if", + "libc", + "r-efi", + "wasip2", +] + +[[package]] +name = "itoa" +version = "1.0.18" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8f42a60cbdf9a97f5d2305f08a87dc4e09308d1276d28c869c684d7777685682" + +[[package]] +name = "lazy_static" +version = "1.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bbd2bcb4c963f2ddae06a2efc7e9f3591312473c50c6685e1f298068316e66fe" + +[[package]] +name = "lcm-msgs" +version = "0.1.0" +source = "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#e7c9428b7201cdfeadecd181c77c9e2d60a14503" +dependencies = [ + "byteorder", +] + +[[package]] +name = "libc" +version = "0.2.186" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "68ab91017fe16c622486840e4c83c9a37afeff978bd239b5293d61ece587de66" + +[[package]] +name = "log" +version = "0.4.30" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "616ec5685824bcc94416c6d4a7a446eea774a31efd7062c8480ba6fd06d7a6e5" + +[[package]] +name = "matchers" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d1525a2a28c7f4fa0fc98bb91ae755d1e2d1505079e05539e35bc876b5d65ae9" +dependencies = [ + "regex-automata", +] + +[[package]] +name = "memchr" +version = "2.8.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6b947ae49db0d222b1dbc6b113ce7248a3fc3a6ca21b696717bfc000ba4484d8" + +[[package]] +name = "mio" +version = "1.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "02bd0af71c67b473010cbbc60715ee815645a4dc942899111f494b4b737d6fda" +dependencies = [ + "libc", + "wasi", + "windows-sys 0.61.2", +] + +[[package]] +name = "nu-ansi-term" +version = "0.50.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7957b9740744892f114936ab4a57b3f487491bbeafaf8083688b16841a4240e5" +dependencies = [ + "windows-sys 0.61.2", +] + +[[package]] +name = "once_cell" +version = "1.21.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9f7c3e4beb33f85d45ae3e3a1792185706c8e16d043238c593331cc7cd313b50" + +[[package]] +name = "pin-project-lite" +version = "0.2.17" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a89322df9ebe1c1578d689c92318e070967d1042b512afbe49518723f4e6d5cd" + +[[package]] +name = "proc-macro2" +version = "1.0.106" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8fd00f0bb2e90d81d1044c2b32617f68fcb9fa3bb7640c23e9c748e53fb30934" +dependencies = [ + "unicode-ident", +] + +[[package]] +name = "quote" +version = "1.0.45" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "41f2619966050689382d2b44f664f4bc593e129785a36d6ee376ddf37259b924" +dependencies = [ + "proc-macro2", +] + +[[package]] +name = "r-efi" +version = "5.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "69cdb34c158ceb288df11e18b4bd39de994f6657d83847bdffdbd7f346754b0f" + +[[package]] +name = "regex-automata" +version = "0.4.14" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6e1dd4122fc1595e8162618945476892eefca7b88c52820e74af6262213cae8f" +dependencies = [ + "aho-corasick", + "memchr", + "regex-syntax", +] + +[[package]] +name = "regex-syntax" +version = "0.8.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dc897dd8d9e8bd1ed8cdad82b5966c3e0ecae09fb1907d58efaa013543185d0a" + +[[package]] +name = "serde" +version = "1.0.228" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9a8e94ea7f378bd32cbbd37198a4a91436180c5bb472411e48b5ec2e2124ae9e" +dependencies = [ + "serde_core", + "serde_derive", +] + +[[package]] +name = "serde_core" +version = "1.0.228" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "41d385c7d4ca58e59fc732af25c3983b67ac852c1a25000afe1175de458b67ad" +dependencies = [ + "serde_derive", +] + +[[package]] +name = "serde_derive" +version = "1.0.228" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d540f220d3187173da220f885ab66608367b6574e925011a9353e4badda91d79" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "serde_json" +version = "1.0.150" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e8014e44b4736ed0538adeecded0fce2a272f22dc9578a7eb6b2d9993c74cfb9" +dependencies = [ + "itoa", + "memchr", + "serde", + "serde_core", + "zmij", +] + +[[package]] +name = "sharded-slab" +version = "0.1.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f40ca3c46823713e0d4209592e8d6e826aa57e928f09752619fc696c499637f6" +dependencies = [ + "lazy_static", +] + +[[package]] +name = "signal-hook-registry" +version = "1.4.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c4db69cba1110affc0e9f7bcd48bbf87b3f4fc7c61fc9155afd4c469eb3d6c1b" +dependencies = [ + "errno", + "libc", +] + +[[package]] +name = "smallvec" +version = "1.15.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "67b1b7a3b5fe4f1376887184045fcf45c69e92af734b7aaddc05fb777b6fbd03" + +[[package]] +name = "socket2" +version = "0.5.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e22376abed350d73dd1cd119b57ffccad95b4e585a7cda43e286245ce23c0678" +dependencies = [ + "libc", + "windows-sys 0.52.0", +] + +[[package]] +name = "socket2" +version = "0.6.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "52d1cfed4120b4d927bf7c0f86d2087a4a7d6027c906d9f9d525a80573b9be51" +dependencies = [ + "libc", + "windows-sys 0.61.2", +] + +[[package]] +name = "syn" +version = "2.0.117" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e665b8803e7b1d2a727f4023456bbbbe74da67099c585258af0ad9c5013b9b99" +dependencies = [ + "proc-macro2", + "quote", + "unicode-ident", +] + +[[package]] +name = "thread_local" +version = "1.1.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f60246a4944f24f6e018aa17cdeffb7818b76356965d03b07d6a9886e8962185" +dependencies = [ + "cfg-if", +] + +[[package]] +name = "tokio" +version = "1.52.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8fc7f01b389ac15039e4dc9531aa973a135d7a4135281b12d7c1bc79fd57fffe" +dependencies = [ + "bytes", + "libc", + "mio", + "pin-project-lite", + "signal-hook-registry", + "socket2 0.6.4", + "tokio-macros", + "windows-sys 0.61.2", +] + +[[package]] +name = "tokio-macros" +version = "2.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "385a6cb71ab9ab790c5fe8d67f1645e6c450a7ce006a33de03daa956cf70a496" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "tracing" +version = "0.1.44" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "63e71662fa4b2a2c3a26f570f037eb95bb1f85397f3cd8076caed2f026a6d100" +dependencies = [ + "pin-project-lite", + "tracing-attributes", + "tracing-core", +] + +[[package]] +name = "tracing-attributes" +version = "0.1.31" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7490cfa5ec963746568740651ac6781f701c9c5ea257c58e057f3ba8cf69e8da" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "tracing-core" +version = "0.1.36" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "db97caf9d906fbde555dd62fa95ddba9eecfd14cb388e4f491a66d74cd5fb79a" +dependencies = [ + "once_cell", + "valuable", +] + +[[package]] +name = "tracing-log" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ee855f1f400bd0e5c02d150ae5de3840039a3f54b025156404e34c23c03f47c3" +dependencies = [ + "log", + "once_cell", + "tracing-core", +] + +[[package]] +name = "tracing-serde" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "704b1aeb7be0d0a84fc9828cae51dab5970fee5088f83d1dd7ee6f6246fc6ff1" +dependencies = [ + "serde", + "tracing-core", +] + +[[package]] +name = "tracing-subscriber" +version = "0.3.23" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cb7f578e5945fb242538965c2d0b04418d38ec25c79d160cd279bf0731c8d319" +dependencies = [ + "matchers", + "nu-ansi-term", + "once_cell", + "regex-automata", + "serde", + "serde_json", + "sharded-slab", + "smallvec", + "thread_local", + "tracing", + "tracing-core", + "tracing-log", + "tracing-serde", +] + +[[package]] +name = "unicode-ident" +version = "1.0.24" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e6e4313cd5fcd3dad5cafa179702e2b244f760991f45397d14d4ebf38247da75" + +[[package]] +name = "valuable" +version = "0.1.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ba73ea9cf16a25df0c8caa16c51acb937d5712a8429db78a3ee29d5dcacd3a65" + +[[package]] +name = "version_check" +version = "0.9.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0b928f33d975fc6ad9f86c8f283853ad26bdd5b10b7f1542aa2fa15e2289105a" + +[[package]] +name = "wasi" +version = "0.11.1+wasi-snapshot-preview1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ccf3ec651a847eb01de73ccad15eb7d99f80485de043efb2f370cd654f4ea44b" + +[[package]] +name = "wasip2" +version = "1.0.3+wasi-0.2.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "20064672db26d7cdc89c7798c48a0fdfac8213434a1186e5ef29fd560ae223d6" +dependencies = [ + "wit-bindgen", +] + +[[package]] +name = "windows-link" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f0805222e57f7521d6a62e36fa9163bc891acd422f971defe97d64e70d0a4fe5" + +[[package]] +name = "windows-sys" +version = "0.52.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "282be5f36a8ce781fad8c8ae18fa3f9beff57ec1b52cb3de0789201425d9a33d" +dependencies = [ + "windows-targets", +] + +[[package]] +name = "windows-sys" +version = "0.61.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ae137229bcbd6cdf0f7b80a31df61766145077ddf49416a728b02cb3921ff3fc" +dependencies = [ + "windows-link", +] + +[[package]] +name = "windows-targets" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9b724f72796e036ab90c1021d4780d4d3d648aca59e491e6b98e725b84e99973" +dependencies = [ + "windows_aarch64_gnullvm", + "windows_aarch64_msvc", + "windows_i686_gnu", + "windows_i686_gnullvm", + "windows_i686_msvc", + "windows_x86_64_gnu", + "windows_x86_64_gnullvm", + "windows_x86_64_msvc", +] + +[[package]] +name = "windows_aarch64_gnullvm" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "32a4622180e7a0ec044bb555404c800bc9fd9ec262ec147edd5989ccd0c02cd3" + +[[package]] +name = "windows_aarch64_msvc" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "09ec2a7bb152e2252b53fa7803150007879548bc709c039df7627cabbd05d469" + +[[package]] +name = "windows_i686_gnu" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8e9b5ad5ab802e97eb8e295ac6720e509ee4c243f69d781394014ebfe8bbfa0b" + +[[package]] +name = "windows_i686_gnullvm" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0eee52d38c090b3caa76c563b86c3a4bd71ef1a819287c19d586d7334ae8ed66" + +[[package]] +name = "windows_i686_msvc" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "240948bc05c5e7c6dabba28bf89d89ffce3e303022809e73deaefe4f6ec56c66" + +[[package]] +name = "windows_x86_64_gnu" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "147a5c80aabfbf0c7d901cb5895d1de30ef2907eb21fbbab29ca94c5b08b1a78" + +[[package]] +name = "windows_x86_64_gnullvm" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "24d5b23dc417412679681396f2b49f3de8c1473deb516bd34410872eff51ed0d" + +[[package]] +name = "windows_x86_64_msvc" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "589f6da84c646204747d1270a2a5661ea66ed1cced2631d546fdfb155959f9ec" + +[[package]] +name = "wit-bindgen" +version = "0.57.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1ebf944e87a7c253233ad6766e082e3cd714b5d03812acc24c318f549614536e" + +[[package]] +name = "zerocopy" +version = "0.8.49" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bce33a6288fa3f072a8c2c7d0f2fdbb90e28298f0135c1f99b96c3db2efcc60b" +dependencies = [ + "zerocopy-derive", +] + +[[package]] +name = "zerocopy-derive" +version = "0.8.49" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8fd425244944f4ab65ccff928e7323354c5a018c75838362fdce749dfad2ee1e" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "zmij" +version = "1.0.21" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b8848ee67ecc8aedbaf3e4122217aff892639231befc6a1b58d29fff4c2cabaa" diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/Cargo.toml b/dimos/navigation/nav_stack/modules/mls_planner/rust/Cargo.toml new file mode 100644 index 0000000000..5d4c655c9b --- /dev/null +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/Cargo.toml @@ -0,0 +1,22 @@ +[package] +name = "dimos-mls-planner" +version = "0.1.0" +edition = "2021" +description = "Native Rust multi-level surface path planner for dimos" +license = "Apache-2.0" + +[[bin]] +name = "mls_planner" +path = "src/main.rs" + +[dependencies] +dimos-module = { path = "../../../../../../native/rust/dimos-module" } +lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen" } +tokio = { version = "1", features = ["rt-multi-thread", "macros", "signal"] } +serde = { version = "1", features = ["derive"] } +ahash = "0.8" +tracing = "0.1" + +[profile.release] +lto = "thin" +codegen-units = 1 diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs new file mode 100644 index 0000000000..17e3d2bc1a --- /dev/null +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs @@ -0,0 +1,137 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 + +use dimos_module::{run, Input, LcmTransport, Module, Output}; +use lcm_msgs::nav_msgs::{Odometry, Path}; +use lcm_msgs::sensor_msgs::PointCloud2; +use serde::Deserialize; +use tracing::info; + +#[allow(dead_code)] // fields populated incrementally as algorithm stages land +#[derive(Debug, Deserialize)] +#[serde(deny_unknown_fields)] +struct Config { + world_frame: String, + voxel_size: f32, + robot_height: f32, + surface_dilation_passes: u32, + surface_erosion_passes: u32, + node_spacing_m: f32, + node_wall_buffer_m: f32, + node_step_threshold_m: f32, +} + +#[allow(dead_code)] // outputs wired up incrementally as algorithm stages land +#[derive(Module)] +#[module(setup = setup)] +struct MlsPlanner { + #[input(decode = PointCloud2::decode, handler = on_global_map)] + global_map: Input, + + #[input(decode = Odometry::decode, handler = on_start_pose)] + start_pose: Input, + + #[input(decode = Odometry::decode, handler = on_goal_pose)] + goal_pose: Input, + + #[output(encode = PointCloud2::encode)] + surface_map: Output, + + #[output(encode = PointCloud2::encode)] + nodes: Output, + + #[output(encode = Path::encode)] + node_edges: Output, + + #[output(encode = Path::encode)] + path: Output, + + #[config] + config: Config, + + height_cells: i32, + step_cells: i32, +} + +impl MlsPlanner { + async fn setup(&mut self) { + let cfg = &self.config; + if !cfg.voxel_size.is_finite() || cfg.voxel_size <= 0.0 { + panic!( + "mls_planner: voxel_size must be > 0, got {}", + cfg.voxel_size + ); + } + if !cfg.robot_height.is_finite() || cfg.robot_height <= 0.0 { + panic!( + "mls_planner: robot_height must be > 0, got {}", + cfg.robot_height + ); + } + if !cfg.node_spacing_m.is_finite() || cfg.node_spacing_m <= 0.0 { + panic!( + "mls_planner: node_spacing_m must be > 0, got {}", + cfg.node_spacing_m + ); + } + if !cfg.node_wall_buffer_m.is_finite() || cfg.node_wall_buffer_m < 0.0 { + panic!( + "mls_planner: node_wall_buffer_m must be >= 0, got {}", + cfg.node_wall_buffer_m + ); + } + if !cfg.node_step_threshold_m.is_finite() || cfg.node_step_threshold_m < 0.0 { + panic!( + "mls_planner: node_step_threshold_m must be >= 0, got {}", + cfg.node_step_threshold_m + ); + } + + self.height_cells = (cfg.robot_height / cfg.voxel_size).ceil() as i32; + self.step_cells = (cfg.node_step_threshold_m / cfg.voxel_size).floor() as i32; + + info!( + world_frame = %cfg.world_frame, + voxel_size = cfg.voxel_size, + robot_height = cfg.robot_height, + height_cells = self.height_cells, + step_cells = self.step_cells, + "mls_planner ready", + ); + } + + async fn on_global_map(&mut self, msg: PointCloud2) { + let n = (msg.width as usize) * (msg.height as usize); + info!(points = n, "global_map stub: not yet implemented"); + } + + async fn on_start_pose(&mut self, msg: Odometry) { + let p = &msg.pose.pose.position; + info!( + x = p.x, + y = p.y, + z = p.z, + "start_pose stub: not yet implemented" + ); + } + + async fn on_goal_pose(&mut self, msg: Odometry) { + let p = &msg.pose.pose.position; + info!( + x = p.x, + y = p.y, + z = p.z, + "goal_pose stub: not yet implemented" + ); + } +} + +#[tokio::main] +async fn main() { + let transport = LcmTransport::new() + .await + .expect("failed to create LCM transport"); + run::(transport) + .await + .expect("mls_planner run failed"); +} From 33d80dbf5ba9bf27d6cd2440d04b72abb9a331d0 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Thu, 28 May 2026 18:09:22 -0700 Subject: [PATCH 29/55] Voxels --- .../modules/mls_planner/rust/src/main.rs | 2 + .../modules/mls_planner/rust/src/voxel.rs | 52 +++++++++++++++++++ 2 files changed, 54 insertions(+) create mode 100644 dimos/navigation/nav_stack/modules/mls_planner/rust/src/voxel.rs diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs index 17e3d2bc1a..5df387281d 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs @@ -1,6 +1,8 @@ // Copyright 2026 Dimensional Inc. // SPDX-License-Identifier: Apache-2.0 +mod voxel; + use dimos_module::{run, Input, LcmTransport, Module, Output}; use lcm_msgs::nav_msgs::{Odometry, Path}; use lcm_msgs::sensor_msgs::PointCloud2; diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/voxel.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/voxel.rs new file mode 100644 index 0000000000..302614d7d3 --- /dev/null +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/voxel.rs @@ -0,0 +1,52 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 + +//! Voxel-grid coordinate math. + +#![allow(dead_code)] // consumed incrementally by later stage modules + +pub type VoxelKey = (i32, i32, i32); + +/// XY centered in the cell, Z at the cell's top face. +#[inline] +pub fn surface_point_xyz(ix: i32, iy: i32, iz: i32, voxel_size: f32) -> (f32, f32, f32) { + ( + (ix as f32 + 0.5) * voxel_size, + (iy as f32 + 0.5) * voxel_size, + (iz as f32 + 1.0) * voxel_size, + ) +} + +#[cfg(test)] +mod tests { + use super::*; + + fn approx_eq(a: f32, b: f32) { + let eps = 1e-6; + assert!((a - b).abs() < eps, "{a} != {b} (eps {eps})"); + } + + #[test] + fn origin_cell_at_voxel_1() { + let (x, y, z) = surface_point_xyz(0, 0, 0, 1.0); + approx_eq(x, 0.5); + approx_eq(y, 0.5); + approx_eq(z, 1.0); + } + + #[test] + fn positive_cell_at_voxel_0_1() { + let (x, y, z) = surface_point_xyz(3, 2, 5, 0.1); + approx_eq(x, 0.35); + approx_eq(y, 0.25); + approx_eq(z, 0.6); + } + + #[test] + fn negative_cell() { + let (x, y, z) = surface_point_xyz(-2, -1, -3, 1.0); + approx_eq(x, -1.5); + approx_eq(y, -0.5); + approx_eq(z, -2.0); + } +} From e074bb3d78aac9eca6d89f3304410e50999f0ea8 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Thu, 28 May 2026 18:19:39 -0700 Subject: [PATCH 30/55] Adjacency --- .../modules/mls_planner/rust/src/adjacency.rs | 212 ++++++++++++++++++ .../modules/mls_planner/rust/src/main.rs | 1 + 2 files changed, 213 insertions(+) create mode 100644 dimos/navigation/nav_stack/modules/mls_planner/rust/src/adjacency.rs diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/adjacency.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/adjacency.rs new file mode 100644 index 0000000000..da4f7ce498 --- /dev/null +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/adjacency.rs @@ -0,0 +1,212 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 + +//! Per-column surface lookup and 4-connected adjacency over surface cells. +//! +//! Wall-safe cost smoothing in nodes.rs should make paths equivalent. + +#![allow(dead_code)] // consumed incrementally by later stage modules + +use ahash::AHashMap; + +use crate::voxel::VoxelKey; + +pub type SurfaceLookup = AHashMap<(i32, i32), Vec>; + +const NEIGHBORS_4: [(i32, i32); 4] = [(-1, 0), (1, 0), (0, -1), (0, 1)]; + +pub struct CsrAdjacency { + pub indptr: Vec, + pub indices: Vec, + pub data: Vec, + pub n: u32, +} + +pub struct SurfaceAdjacency { + pub adj: CsrAdjacency, + pub idx_to_cell: Vec, + pub cell_to_idx: AHashMap, +} + +/// Group cells by XY column with sorted unique iz per column. +pub fn build_surface_lookup(cells: &[VoxelKey]) -> SurfaceLookup { + let mut lookup: SurfaceLookup = AHashMap::new(); + for &(ix, iy, iz) in cells { + lookup.entry((ix, iy)).or_default().push(iz); + } + for zs in lookup.values_mut() { + zs.sort_unstable(); + zs.dedup(); + } + lookup +} + +/// 4-connected XY adjacency with per-step dz cap. Cell ordering is +/// lex-sorted by column then iz so the output is deterministic across runs. +pub fn build_surface_adjacency( + surface_lookup: &SurfaceLookup, + voxel_size: f32, + step_threshold_cells: i32, +) -> SurfaceAdjacency { + let mut columns: Vec<(i32, i32)> = surface_lookup.keys().copied().collect(); + columns.sort_unstable(); + + let mut idx_to_cell: Vec = Vec::new(); + for &(ix, iy) in &columns { + for &iz in &surface_lookup[&(ix, iy)] { + idx_to_cell.push((ix, iy, iz)); + } + } + let n = idx_to_cell.len(); + let cell_to_idx: AHashMap = idx_to_cell + .iter() + .enumerate() + .map(|(i, &c)| (c, i as u32)) + .collect(); + + let mut indptr: Vec = Vec::with_capacity(n + 1); + indptr.push(0); + let mut indices: Vec = Vec::new(); + let mut data: Vec = Vec::new(); + + for &(ix, iy, iz) in &idx_to_cell { + for (dx, dy) in NEIGHBORS_4 { + let Some(zs) = surface_lookup.get(&(ix + dx, iy + dy)) else { + continue; + }; + for &nz in zs { + let dz = nz - iz; + if dz.abs() > step_threshold_cells { + continue; + } + let dst = cell_to_idx[&(ix + dx, iy + dy, nz)]; + let cost = ((dx * dx + dy * dy + dz * dz) as f32).sqrt() * voxel_size; + indices.push(dst); + data.push(cost); + } + } + indptr.push(indices.len() as u32); + } + + SurfaceAdjacency { + adj: CsrAdjacency { + indptr, + indices, + data, + n: n as u32, + }, + idx_to_cell, + cell_to_idx, + } +} + +#[cfg(test)] +mod tests { + use super::*; + + const VOXEL: f32 = 0.1; + + fn approx_eq(a: f32, b: f32) { + let eps = 1e-5; + assert!((a - b).abs() < eps, "{a} != {b} (eps {eps})"); + } + + fn edges(sa: &SurfaceAdjacency) -> Vec<(VoxelKey, VoxelKey, f32)> { + let mut out = Vec::new(); + for src in 0..sa.adj.n as usize { + let lo = sa.adj.indptr[src] as usize; + let hi = sa.adj.indptr[src + 1] as usize; + for k in lo..hi { + let dst = sa.adj.indices[k] as usize; + out.push((sa.idx_to_cell[src], sa.idx_to_cell[dst], sa.adj.data[k])); + } + } + out + } + + #[test] + fn empty_input_yields_empty_adjacency() { + let lookup = build_surface_lookup(&[]); + let sa = build_surface_adjacency(&lookup, VOXEL, 2); + assert_eq!(sa.adj.n, 0); + assert_eq!(sa.adj.indptr, vec![0]); + assert!(sa.adj.indices.is_empty()); + assert!(sa.adj.data.is_empty()); + assert!(sa.idx_to_cell.is_empty()); + } + + #[test] + fn single_cell_has_no_edges() { + let lookup = build_surface_lookup(&[(0, 0, 0)]); + let sa = build_surface_adjacency(&lookup, VOXEL, 2); + assert_eq!(sa.adj.n, 1); + assert_eq!(sa.adj.indptr, vec![0, 0]); + assert!(sa.adj.indices.is_empty()); + } + + #[test] + fn same_z_neighbors_are_bidirectional() { + let lookup = build_surface_lookup(&[(0, 0, 0), (1, 0, 0)]); + let sa = build_surface_adjacency(&lookup, VOXEL, 2); + assert_eq!(sa.adj.indices.len(), 2); + for e in edges(&sa) { + approx_eq(e.2, VOXEL); + } + } + + #[test] + fn diagonal_not_connected_under_4_connectivity() { + let lookup = build_surface_lookup(&[(0, 0, 0), (1, 1, 0)]); + let sa = build_surface_adjacency(&lookup, VOXEL, 2); + assert!( + sa.adj.indices.is_empty(), + "diagonal must not connect under 4-connectivity" + ); + } + + #[test] + fn step_threshold_blocks_large_dz() { + let lookup = build_surface_lookup(&[(0, 0, 0), (1, 0, 5)]); + let sa = build_surface_adjacency(&lookup, VOXEL, 2); + assert!( + sa.adj.indices.is_empty(), + "dz=5 must not connect when step_threshold=2" + ); + } + + #[test] + fn step_within_threshold_uses_3d_distance() { + let lookup = build_surface_lookup(&[(0, 0, 0), (1, 0, 1)]); + let sa = build_surface_adjacency(&lookup, VOXEL, 2); + assert_eq!(sa.adj.indices.len(), 2); + let expected = (2.0_f32).sqrt() * VOXEL; + for e in edges(&sa) { + approx_eq(e.2, expected); + } + } + + #[test] + fn same_column_cells_are_not_self_connected() { + let lookup = build_surface_lookup(&[(0, 0, 0), (0, 0, 5)]); + let sa = build_surface_adjacency(&lookup, VOXEL, 10); + assert!(sa.adj.indices.is_empty()); + } + + #[test] + fn plus_pattern_center_has_four_neighbors() { + let cells = vec![(0, 0, 0), (1, 0, 0), (-1, 0, 0), (0, 1, 0), (0, -1, 0)]; + let lookup = build_surface_lookup(&cells); + let sa = build_surface_adjacency(&lookup, VOXEL, 2); + let center_idx = sa.cell_to_idx[&(0, 0, 0)] as usize; + let lo = sa.adj.indptr[center_idx] as usize; + let hi = sa.adj.indptr[center_idx + 1] as usize; + assert_eq!(hi - lo, 4); + } + + #[test] + fn deduplicates_repeated_cells() { + let lookup = build_surface_lookup(&[(0, 0, 0), (0, 0, 0), (1, 0, 0)]); + let sa = build_surface_adjacency(&lookup, VOXEL, 2); + assert_eq!(sa.adj.n, 2); + } +} diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs index 5df387281d..1069ab9dad 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs @@ -1,6 +1,7 @@ // Copyright 2026 Dimensional Inc. // SPDX-License-Identifier: Apache-2.0 +mod adjacency; mod voxel; use dimos_module::{run, Input, LcmTransport, Module, Output}; From 2c833994ea63a1e49e5f59eb33b1383d65bce561 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Thu, 28 May 2026 18:32:22 -0700 Subject: [PATCH 31/55] Surfaces --- .../modules/mls_planner/rust/Cargo.lock | 438 +++++++++++++++++- .../modules/mls_planner/rust/Cargo.toml | 2 + .../modules/mls_planner/rust/src/main.rs | 1 + .../modules/mls_planner/rust/src/surfaces.rs | 250 ++++++++++ 4 files changed, 690 insertions(+), 1 deletion(-) create mode 100644 dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/Cargo.lock b/dimos/navigation/nav_stack/modules/mls_planner/rust/Cargo.lock index 81cf7183b9..77f0f06e98 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/Cargo.lock +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/Cargo.lock @@ -2,6 +2,22 @@ # It is not intended for manual editing. version = 4 +[[package]] +name = "ab_glyph" +version = "0.2.32" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "01c0457472c38ea5bd1c3b5ada5e368271cb550be7a4ca4a0b4634e9913f6cc2" +dependencies = [ + "ab_glyph_rasterizer", + "owned_ttf_parser", +] + +[[package]] +name = "ab_glyph_rasterizer" +version = "0.1.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "366ffbaa4442f4684d91e2cd7c5ea7c4ed8add41959a31447066e279e432b618" + [[package]] name = "ahash" version = "0.8.12" @@ -9,7 +25,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "5a15f179cd60c4584b8a8c596927aadc462e27f2ca70c04e0071964a73ba7a75" dependencies = [ "cfg-if", - "getrandom", + "getrandom 0.3.4", "once_cell", "version_check", "zerocopy", @@ -24,12 +40,45 @@ dependencies = [ "memchr", ] +[[package]] +name = "approx" +version = "0.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cab112f0a86d568ea0e627cc1d6be74a1e9cd55214684db5561995f6dad897c6" +dependencies = [ + "num-traits", +] + +[[package]] +name = "autocfg" +version = "1.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f2032f911046de80f0a198e0901378627c33f59ea0ac00e363d481118bd70a53" + +[[package]] +name = "bumpalo" +version = "3.20.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "72f5acc6cb2ba439de613abc23857ec3d78374d8ed5ac84e9d11336e87da8649" + +[[package]] +name = "bytemuck" +version = "1.25.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c8efb64bd706a16a1bdde310ae86b351e4d21550d98d056f22f8a7f7a2183fec" + [[package]] name = "byteorder" version = "1.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "1fd0f2584146f6f2ef48085050886acf353beff7305ebd1ae69500e27c67f64b" +[[package]] +name = "byteorder-lite" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8f1fe948ff07f4bd06c30984e69f5b4899c516a3ef74f34df92a2df2ab535495" + [[package]] name = "bytes" version = "1.11.1" @@ -58,6 +107,8 @@ version = "0.1.0" dependencies = [ "ahash", "dimos-module", + "image", + "imageproc", "lcm-msgs", "serde", "tokio", @@ -86,6 +137,12 @@ dependencies = [ "syn", ] +[[package]] +name = "either" +version = "1.16.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "91622ff5e7162018101f2fea40d6ebf4a78bbe5a49736a2020649edf9693679e" + [[package]] name = "errno" version = "0.3.14" @@ -96,6 +153,43 @@ dependencies = [ "windows-sys 0.61.2", ] +[[package]] +name = "futures-core" +version = "0.3.32" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7e3450815272ef58cec6d564423f6e755e25379b217b0bc688e295ba24df6b1d" + +[[package]] +name = "futures-task" +version = "0.3.32" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "037711b3d59c33004d3856fbdc83b99d4ff37a24768fa1be9ce3538a1cde4393" + +[[package]] +name = "futures-util" +version = "0.3.32" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "389ca41296e6190b48053de0321d02a77f32f8a5d2461dd38762c0593805c6d6" +dependencies = [ + "futures-core", + "futures-task", + "pin-project-lite", + "slab", +] + +[[package]] +name = "getrandom" +version = "0.2.17" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ff2abc00be7fca6ebc474524697ae276ad847ad0a6b3faa4bcb027e9a4614ad0" +dependencies = [ + "cfg-if", + "js-sys", + "libc", + "wasi", + "wasm-bindgen", +] + [[package]] name = "getrandom" version = "0.3.4" @@ -108,12 +202,62 @@ dependencies = [ "wasip2", ] +[[package]] +name = "image" +version = "0.25.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "85ab80394333c02fe689eaf900ab500fbd0c2213da414687ebf995a65d5a6104" +dependencies = [ + "bytemuck", + "byteorder-lite", + "moxcms", + "num-traits", +] + +[[package]] +name = "imageproc" +version = "0.25.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "602b4e8a4cc3e98372b766cd184ab532999bc0e839b7469e759511ccabc65d77" +dependencies = [ + "ab_glyph", + "approx", + "getrandom 0.2.17", + "image", + "itertools", + "nalgebra", + "num", + "rand", + "rand_distr", +] + +[[package]] +name = "itertools" +version = "0.12.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ba291022dbbd398a455acf126c1e341954079855bc60dfdda641363bd6922569" +dependencies = [ + "either", +] + [[package]] name = "itoa" version = "1.0.18" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "8f42a60cbdf9a97f5d2305f08a87dc4e09308d1276d28c869c684d7777685682" +[[package]] +name = "js-sys" +version = "0.3.99" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "142bc4740e452c1e57ade0cbc129f139c9093e354346f0872ef985f4f5cf5f11" +dependencies = [ + "cfg-if", + "futures-util", + "once_cell", + "wasm-bindgen", +] + [[package]] name = "lazy_static" version = "1.5.0" @@ -134,6 +278,12 @@ version = "0.2.186" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "68ab91017fe16c622486840e4c83c9a37afeff978bd239b5293d61ece587de66" +[[package]] +name = "libm" +version = "0.2.16" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b6d2cec3eae94f9f509c767b45932f1ada8350c4bdb85af2fcab4a3c14807981" + [[package]] name = "log" version = "0.4.30" @@ -149,6 +299,16 @@ dependencies = [ "regex-automata", ] +[[package]] +name = "matrixmultiply" +version = "0.3.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a06de3016e9fae57a36fd14dba131fccf49f74b40b7fbdb472f96e361ec71a08" +dependencies = [ + "autocfg", + "rawpointer", +] + [[package]] name = "memchr" version = "2.8.1" @@ -166,6 +326,31 @@ dependencies = [ "windows-sys 0.61.2", ] +[[package]] +name = "moxcms" +version = "0.8.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bb85c154ba489f01b25c0d36ae69a87e4a1c73a72631fc6c0eb6dde34a73e44b" +dependencies = [ + "num-traits", + "pxfm", +] + +[[package]] +name = "nalgebra" +version = "0.32.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7b5c17de023a86f59ed79891b2e5d5a94c705dbe904a5b5c9c952ea6221b03e4" +dependencies = [ + "approx", + "matrixmultiply", + "num-complex", + "num-rational", + "num-traits", + "simba", + "typenum", +] + [[package]] name = "nu-ansi-term" version = "0.50.3" @@ -175,18 +360,116 @@ dependencies = [ "windows-sys 0.61.2", ] +[[package]] +name = "num" +version = "0.4.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "35bd024e8b2ff75562e5f34e7f4905839deb4b22955ef5e73d2fea1b9813cb23" +dependencies = [ + "num-bigint", + "num-complex", + "num-integer", + "num-iter", + "num-rational", + "num-traits", +] + +[[package]] +name = "num-bigint" +version = "0.4.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a5e44f723f1133c9deac646763579fdb3ac745e418f2a7af9cd0c431da1f20b9" +dependencies = [ + "num-integer", + "num-traits", +] + +[[package]] +name = "num-complex" +version = "0.4.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "73f88a1307638156682bada9d7604135552957b7818057dcef22705b4d509495" +dependencies = [ + "num-traits", +] + +[[package]] +name = "num-integer" +version = "0.1.46" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7969661fd2958a5cb096e56c8e1ad0444ac2bbcd0061bd28660485a44879858f" +dependencies = [ + "num-traits", +] + +[[package]] +name = "num-iter" +version = "0.1.45" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1429034a0490724d0075ebb2bc9e875d6503c3cf69e235a8941aa757d83ef5bf" +dependencies = [ + "autocfg", + "num-integer", + "num-traits", +] + +[[package]] +name = "num-rational" +version = "0.4.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f83d14da390562dca69fc84082e73e548e1ad308d24accdedd2720017cb37824" +dependencies = [ + "num-bigint", + "num-integer", + "num-traits", +] + +[[package]] +name = "num-traits" +version = "0.2.19" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "071dfc062690e90b734c0b2273ce72ad0ffa95f0c74596bc250dcfd960262841" +dependencies = [ + "autocfg", + "libm", +] + [[package]] name = "once_cell" version = "1.21.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "9f7c3e4beb33f85d45ae3e3a1792185706c8e16d043238c593331cc7cd313b50" +[[package]] +name = "owned_ttf_parser" +version = "0.25.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "36820e9051aca1014ddc75770aab4d68bc1e9e632f0f5627c4086bc216fb583b" +dependencies = [ + "ttf-parser", +] + +[[package]] +name = "paste" +version = "1.0.15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "57c0d7b74b563b49d38dae00a0c37d4d6de9b432382b2892f0574ddcae73fd0a" + [[package]] name = "pin-project-lite" version = "0.2.17" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a89322df9ebe1c1578d689c92318e070967d1042b512afbe49518723f4e6d5cd" +[[package]] +name = "ppv-lite86" +version = "0.2.21" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "85eae3c4ed2f50dcfe72643da4befc30deadb458a9b590d720cde2f2b1e97da9" +dependencies = [ + "zerocopy", +] + [[package]] name = "proc-macro2" version = "1.0.106" @@ -196,6 +479,12 @@ dependencies = [ "unicode-ident", ] +[[package]] +name = "pxfm" +version = "0.1.29" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e0c5ccf5294c6ccd63a74f1565028353830a9c2f5eb0c682c355c471726a6e3f" + [[package]] name = "quote" version = "1.0.45" @@ -211,6 +500,52 @@ version = "5.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "69cdb34c158ceb288df11e18b4bd39de994f6657d83847bdffdbd7f346754b0f" +[[package]] +name = "rand" +version = "0.8.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5ca0ecfa931c29007047d1bc58e623ab12e5590e8c7cc53200d5202b69266d8a" +dependencies = [ + "libc", + "rand_chacha", + "rand_core", +] + +[[package]] +name = "rand_chacha" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e6c10a63a0fa32252be49d21e7709d4d4baf8d231c2dbce1eaa8141b9b127d88" +dependencies = [ + "ppv-lite86", + "rand_core", +] + +[[package]] +name = "rand_core" +version = "0.6.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c" +dependencies = [ + "getrandom 0.2.17", +] + +[[package]] +name = "rand_distr" +version = "0.4.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "32cb0b9bc82b0a0876c2dd994a7e7a2683d3e7390ca40e6886785ef0c7e3ee31" +dependencies = [ + "num-traits", + "rand", +] + +[[package]] +name = "rawpointer" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "60a357793950651c4ed0f3f52338f53b2f809f32d83a07f72909fa13e4c6c1e3" + [[package]] name = "regex-automata" version = "0.4.14" @@ -228,6 +563,21 @@ version = "0.8.10" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "dc897dd8d9e8bd1ed8cdad82b5966c3e0ecae09fb1907d58efaa013543185d0a" +[[package]] +name = "rustversion" +version = "1.0.22" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b39cdef0fa800fc44525c84ccb54a029961a8215f9619753635a9c0d2538d46d" + +[[package]] +name = "safe_arch" +version = "0.7.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "96b02de82ddbe1b636e6170c21be622223aea188ef2e139be0a5b219ec215323" +dependencies = [ + "bytemuck", +] + [[package]] name = "serde" version = "1.0.228" @@ -290,6 +640,25 @@ dependencies = [ "libc", ] +[[package]] +name = "simba" +version = "0.8.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "061507c94fc6ab4ba1c9a0305018408e312e17c041eb63bef8aa726fa33aceae" +dependencies = [ + "approx", + "num-complex", + "num-traits", + "paste", + "wide", +] + +[[package]] +name = "slab" +version = "0.4.12" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0c790de23124f9ab44544d7ac05d60440adc586479ce501c1d6d7da3cd8c9cf5" + [[package]] name = "smallvec" version = "1.15.1" @@ -437,6 +806,18 @@ dependencies = [ "tracing-serde", ] +[[package]] +name = "ttf-parser" +version = "0.25.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d2df906b07856748fa3f6e0ad0cbaa047052d4a7dd609e231c4f72cee8c36f31" + +[[package]] +name = "typenum" +version = "1.20.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "40ce102ab67701b8526c123c1bab5cbe42d7040ccfd0f64af1a385808d2f43de" + [[package]] name = "unicode-ident" version = "1.0.24" @@ -470,6 +851,61 @@ dependencies = [ "wit-bindgen", ] +[[package]] +name = "wasm-bindgen" +version = "0.2.122" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3ed04576f974d2b2fba0f38c51dbc5518011e38c36bf1143164be765528fd409" +dependencies = [ + "cfg-if", + "once_cell", + "rustversion", + "wasm-bindgen-macro", + "wasm-bindgen-shared", +] + +[[package]] +name = "wasm-bindgen-macro" +version = "0.2.122" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "916151b09da36bd82f6615cbf3a419e2f0ba23a03c6160e8e92eb6bd4aa1dec6" +dependencies = [ + "quote", + "wasm-bindgen-macro-support", +] + +[[package]] +name = "wasm-bindgen-macro-support" +version = "0.2.122" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "299047362ccbfce148b67ab7e73349f77748e00c8296f9542adfad2ad82c5c5e" +dependencies = [ + "bumpalo", + "proc-macro2", + "quote", + "syn", + "wasm-bindgen-shared", +] + +[[package]] +name = "wasm-bindgen-shared" +version = "0.2.122" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9a929b2c61f11ba3e9bc35b50c1f25cb38e0e892c0c231ae2b8cf78d5dad4437" +dependencies = [ + "unicode-ident", +] + +[[package]] +name = "wide" +version = "0.7.33" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0ce5da8ecb62bcd8ec8b7ea19f69a51275e91299be594ea5cc6ef7819e16cd03" +dependencies = [ + "bytemuck", + "safe_arch", +] + [[package]] name = "windows-link" version = "0.2.1" diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/Cargo.toml b/dimos/navigation/nav_stack/modules/mls_planner/rust/Cargo.toml index 5d4c655c9b..a64e56cb40 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/Cargo.toml +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/Cargo.toml @@ -16,6 +16,8 @@ tokio = { version = "1", features = ["rt-multi-thread", "macros", "signal"] } serde = { version = "1", features = ["derive"] } ahash = "0.8" tracing = "0.1" +image = { version = "0.25", default-features = false } +imageproc = { version = "0.25", default-features = false } [profile.release] lto = "thin" diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs index 1069ab9dad..f50114035b 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs @@ -2,6 +2,7 @@ // SPDX-License-Identifier: Apache-2.0 mod adjacency; +mod surfaces; mod voxel; use dimos_module::{run, Input, LcmTransport, Module, Output}; diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs new file mode 100644 index 0000000000..6f432d20ee --- /dev/null +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs @@ -0,0 +1,250 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 + +//! Surface extraction: voxelize a cloud, mark cells with robot-height clearance +//! above as standable, then morphologically close per-z-level holes without +//! letting closing bridge across walls. + +#![allow(dead_code)] // consumed incrementally + +use ahash::{AHashMap, AHashSet}; +use image::{GrayImage, Luma}; +use imageproc::distance_transform::Norm; +use imageproc::morphology::{dilate, erode}; + +use crate::voxel::VoxelKey; + +const ON: Luma = Luma([255]); +const OFF: Luma = Luma([0]); + +#[inline] +fn voxelize(p: (f32, f32, f32), voxel_size: f32) -> VoxelKey { + let inv = 1.0 / voxel_size; + ( + (p.0 * inv).floor() as i32, + (p.1 * inv).floor() as i32, + (p.2 * inv).floor() as i32, + ) +} + +/// A cell (ix, iy, iz) is standable iff its column has no obstacle +/// within the height of the robot. +fn is_standable( + ix: i32, + iy: i32, + iz: i32, + obstacles: &AHashSet, + height_cells: i32, +) -> bool { + for dz in 1..=height_cells { + if obstacles.contains(&(ix, iy, iz + dz)) { + return false; + } + } + true +} + +/// Extract standable cells from a point cloud, then close small holes. +/// +/// Returns cell indices. +pub fn extract_surfaces( + points: &[(f32, f32, f32)], + voxel_size: f32, + height_cells: i32, + dilation_passes: u32, + erosion_passes: u32, +) -> Vec { + if points.is_empty() { + return Vec::new(); + } + + let obstacles: AHashSet = points.iter().map(|&p| voxelize(p, voxel_size)).collect(); + + let mut by_col: AHashMap<(i32, i32), Vec> = AHashMap::new(); + for &(ix, iy, iz) in &obstacles { + by_col.entry((ix, iy)).or_default().push(iz); + } + + let mut standable: Vec = Vec::new(); + for ((ix, iy), zs) in by_col.iter_mut() { + zs.sort_unstable(); + for w in zs.windows(2) { + if w[1] - w[0] > height_cells { + standable.push((*ix, *iy, w[0])); + } + } + if let Some(&last_iz) = zs.last() { + standable.push((*ix, *iy, last_iz)); + } + } + + close_surface_holes( + standable, + &obstacles, + dilation_passes, + erosion_passes, + height_cells, + ) +} + +fn close_surface_holes( + standable: Vec, + obstacles: &AHashSet, + dilation_passes: u32, + erosion_passes: u32, + height_cells: i32, +) -> Vec { + if standable.is_empty() || (dilation_passes == 0 && erosion_passes == 0) { + return standable; + } + + let mut by_z: AHashMap> = AHashMap::new(); + for &(ix, iy, iz) in &standable { + by_z.entry(iz).or_default().push((ix, iy)); + } + + let mut out = Vec::new(); + for (iz, xys) in by_z { + out.extend(close_at_z( + &xys, + iz, + obstacles, + dilation_passes, + erosion_passes, + height_cells, + )); + } + out +} + +fn close_at_z( + xys: &[(i32, i32)], + iz: i32, + obstacles: &AHashSet, + dilation_passes: u32, + erosion_passes: u32, + height_cells: i32, +) -> Vec { + let pad = dilation_passes as i32; + let mut min_x = i32::MAX; + let mut max_x = i32::MIN; + let mut min_y = i32::MAX; + let mut max_y = i32::MIN; + for &(ix, iy) in xys { + min_x = min_x.min(ix); + max_x = max_x.max(ix); + min_y = min_y.min(iy); + max_y = max_y.max(iy); + } + + let w = (max_x - min_x + 1 + 2 * pad) as u32; + let h = (max_y - min_y + 1 + 2 * pad) as u32; + let x0 = min_x - pad; + let y0 = min_y - pad; + + let mut img = GrayImage::from_pixel(w, h, OFF); + for &(ix, iy) in xys { + img.put_pixel((ix - x0) as u32, (iy - y0) as u32, ON); + } + + // L1 norm for 4 neighbor connectivity + if dilation_passes > 0 { + img = dilate(&img, Norm::L1, dilation_passes as u8); + } + if erosion_passes > 0 { + img = erode(&img, Norm::L1, erosion_passes as u8); + } + + let mut out = Vec::new(); + for py in 0..h { + for px in 0..w { + if img.get_pixel(px, py).0[0] == 0 { + continue; + } + let ix = x0 + px as i32; + let iy = y0 + py as i32; + if !is_standable(ix, iy, iz, obstacles, height_cells) { + continue; + } + out.push((ix, iy, iz)); + } + } + out +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn empty_input() { + assert!(extract_surfaces(&[], 1.0, 5, 0, 0).is_empty()); + } + + #[test] + fn single_point_is_topmost_surface() { + let s = extract_surfaces(&[(0.5, 0.5, 0.5)], 1.0, 5, 0, 0); + assert_eq!(s, vec![(0, 0, 0)]); + } + + #[test] + fn stacked_points_within_headroom_only_topmost_is_surface() { + let pts: Vec<(f32, f32, f32)> = (0..5).map(|z| (0.5, 0.5, z as f32 + 0.5)).collect(); + let s = extract_surfaces(&pts, 1.0, 5, 0, 0); + assert_eq!(s, vec![(0, 0, 4)]); + } + + #[test] + fn gap_larger_than_headroom_makes_lower_cell_standable() { + // Obstacles at iz=0 and iz=10 with height_cells=5. Lower cell has gap=10 > 5. + let pts: Vec<(f32, f32, f32)> = vec![(0.5, 0.5, 0.5), (0.5, 0.5, 10.5)]; + let mut s = extract_surfaces(&pts, 1.0, 5, 0, 0); + s.sort(); + assert_eq!(s, vec![(0, 0, 0), (0, 0, 10)]); + } + + #[test] + fn morphological_closing_fills_center_hole() { + // Ring of 8 cells around (0,0) at iz=0, no obstacles above. + let mut pts: Vec<(f32, f32, f32)> = Vec::new(); + for (dx, dy) in [ + (-1, -1), + (-1, 0), + (-1, 1), + (0, -1), + (0, 1), + (1, -1), + (1, 0), + (1, 1), + ] { + pts.push((dx as f32 + 0.5, dy as f32 + 0.5, 0.5)); + } + let s = extract_surfaces(&pts, 1.0, 5, 3, 3); + assert!( + s.contains(&(0, 0, 0)), + "closing should fill the center hole" + ); + } + + #[test] + fn closing_does_not_bridge_obstacle_in_headroom() { + // Ring of 8 cells at iz=0 + an obstacle directly above the hole at (0,0,1). + // The hole at (0,0,0) is vetoed because its headroom column has an obstacle. + let mut pts: Vec<(f32, f32, f32)> = Vec::new(); + for (dx, dy) in [ + (-1, -1), + (-1, 0), + (-1, 1), + (0, -1), + (0, 1), + (1, -1), + (1, 0), + (1, 1), + ] { + pts.push((dx as f32 + 0.5, dy as f32 + 0.5, 0.5)); + } + pts.push((0.5, 0.5, 1.5)); + let s = extract_surfaces(&pts, 1.0, 5, 3, 3); + assert!(!s.contains(&(0, 0, 0))); + } +} From cac8a70c30dbebe3212f4c0dcce95a6e1fa24c74 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Thu, 28 May 2026 18:48:24 -0700 Subject: [PATCH 32/55] Nodes --- .../modules/mls_planner/rust/src/main.rs | 1 + .../modules/mls_planner/rust/src/nodes.rs | 343 ++++++++++++++++++ 2 files changed, 344 insertions(+) create mode 100644 dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs index f50114035b..3c21cf505e 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs @@ -2,6 +2,7 @@ // SPDX-License-Identifier: Apache-2.0 mod adjacency; +mod nodes; mod surfaces; mod voxel; diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs new file mode 100644 index 0000000000..a8131fef5f --- /dev/null +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs @@ -0,0 +1,343 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 + +//! Node placement: identify standable cells far from any wall, place graph +//! nodes at local maxima via NMS, and rescale cell-edge costs to push paths +//! toward corridor centers. + +#![allow(dead_code)] // consumed incrementally + +use std::cmp::Ordering; +use std::collections::BinaryHeap; + +use ahash::AHashMap; + +use crate::adjacency::{ + build_surface_adjacency, build_surface_lookup, CsrAdjacency, SurfaceAdjacency, SurfaceLookup, +}; +use crate::voxel::{surface_point_xyz, VoxelKey}; + +pub struct NodeData { + pub cell: VoxelKey, + pub pos: (f32, f32, f32), +} + +pub struct SurfaceGraph { + pub adj: CsrAdjacency, + pub idx_to_cell: Vec, + pub cell_to_idx: AHashMap, + pub surface_lookup: SurfaceLookup, + pub nodes: Vec, +} + +pub fn place_nodes( + surface_cells: &[VoxelKey], + voxel_size: f32, + step_cells: i32, + node_spacing_m: f32, + node_wall_buffer_m: f32, +) -> SurfaceGraph { + let surface_lookup = build_surface_lookup(surface_cells); + let SurfaceAdjacency { + adj, + idx_to_cell, + cell_to_idx, + } = build_surface_adjacency(&surface_lookup, voxel_size, step_cells); + let n = adj.n as usize; + + if n == 0 { + return SurfaceGraph { + adj, + idx_to_cell, + cell_to_idx, + surface_lookup, + nodes: Vec::new(), + }; + } + + let wall_seeds = wall_adjacent_cells(&adj, &idx_to_cell); + let dist = dijkstra_multi_source(&adj, &wall_seeds); + + let mut candidates: Vec = (0..n as u32) + .filter(|&i| { + let d = dist[i as usize]; + d.is_finite() && d >= node_wall_buffer_m + }) + .collect(); + candidates.sort_by(|&a, &b| dist[b as usize].total_cmp(&dist[a as usize])); + + let survivors = nms_grid(&candidates, &idx_to_cell, voxel_size, node_spacing_m); + + let nodes: Vec = survivors + .iter() + .map(|&idx| { + let cell = idx_to_cell[idx as usize]; + NodeData { + cell, + pos: surface_point_xyz(cell.0, cell.1, cell.2, voxel_size), + } + }) + .collect(); + + let scaled_adj = wall_safe_adjacency(&adj, &dist, node_wall_buffer_m); + + SurfaceGraph { + adj: scaled_adj, + idx_to_cell, + cell_to_idx, + surface_lookup, + nodes, + } +} + +/// Cells with fewer than 4 same-z neighbors are the boundary of standable +/// terrain and act as Dijkstra seeds for distance-from-wall. +fn wall_adjacent_cells(adj: &CsrAdjacency, idx_to_cell: &[VoxelKey]) -> Vec { + let n = adj.n as usize; + let mut same_z = vec![0u8; n]; + for u in 0..n { + let iz_u = idx_to_cell[u].2; + let lo = adj.indptr[u] as usize; + let hi = adj.indptr[u + 1] as usize; + for k in lo..hi { + let v = adj.indices[k] as usize; + if idx_to_cell[v].2 == iz_u { + same_z[u] += 1; + } + } + } + let mut wall: Vec = (0..n as u32).filter(|&i| same_z[i as usize] < 4).collect(); + if wall.is_empty() { + wall.push(0); + } + wall +} + +struct Scored(f32, u32); + +impl PartialEq for Scored { + fn eq(&self, other: &Self) -> bool { + self.0.total_cmp(&other.0) == Ordering::Equal && self.1 == other.1 + } +} +impl Eq for Scored {} +impl PartialOrd for Scored { + fn partial_cmp(&self, other: &Self) -> Option { + Some(self.cmp(other)) + } +} +impl Ord for Scored { + fn cmp(&self, other: &Self) -> Ordering { + // Min-heap: smallest f32 pops first. + other.0.total_cmp(&self.0).then(self.1.cmp(&other.1)) + } +} + +fn dijkstra_multi_source(adj: &CsrAdjacency, sources: &[u32]) -> Vec { + let n = adj.n as usize; + let mut dist = vec![f32::INFINITY; n]; + let mut heap: BinaryHeap = BinaryHeap::new(); + for &s in sources { + dist[s as usize] = 0.0; + heap.push(Scored(0.0, s)); + } + while let Some(Scored(d, u)) = heap.pop() { + if d > dist[u as usize] { + continue; + } + let lo = adj.indptr[u as usize] as usize; + let hi = adj.indptr[u as usize + 1] as usize; + for k in lo..hi { + let v = adj.indices[k]; + let w = adj.data[k]; + let nd = d + w; + if nd < dist[v as usize] { + dist[v as usize] = nd; + heap.push(Scored(nd, v)); + } + } + } + dist +} + +/// Bin placed nodes by node_spacing-sized cells. For each candidate, scan the +/// 27 nearby bins for any node within Euclidean node_spacing. Replaces the +/// Python's cKDTree. +fn nms_grid( + candidates_sorted: &[u32], + idx_to_cell: &[VoxelKey], + voxel_size: f32, + node_spacing_m: f32, +) -> Vec { + let bin_size = ((node_spacing_m / voxel_size) as i32).max(1); + let r_sq = (node_spacing_m as f64) * (node_spacing_m as f64); + let v = voxel_size as f64; + let bin_of = |c: VoxelKey| { + ( + c.0.div_euclid(bin_size), + c.1.div_euclid(bin_size), + c.2.div_euclid(bin_size), + ) + }; + + let mut bins: AHashMap<(i32, i32, i32), Vec> = AHashMap::new(); + let mut survivors = Vec::new(); + for &cand in candidates_sorted { + let cell = idx_to_cell[cand as usize]; + let (bx, by, bz) = bin_of(cell); + let mut killed = false; + 'outer: for dbx in -1..=1 { + for dby in -1..=1 { + for dbz in -1..=1 { + if let Some(nearby) = bins.get(&(bx + dbx, by + dby, bz + dbz)) { + for &n in nearby { + let dx = (cell.0 - n.0) as f64 * v; + let dy = (cell.1 - n.1) as f64 * v; + let dz = (cell.2 - n.2) as f64 * v; + if dx * dx + dy * dy + dz * dz <= r_sq { + killed = true; + break 'outer; + } + } + } + } + } + } + if !killed { + survivors.push(cand); + bins.entry((bx, by, bz)).or_default().push(cell); + } + } + survivors +} + +/// Linear ramp from penalty=2 at wall to penalty=1 at buffer, capped at 1. +/// Per-edge multiplier averages the two endpoints' penalties. +fn wall_safe_adjacency(adj: &CsrAdjacency, dist: &[f32], buffer_m: f32) -> CsrAdjacency { + let n = adj.n as usize; + let penalty: Vec = (0..n) + .map(|i| (1.0 + (buffer_m - dist[i]) / buffer_m).max(1.0)) + .collect(); + + let mut data = Vec::with_capacity(adj.data.len()); + for u in 0..n { + let lo = adj.indptr[u] as usize; + let hi = adj.indptr[u + 1] as usize; + let pu = penalty[u]; + for k in lo..hi { + let v = adj.indices[k] as usize; + let pv = penalty[v]; + data.push(adj.data[k] * (pu + pv) / 2.0); + } + } + + CsrAdjacency { + indptr: adj.indptr.clone(), + indices: adj.indices.clone(), + data, + n: adj.n, + } +} + +#[cfg(test)] +mod tests { + use super::*; + + const VOXEL: f32 = 0.1; + + fn open_patch_5x5() -> Vec { + let mut c = Vec::new(); + for ix in 0..5 { + for iy in 0..5 { + c.push((ix, iy, 0)); + } + } + c + } + + fn open_patch(ix0: i32, iy0: i32, size: i32) -> Vec { + let mut c = Vec::new(); + for dx in 0..size { + for dy in 0..size { + c.push((ix0 + dx, iy0 + dy, 0)); + } + } + c + } + + #[test] + fn empty_input() { + let sg = place_nodes(&[], VOXEL, 2, 1.0, 0.3); + assert_eq!(sg.adj.n, 0); + assert!(sg.nodes.is_empty()); + } + + #[test] + fn isolated_cell_places_no_node() { + // Single cell has 0 neighbors, is wall-adjacent, dist=0, below buffer. + let sg = place_nodes(&[(0, 0, 0)], VOXEL, 2, 1.0, 0.3); + assert!(sg.nodes.is_empty()); + } + + #[test] + fn open_patch_places_at_least_one_node() { + // 10x10 at voxel=0.1 is 1m x 1m. Center is ~0.5m from any wall, well above buffer=0.3m. + let sg = place_nodes(&open_patch(0, 0, 10), VOXEL, 2, 1.0, 0.3); + assert!(!sg.nodes.is_empty()); + for n in &sg.nodes { + let (ix, iy, _) = n.cell; + assert!((0..10).contains(&ix) && (0..10).contains(&iy)); + } + } + + #[test] + fn nms_enforces_spacing() { + // Two 10x10 patches separated by 1m gap; each places at least one node, no pair within 1m. + let mut cells = open_patch(0, 0, 10); + cells.extend(open_patch(20, 0, 10)); + let sg = place_nodes(&cells, VOXEL, 2, 1.0, 0.3); + assert!(sg.nodes.len() >= 2); + for i in 0..sg.nodes.len() { + for j in (i + 1)..sg.nodes.len() { + let a = sg.nodes[i].pos; + let b = sg.nodes[j].pos; + let dx = a.0 - b.0; + let dy = a.1 - b.1; + let dz = a.2 - b.2; + let d_sq = dx * dx + dy * dy + dz * dz; + assert!(d_sq > 1.0 * 1.0 - 1e-4); + } + } + } + + #[test] + fn wall_cells_scale_outbound_cost() { + // Strip of 10 cells. End cells have 1 same-z neighbor → wall-adjacent → dist=0 → penalty=2. + let cells: Vec = (0..10).map(|ix| (ix, 0, 0)).collect(); + let sg = place_nodes(&cells, VOXEL, 2, 1.0, 0.3); + let end_idx = sg.cell_to_idx[&(0, 0, 0)] as usize; + let lo = sg.adj.indptr[end_idx] as usize; + let hi = sg.adj.indptr[end_idx + 1] as usize; + assert!(hi > lo); + // End cell penalty=2, neighbor penalty>=1, so outbound cost >= 1.5 * VOXEL. + for k in lo..hi { + assert!(sg.adj.data[k] >= 1.5 * VOXEL - 1e-5); + } + } + + #[test] + fn dijkstra_distances_grow_from_seeds() { + let cells = open_patch_5x5(); + let lookup = build_surface_lookup(&cells); + let SurfaceAdjacency { + adj, idx_to_cell, .. + } = build_surface_adjacency(&lookup, VOXEL, 2); + let seeds = wall_adjacent_cells(&adj, &idx_to_cell); + let dist = dijkstra_multi_source(&adj, &seeds); + + let center = idx_to_cell.iter().position(|&c| c == (2, 2, 0)).unwrap(); + let corner = idx_to_cell.iter().position(|&c| c == (0, 0, 0)).unwrap(); + assert!(dist[center] > dist[corner]); + assert_eq!(dist[corner], 0.0); + } +} From 8d5c17ea9a80ee21a259be95266bfc2604eb6493 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Thu, 28 May 2026 19:01:07 -0700 Subject: [PATCH 33/55] Edges via Dijkstra --- .../modules/mls_planner/rust/src/dijkstra.rs | 196 ++++++++++++++ .../modules/mls_planner/rust/src/edges.rs | 254 ++++++++++++++++++ .../modules/mls_planner/rust/src/main.rs | 2 + .../modules/mls_planner/rust/src/nodes.rs | 62 +---- 4 files changed, 458 insertions(+), 56 deletions(-) create mode 100644 dimos/navigation/nav_stack/modules/mls_planner/rust/src/dijkstra.rs create mode 100644 dimos/navigation/nav_stack/modules/mls_planner/rust/src/edges.rs diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/dijkstra.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/dijkstra.rs new file mode 100644 index 0000000000..2940f2617f --- /dev/null +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/dijkstra.rs @@ -0,0 +1,196 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 + +//! Multi-source Dijkstra over the CSR adjacency. +//! +//! Always tracks distance, predecessors, and per-cell source labels. Some +//! callers only need `dist` (e.g. wall-distance in nodes.rs) and ignore the +//! rest. The extra arrays cost two Vec allocations and two writes per +//! heap pop, which is negligible against the algorithmic work. + +#![allow(dead_code)] + +use std::cmp::Ordering; +use std::collections::BinaryHeap; + +use crate::adjacency::CsrAdjacency; + +pub struct DijkstraResult { + pub dist: Vec, + /// Predecessor cell along the shortest path back to a source. -1 marks + /// source cells and unreachable cells. Used downstream to reconstruct + /// cell-by-cell paths lazily. + pub pred: Vec, + /// Index into the caller's `sources` slice. When the caller passes node + /// cells in node-id order this doubles as the nearest-node id, which is + /// the Voronoi partition. -1 for unreachable cells. + pub source: Vec, +} + +pub fn dijkstra(adj: &CsrAdjacency, sources: &[u32]) -> DijkstraResult { + let n = adj.n as usize; + let mut dist = vec![f32::INFINITY; n]; + let mut pred = vec![-1i32; n]; + let mut source = vec![-1i32; n]; + let mut heap: BinaryHeap = BinaryHeap::new(); + + for (label, &s) in sources.iter().enumerate() { + let su = s as usize; + dist[su] = 0.0; + source[su] = label as i32; + heap.push(Scored(0.0, s)); + } + + while let Some(Scored(d, u)) = heap.pop() { + if d > dist[u as usize] { + continue; + } + let lo = adj.indptr[u as usize] as usize; + let hi = adj.indptr[u as usize + 1] as usize; + for k in lo..hi { + let v = adj.indices[k]; + let nd = d + adj.data[k]; + if nd < dist[v as usize] { + dist[v as usize] = nd; + pred[v as usize] = u as i32; + source[v as usize] = source[u as usize]; + heap.push(Scored(nd, v)); + } + } + } + + DijkstraResult { dist, pred, source } +} + +struct Scored(f32, u32); + +impl PartialEq for Scored { + fn eq(&self, other: &Self) -> bool { + self.0.total_cmp(&other.0) == Ordering::Equal && self.1 == other.1 + } +} +impl Eq for Scored {} +impl PartialOrd for Scored { + fn partial_cmp(&self, other: &Self) -> Option { + Some(self.cmp(other)) + } +} +impl Ord for Scored { + fn cmp(&self, other: &Self) -> Ordering { + // Min-heap on f32 score. + other.0.total_cmp(&self.0).then(self.1.cmp(&other.1)) + } +} + +#[cfg(test)] +mod tests { + use super::*; + + /// Build a CSR from a Vec of (src, dst, cost) edges over n cells. + /// Edges are kept directed; caller emits both directions for an undirected graph. + fn csr(n: u32, mut edges: Vec<(u32, u32, f32)>) -> CsrAdjacency { + edges.sort_by_key(|&(s, _, _)| s); + let mut indptr = vec![0u32; (n + 1) as usize]; + for &(s, _, _) in &edges { + indptr[s as usize + 1] += 1; + } + for i in 1..indptr.len() { + indptr[i] += indptr[i - 1]; + } + let indices: Vec = edges.iter().map(|&(_, d, _)| d).collect(); + let data: Vec = edges.iter().map(|&(_, _, w)| w).collect(); + CsrAdjacency { + indptr, + indices, + data, + n, + } + } + + /// Bidirectional chain 0 - 1 - 2 - 3 - 4 with unit edge cost. + fn chain(n: u32) -> CsrAdjacency { + let mut edges = Vec::new(); + for i in 0..n - 1 { + edges.push((i, i + 1, 1.0)); + edges.push((i + 1, i, 1.0)); + } + csr(n, edges) + } + + #[test] + fn empty_sources_leaves_everything_unreachable() { + let adj = chain(4); + let r = dijkstra(&adj, &[]); + for i in 0..4 { + assert!(r.dist[i].is_infinite()); + assert_eq!(r.pred[i], -1); + assert_eq!(r.source[i], -1); + } + } + + #[test] + fn single_source_dist_and_pred() { + let adj = chain(5); + let r = dijkstra(&adj, &[0]); + assert_eq!(r.dist, vec![0.0, 1.0, 2.0, 3.0, 4.0]); + assert_eq!(r.source, vec![0, 0, 0, 0, 0]); + // Predecessor chain walks back to the source. + assert_eq!(r.pred[0], -1); + let mut cur: i32 = 4; + let mut hops = 0; + while r.pred[cur as usize] >= 0 { + cur = r.pred[cur as usize]; + hops += 1; + } + assert_eq!(cur, 0); + assert_eq!(hops, 4); + } + + #[test] + fn multi_source_labels_by_nearest() { + // Sources at 0 and 4 on a 5-cell chain. Cells 0-1 closer to source 0, + // cells 3-4 closer to source 1. Cell 2 is equidistant. + let adj = chain(5); + let r = dijkstra(&adj, &[0, 4]); + assert_eq!(r.source[0], 0); + assert_eq!(r.source[1], 0); + assert_eq!(r.source[3], 1); + assert_eq!(r.source[4], 1); + // Cell 2 labeling is implementation-defined for ties but must be one of the two. + assert!(r.source[2] == 0 || r.source[2] == 1); + assert_eq!(r.dist, vec![0.0, 1.0, 2.0, 1.0, 0.0]); + } + + #[test] + fn disconnected_cells_stay_unreachable() { + // Two separate chains 0-1 and 2-3, source at 0. + let edges = vec![(0, 1, 1.0), (1, 0, 1.0), (2, 3, 1.0), (3, 2, 1.0)]; + let adj = csr(4, edges); + let r = dijkstra(&adj, &[0]); + assert_eq!(r.dist[0], 0.0); + assert_eq!(r.dist[1], 1.0); + assert!(r.dist[2].is_infinite()); + assert!(r.dist[3].is_infinite()); + assert_eq!(r.source[2], -1); + assert_eq!(r.source[3], -1); + assert_eq!(r.pred[2], -1); + assert_eq!(r.pred[3], -1); + } + + #[test] + fn shorter_path_overrides_longer() { + // 0 - 1 with cost 10, 0 - 2 - 1 with cost 1+1=2. + let edges = vec![ + (0, 1, 10.0), + (1, 0, 10.0), + (0, 2, 1.0), + (2, 0, 1.0), + (2, 1, 1.0), + (1, 2, 1.0), + ]; + let adj = csr(3, edges); + let r = dijkstra(&adj, &[0]); + assert_eq!(r.dist[1], 2.0); + assert_eq!(r.pred[1], 2); // came via 2, not directly + } +} diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/edges.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/edges.rs new file mode 100644 index 0000000000..022cfb5e44 --- /dev/null +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/edges.rs @@ -0,0 +1,254 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 + +//! Node-graph edge construction. +//! +//! Run multi-source Dijkstra from every node cell to partition the surface +//! into Voronoi regions (one per node). Walk every cell-level edge that +//! straddles a region boundary; for each ordered node pair keep the cheapest +//! crossing as a node-level edge. +//! +//! Each `NodeEdge` stores only the two boundary cells and the total cost. +//! The cell-by-cell path along an edge is reconstructed lazily in plan.rs +//! by walking the saved predecessor array. + +#![allow(dead_code)] + +use ahash::AHashMap; + +use crate::adjacency::{CsrAdjacency, SurfaceLookup}; +use crate::dijkstra::{dijkstra, DijkstraResult}; +use crate::nodes::{NodeData, SurfaceGraph}; +use crate::voxel::VoxelKey; + +pub struct NodeEdge { + pub a: u32, + pub b: u32, + pub cost: f32, + /// Cell on a's side of the cheapest Voronoi boundary crossing. + pub boundary_u: u32, + /// Cell on b's side. + pub boundary_v: u32, +} + +pub struct PlannerGraph { + pub adj: CsrAdjacency, + pub idx_to_cell: Vec, + pub cell_to_idx: AHashMap, + pub surface_lookup: SurfaceLookup, + pub nodes: Vec, + pub node_edges: Vec, + /// node_id -> indices into `node_edges`. Adjacency for the node-level + /// shortest-path query in Stage D. + pub node_adj: Vec>, + /// Per-cell predecessor along the multi-source Dijkstra from node seeds. + /// -1 marks source cells and unreachable cells. Used by plan.rs to walk + /// cell paths for the edges on a chosen route. + pub cell_predecessors: Vec, +} + +pub fn add_node_edges(sg: SurfaceGraph) -> PlannerGraph { + let SurfaceGraph { + adj, + idx_to_cell, + cell_to_idx, + surface_lookup, + nodes, + } = sg; + + if nodes.is_empty() { + let n = adj.n as usize; + return PlannerGraph { + adj, + idx_to_cell, + cell_to_idx, + surface_lookup, + nodes, + node_edges: Vec::new(), + node_adj: Vec::new(), + cell_predecessors: vec![-1; n], + }; + } + + let source_indices: Vec = nodes.iter().map(|n| cell_to_idx[&n.cell]).collect(); + let DijkstraResult { dist, pred, source } = dijkstra(&adj, &source_indices); + let node_edges = best_boundary_edges(&adj, &dist, &source); + + let mut node_adj: Vec> = vec![Vec::new(); nodes.len()]; + for (edge_idx, edge) in node_edges.iter().enumerate() { + node_adj[edge.a as usize].push(edge_idx as u32); + node_adj[edge.b as usize].push(edge_idx as u32); + } + + PlannerGraph { + adj, + idx_to_cell, + cell_to_idx, + surface_lookup, + nodes, + node_edges, + node_adj, + cell_predecessors: pred, + } +} + +fn best_boundary_edges(adj: &CsrAdjacency, dist: &[f32], source: &[i32]) -> Vec { + let mut best: AHashMap<(u32, u32), NodeEdge> = AHashMap::new(); + + for u in 0..adj.n as usize { + let sa = source[u]; + if sa < 0 { + continue; + } + let lo = adj.indptr[u] as usize; + let hi = adj.indptr[u + 1] as usize; + for k in lo..hi { + let v = adj.indices[k] as usize; + let sb = source[v]; + if sb < 0 || sa == sb { + continue; + } + let edge_w = adj.data[k]; + let cost = dist[u] + edge_w + dist[v]; + + let (key_a, key_b, bu, bv) = if (sa as u32) < (sb as u32) { + (sa as u32, sb as u32, u as u32, v as u32) + } else { + (sb as u32, sa as u32, v as u32, u as u32) + }; + + let entry = best.entry((key_a, key_b)).or_insert(NodeEdge { + a: key_a, + b: key_b, + cost: f32::INFINITY, + boundary_u: 0, + boundary_v: 0, + }); + if cost < entry.cost { + entry.cost = cost; + entry.boundary_u = bu; + entry.boundary_v = bv; + } + } + } + + let mut out: Vec = best.into_values().collect(); + out.sort_by_key(|e| (e.a, e.b)); + out +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::adjacency::{build_surface_adjacency, build_surface_lookup, SurfaceAdjacency}; + use crate::voxel::surface_point_xyz; + + const VOXEL: f32 = 0.1; + + /// Build a SurfaceGraph from a list of surface cells and a list of node + /// cells (which must be a subset of the surface cells). Bypasses + /// place_nodes so the test author controls which cells become nodes. + fn graph_with_nodes(surface_cells: &[VoxelKey], node_cells: &[VoxelKey]) -> SurfaceGraph { + let surface_lookup = build_surface_lookup(surface_cells); + let SurfaceAdjacency { + adj, + idx_to_cell, + cell_to_idx, + } = build_surface_adjacency(&surface_lookup, VOXEL, 2); + let nodes: Vec = node_cells + .iter() + .map(|&c| NodeData { + cell: c, + pos: surface_point_xyz(c.0, c.1, c.2, VOXEL), + }) + .collect(); + SurfaceGraph { + adj, + idx_to_cell, + cell_to_idx, + surface_lookup, + nodes, + } + } + + /// 20-cell strip along x at iz=0. + fn strip_cells() -> Vec { + (0..20).map(|x| (x, 0, 0)).collect() + } + + #[test] + fn no_nodes_yields_no_edges() { + let sg = graph_with_nodes(&strip_cells(), &[]); + let pg = add_node_edges(sg); + assert!(pg.node_edges.is_empty()); + assert!(pg.node_adj.is_empty()); + } + + #[test] + fn single_node_has_no_edges() { + let sg = graph_with_nodes(&strip_cells(), &[(10, 0, 0)]); + let pg = add_node_edges(sg); + assert!(pg.node_edges.is_empty()); + assert_eq!(pg.node_adj.len(), 1); + assert!(pg.node_adj[0].is_empty()); + } + + #[test] + fn two_nodes_on_strip_have_one_edge() { + let sg = graph_with_nodes(&strip_cells(), &[(3, 0, 0), (15, 0, 0)]); + let pg = add_node_edges(sg); + assert_eq!(pg.node_edges.len(), 1); + let e = &pg.node_edges[0]; + assert_eq!((e.a, e.b), (0, 1)); + assert_eq!(pg.node_adj[0], vec![0]); + assert_eq!(pg.node_adj[1], vec![0]); + } + + #[test] + fn three_nodes_in_line_form_a_chain() { + // Nodes at 3, 10, 17 in a strip 0..20. Voronoi boundaries are + // around 6-7 and 13-14, so we get edges (0,1) and (1,2) but no (0,2). + let sg = graph_with_nodes(&strip_cells(), &[(3, 0, 0), (10, 0, 0), (17, 0, 0)]); + let pg = add_node_edges(sg); + let pairs: Vec<(u32, u32)> = pg.node_edges.iter().map(|e| (e.a, e.b)).collect(); + assert_eq!(pairs, vec![(0, 1), (1, 2)]); + } + + #[test] + fn disconnected_components_have_no_edge() { + // Two strips with a gap, one node in each. + let mut cells: Vec = (0..5).map(|x| (x, 0, 0)).collect(); + cells.extend((10..15).map(|x| (x, 0, 0))); + let sg = graph_with_nodes(&cells, &[(2, 0, 0), (12, 0, 0)]); + let pg = add_node_edges(sg); + assert!(pg.node_edges.is_empty()); + } + + #[test] + fn predecessor_walk_recovers_cell_path() { + // Two nodes at strip ends. Walk preds from each boundary cell back to + // its owning node cell and verify the chain reaches the node. + let sg = graph_with_nodes(&strip_cells(), &[(0, 0, 0), (19, 0, 0)]); + let pg = add_node_edges(sg); + assert_eq!(pg.node_edges.len(), 1); + let e = &pg.node_edges[0]; + + let cell_a = pg.cell_to_idx[&pg.nodes[0].cell]; + let cell_b = pg.cell_to_idx[&pg.nodes[1].cell]; + + let mut cur = e.boundary_u as i32; + let mut hops = 0; + while pg.cell_predecessors[cur as usize] >= 0 { + cur = pg.cell_predecessors[cur as usize]; + hops += 1; + assert!(hops < 1000, "predecessor walk did not terminate"); + } + assert_eq!(cur as u32, cell_a, "u-side preds must reach node a"); + + let mut cur = e.boundary_v as i32; + while pg.cell_predecessors[cur as usize] >= 0 { + cur = pg.cell_predecessors[cur as usize]; + } + assert_eq!(cur as u32, cell_b, "v-side preds must reach node b"); + } +} diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs index 3c21cf505e..c4ad4216f1 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs @@ -2,6 +2,8 @@ // SPDX-License-Identifier: Apache-2.0 mod adjacency; +mod dijkstra; +mod edges; mod nodes; mod surfaces; mod voxel; diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs index a8131fef5f..5c5c07bcbc 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs @@ -7,14 +7,12 @@ #![allow(dead_code)] // consumed incrementally -use std::cmp::Ordering; -use std::collections::BinaryHeap; - use ahash::AHashMap; use crate::adjacency::{ build_surface_adjacency, build_surface_lookup, CsrAdjacency, SurfaceAdjacency, SurfaceLookup, }; +use crate::dijkstra::dijkstra; use crate::voxel::{surface_point_xyz, VoxelKey}; pub struct NodeData { @@ -56,7 +54,7 @@ pub fn place_nodes( } let wall_seeds = wall_adjacent_cells(&adj, &idx_to_cell); - let dist = dijkstra_multi_source(&adj, &wall_seeds); + let dist = dijkstra(&adj, &wall_seeds).dist; let mut candidates: Vec = (0..n as u32) .filter(|&i| { @@ -90,8 +88,8 @@ pub fn place_nodes( } } -/// Cells with fewer than 4 same-z neighbors are the boundary of standable -/// terrain and act as Dijkstra seeds for distance-from-wall. +/// Cells that are missing any of the 4 neighbors are considered +/// on the edge of walkable terrain. fn wall_adjacent_cells(adj: &CsrAdjacency, idx_to_cell: &[VoxelKey]) -> Vec { let n = adj.n as usize; let mut same_z = vec![0u8; n]; @@ -113,56 +111,8 @@ fn wall_adjacent_cells(adj: &CsrAdjacency, idx_to_cell: &[VoxelKey]) -> Vec wall } -struct Scored(f32, u32); - -impl PartialEq for Scored { - fn eq(&self, other: &Self) -> bool { - self.0.total_cmp(&other.0) == Ordering::Equal && self.1 == other.1 - } -} -impl Eq for Scored {} -impl PartialOrd for Scored { - fn partial_cmp(&self, other: &Self) -> Option { - Some(self.cmp(other)) - } -} -impl Ord for Scored { - fn cmp(&self, other: &Self) -> Ordering { - // Min-heap: smallest f32 pops first. - other.0.total_cmp(&self.0).then(self.1.cmp(&other.1)) - } -} - -fn dijkstra_multi_source(adj: &CsrAdjacency, sources: &[u32]) -> Vec { - let n = adj.n as usize; - let mut dist = vec![f32::INFINITY; n]; - let mut heap: BinaryHeap = BinaryHeap::new(); - for &s in sources { - dist[s as usize] = 0.0; - heap.push(Scored(0.0, s)); - } - while let Some(Scored(d, u)) = heap.pop() { - if d > dist[u as usize] { - continue; - } - let lo = adj.indptr[u as usize] as usize; - let hi = adj.indptr[u as usize + 1] as usize; - for k in lo..hi { - let v = adj.indices[k]; - let w = adj.data[k]; - let nd = d + w; - if nd < dist[v as usize] { - dist[v as usize] = nd; - heap.push(Scored(nd, v)); - } - } - } - dist -} - /// Bin placed nodes by node_spacing-sized cells. For each candidate, scan the -/// 27 nearby bins for any node within Euclidean node_spacing. Replaces the -/// Python's cKDTree. +/// 27 nearby bins for any node within Euclidean node_spacing. fn nms_grid( candidates_sorted: &[u32], idx_to_cell: &[VoxelKey], @@ -333,7 +283,7 @@ mod tests { adj, idx_to_cell, .. } = build_surface_adjacency(&lookup, VOXEL, 2); let seeds = wall_adjacent_cells(&adj, &idx_to_cell); - let dist = dijkstra_multi_source(&adj, &seeds); + let dist = dijkstra(&adj, &seeds).dist; let center = idx_to_cell.iter().position(|&c| c == (2, 2, 0)).unwrap(); let corner = idx_to_cell.iter().position(|&c| c == (0, 0, 0)).unwrap(); From 4d832e78d65e09b701822b0671e5683a324f67e2 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Thu, 28 May 2026 19:06:02 -0700 Subject: [PATCH 34/55] Planner --- .../modules/mls_planner/rust/src/main.rs | 1 + .../modules/mls_planner/rust/src/plan.rs | 270 ++++++++++++++++++ 2 files changed, 271 insertions(+) create mode 100644 dimos/navigation/nav_stack/modules/mls_planner/rust/src/plan.rs diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs index c4ad4216f1..380f713f78 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs @@ -5,6 +5,7 @@ mod adjacency; mod dijkstra; mod edges; mod nodes; +mod plan; mod surfaces; mod voxel; diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/plan.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/plan.rs new file mode 100644 index 0000000000..0f064d5f4e --- /dev/null +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/plan.rs @@ -0,0 +1,270 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 + +//! Query-time planning: snap start/goal poses to nodes, run shortest path on +//! the node graph, and stitch cached edge cell paths into XYZ waypoints. +#![allow(dead_code)] + +use crate::adjacency::CsrAdjacency; +use crate::dijkstra::dijkstra; +use crate::edges::PlannerGraph; +use crate::voxel::{surface_point_xyz, VoxelKey}; + +/// Snap a query pose to the nearest node by 3D Euclidean distance, rejecting +/// nodes whose z differs from the pose's z by more than `z_tolerance_m`. +pub fn snap_pose_to_node( + plg: &PlannerGraph, + pose: (f32, f32, f32), + z_tolerance_m: f32, +) -> Option { + let mut best: Option<(f32, u32)> = None; + for (i, n) in plg.nodes.iter().enumerate() { + if (pose.2 - n.pos.2).abs() > z_tolerance_m { + continue; + } + let dx = pose.0 - n.pos.0; + let dy = pose.1 - n.pos.1; + let dz = pose.2 - n.pos.2; + let d_sq = dx * dx + dy * dy + dz * dz; + match best { + Some((b, _)) if b <= d_sq => {} + _ => best = Some((d_sq, i as u32)), + } + } + best.map(|(_, i)| i) +} + +/// Plan an XYZ waypoint sequence from `start_pose` to `goal_pose`. +/// Returns None if either pose can't snap, or if the snapped nodes are +/// disconnected in the node graph. +pub fn plan( + plg: &PlannerGraph, + start_pose: (f32, f32, f32), + goal_pose: (f32, f32, f32), + voxel_size: f32, + z_tolerance_m: f32, +) -> Option> { + let start_node = snap_pose_to_node(plg, start_pose, z_tolerance_m)?; + let goal_node = snap_pose_to_node(plg, goal_pose, z_tolerance_m)?; + let node_seq = shortest_path_nodes(plg, start_node, goal_node)?; + Some(assemble_waypoints( + plg, &node_seq, start_pose, goal_pose, voxel_size, + )) +} + +pub fn shortest_path_nodes(plg: &PlannerGraph, start: u32, goal: u32) -> Option> { + if start == goal { + return Some(vec![start]); + } + let csr = build_node_csr(plg); + let r = dijkstra(&csr, &[start]); + if !r.dist[goal as usize].is_finite() { + return None; + } + let mut path = vec![goal]; + let mut cur = goal as i32; + while r.pred[cur as usize] >= 0 { + cur = r.pred[cur as usize]; + path.push(cur as u32); + } + path.reverse(); + Some(path) +} + +fn build_node_csr(plg: &PlannerGraph) -> CsrAdjacency { + let n = plg.nodes.len(); + let mut indptr = vec![0u32; n + 1]; + let mut indices = Vec::new(); + let mut data = Vec::new(); + for (u, edges) in plg.node_adj.iter().enumerate() { + for &edge_idx in edges { + let edge = &plg.node_edges[edge_idx as usize]; + let neighbor = if edge.a as usize == u { edge.b } else { edge.a }; + indices.push(neighbor); + data.push(edge.cost); + } + indptr[u + 1] = indices.len() as u32; + } + CsrAdjacency { + indptr, + indices, + data, + n: n as u32, + } +} + +fn assemble_waypoints( + plg: &PlannerGraph, + node_seq: &[u32], + start_pose: (f32, f32, f32), + goal_pose: (f32, f32, f32), + voxel_size: f32, +) -> Vec<(f32, f32, f32)> { + let mut cells: Vec = Vec::new(); + for pair in node_seq.windows(2) { + let (a, b) = (pair[0], pair[1]); + let edge_idx = + edge_between(plg, a, b).expect("consecutive nodes in path must share an edge"); + let edge = &plg.node_edges[edge_idx as usize]; + let (start_side, end_side) = if a == edge.a { + (edge.boundary_u, edge.boundary_v) + } else { + (edge.boundary_v, edge.boundary_u) + }; + + let mut from_a = walk_preds_to_source(plg, start_side); + from_a.reverse(); + let to_b = walk_preds_to_source(plg, end_side); + + for c in from_a.into_iter().chain(to_b) { + if cells.last() != Some(&c) { + cells.push(c); + } + } + } + + let mut waypoints: Vec<(f32, f32, f32)> = Vec::with_capacity(cells.len() + 2); + waypoints.push(start_pose); + for (ix, iy, iz) in cells { + waypoints.push(surface_point_xyz(ix, iy, iz, voxel_size)); + } + waypoints.push(goal_pose); + waypoints +} + +fn walk_preds_to_source(plg: &PlannerGraph, start_cell: u32) -> Vec { + let mut cells = vec![plg.idx_to_cell[start_cell as usize]]; + let mut cur = start_cell as i32; + while plg.cell_predecessors[cur as usize] >= 0 { + cur = plg.cell_predecessors[cur as usize]; + cells.push(plg.idx_to_cell[cur as usize]); + } + cells +} + +fn edge_between(plg: &PlannerGraph, a: u32, b: u32) -> Option { + for &edge_idx in &plg.node_adj[a as usize] { + let edge = &plg.node_edges[edge_idx as usize]; + let other = if edge.a == a { edge.b } else { edge.a }; + if other == b { + return Some(edge_idx); + } + } + None +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::adjacency::{build_surface_adjacency, build_surface_lookup, SurfaceAdjacency}; + use crate::edges::add_node_edges; + use crate::nodes::{NodeData, SurfaceGraph}; + + const VOXEL: f32 = 0.1; + const Z_TOL: f32 = 1.5; + + fn graph_with_nodes(surface_cells: &[VoxelKey], node_cells: &[VoxelKey]) -> PlannerGraph { + let surface_lookup = build_surface_lookup(surface_cells); + let SurfaceAdjacency { + adj, + idx_to_cell, + cell_to_idx, + } = build_surface_adjacency(&surface_lookup, VOXEL, 2); + let nodes: Vec = node_cells + .iter() + .map(|&c| NodeData { + cell: c, + pos: surface_point_xyz(c.0, c.1, c.2, VOXEL), + }) + .collect(); + let sg = SurfaceGraph { + adj, + idx_to_cell, + cell_to_idx, + surface_lookup, + nodes, + }; + add_node_edges(sg) + } + + fn strip(n: i32) -> Vec { + (0..n).map(|x| (x, 0, 0)).collect() + } + + #[test] + fn snap_returns_none_when_no_nodes() { + let plg = graph_with_nodes(&strip(20), &[]); + assert!(snap_pose_to_node(&plg, (0.5, 0.0, 0.05), Z_TOL).is_none()); + } + + #[test] + fn snap_returns_nearest_node() { + // Nodes at x=3 and x=15 (XYZ positions 0.35 and 1.55). + let plg = graph_with_nodes(&strip(20), &[(3, 0, 0), (15, 0, 0)]); + let snapped = snap_pose_to_node(&plg, (0.5, 0.0, 0.1), Z_TOL).unwrap(); + assert_eq!(snapped, 0); + let snapped = snap_pose_to_node(&plg, (1.4, 0.0, 0.1), Z_TOL).unwrap(); + assert_eq!(snapped, 1); + } + + #[test] + fn snap_rejects_outside_z_tolerance() { + let plg = graph_with_nodes(&strip(20), &[(3, 0, 0)]); + // Node's pos.z = (0+1)*0.1 = 0.1. Pose at z=2.0, tolerance=1.5 → 1.9 > 1.5 → reject. + assert!(snap_pose_to_node(&plg, (0.5, 0.0, 2.0), 1.5).is_none()); + } + + #[test] + fn plan_returns_none_if_start_cant_snap() { + let plg = graph_with_nodes(&strip(20), &[(10, 0, 0)]); + let result = plan(&plg, (0.5, 0.0, 10.0), (1.0, 0.0, 0.1), VOXEL, Z_TOL); + assert!(result.is_none()); + } + + #[test] + fn plan_returns_none_if_disconnected() { + // Two strips with a gap. + let mut cells: Vec = (0..5).map(|x| (x, 0, 0)).collect(); + cells.extend((10..15).map(|x| (x, 0, 0))); + let plg = graph_with_nodes(&cells, &[(2, 0, 0), (12, 0, 0)]); + let result = plan(&plg, (0.25, 0.0, 0.1), (1.25, 0.0, 0.1), VOXEL, Z_TOL); + assert!(result.is_none()); + } + + #[test] + fn plan_same_start_and_goal_returns_two_waypoints() { + let plg = graph_with_nodes(&strip(20), &[(10, 0, 0)]); + let wp = plan(&plg, (1.0, 0.0, 0.05), (1.0, 0.0, 0.05), VOXEL, Z_TOL).unwrap(); + assert_eq!(wp.len(), 2); + assert_eq!(wp[0], (1.0, 0.0, 0.05)); + assert_eq!(wp[1], (1.0, 0.0, 0.05)); + } + + #[test] + fn plan_produces_monotonic_xy_along_strip() { + let plg = graph_with_nodes(&strip(20), &[(3, 0, 0), (15, 0, 0)]); + let wp = plan(&plg, (0.2, 0.0, 0.05), (1.7, 0.0, 0.05), VOXEL, Z_TOL).unwrap(); + // First waypoint is start_pose, last is goal_pose. + assert_eq!(wp.first(), Some(&(0.2, 0.0, 0.05))); + assert_eq!(wp.last(), Some(&(1.7, 0.0, 0.05))); + // Interior waypoints monotonically increase in x. + for w in wp.windows(2).skip(1).take(wp.len() - 3) { + assert!(w[1].0 >= w[0].0 - 1e-5); + } + } + + #[test] + fn plan_three_nodes_visits_them_all() { + let plg = graph_with_nodes(&strip(20), &[(3, 0, 0), (10, 0, 0), (17, 0, 0)]); + let wp = plan(&plg, (0.2, 0.0, 0.05), (1.9, 0.0, 0.05), VOXEL, Z_TOL).unwrap(); + // Each node's XY position should appear in the waypoints. + let node_xy: Vec<(f32, f32)> = plg.nodes.iter().map(|n| (n.pos.0, n.pos.1)).collect(); + for &(nx, ny) in &node_xy { + assert!( + wp.iter() + .any(|w| (w.0 - nx).abs() < 1e-5 && (w.1 - ny).abs() < 1e-5), + "node ({nx}, {ny}) should appear among waypoints" + ); + } + } +} From 6d8b8ee57da489ae39cc1a4e89fa981a1982716b Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Thu, 28 May 2026 19:22:30 -0700 Subject: [PATCH 35/55] Wire to main --- .../nav_stack/evaluator/blueprints.py | 10 +- .../modules/mls_planner/rust/src/edges.rs | 30 ++ .../modules/mls_planner/rust/src/main.rs | 322 +++++++++++++++++- .../modules/mls_planner/rust/src/plan.rs | 12 +- dimos/robot/all_blueprints.py | 1 + 5 files changed, 343 insertions(+), 32 deletions(-) diff --git a/dimos/navigation/nav_stack/evaluator/blueprints.py b/dimos/navigation/nav_stack/evaluator/blueprints.py index 7ed7b832da..0a7bb28dff 100644 --- a/dimos/navigation/nav_stack/evaluator/blueprints.py +++ b/dimos/navigation/nav_stack/evaluator/blueprints.py @@ -35,11 +35,13 @@ ) from dimos.navigation.nav_stack.modules.mls_planner.mls_planner import ( NODE_STEP_THRESHOLD_M, - MLSPlanner, - MLSPlannerConfig, build_surface_adjacency, build_surface_lookup, ) +from dimos.navigation.nav_stack.modules.mls_planner.mls_planner_native import ( + MLSPlannerNative, + MLSPlannerNativeConfig, +) from dimos.visualization.rerun.bridge import RerunBridgeModule from dimos.visualization.rerun.websocket_server import RerunWebSocketServer @@ -148,11 +150,11 @@ def _render_node_edges(msg: Any) -> Any: return rr.LineStrips3D(strips, colors=colors, radii=[0.04] * len(strips)) -_planner_voxel = MLSPlannerConfig().voxel_size +_planner_voxel = MLSPlannerNativeConfig().voxel_size path_planner_eval = autoconnect( Evaluator.blueprint(), - MLSPlanner.blueprint(), + MLSPlannerNative.blueprint(), ClickStartGoalRouter.blueprint(), RerunWebSocketServer.blueprint(), RerunBridgeModule.blueprint( diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/edges.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/edges.rs index 022cfb5e44..10ae192203 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/edges.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/edges.rs @@ -92,6 +92,36 @@ pub fn add_node_edges(sg: SurfaceGraph) -> PlannerGraph { } } +/// Walk every node-graph edge and emit one segment per consecutive cell pair +/// along the reconstructed cell path. Each segment carries the edge's total +/// cost as its traversability so renderers can color the whole edge uniformly. +pub fn edges_to_segments(plg: &PlannerGraph, _voxel_size: f32) -> Vec<(VoxelKey, VoxelKey, f32)> { + let mut segments = Vec::new(); + for edge in &plg.node_edges { + let mut from_a = walk_preds_to_source(plg, edge.boundary_u); + from_a.reverse(); + let to_b = walk_preds_to_source(plg, edge.boundary_v); + let mut path: Vec = from_a; + path.extend(to_b); + for pair in path.windows(2) { + segments.push((pair[0], pair[1], edge.cost)); + } + } + segments +} + +/// Walk the cell-predecessor array from `start_cell` back to its owning source. +/// Returns cells in walk order (start_cell first, source cell last). +pub fn walk_preds_to_source(plg: &PlannerGraph, start_cell: u32) -> Vec { + let mut cells = vec![plg.idx_to_cell[start_cell as usize]]; + let mut cur = start_cell as i32; + while plg.cell_predecessors[cur as usize] >= 0 { + cur = plg.cell_predecessors[cur as usize]; + cells.push(plg.idx_to_cell[cur as usize]); + } + cells +} + fn best_boundary_edges(adj: &CsrAdjacency, dist: &[f32], source: &[i32]) -> Vec { let mut best: AHashMap<(u32, u32), NodeEdge> = AHashMap::new(); diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs index 380f713f78..c1b0b79c19 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs @@ -9,13 +9,21 @@ mod plan; mod surfaces; mod voxel; -use dimos_module::{run, Input, LcmTransport, Module, Output}; +use std::time::{Duration, SystemTime, UNIX_EPOCH}; + +use dimos_module::{error_throttled, run, warn_throttled, Input, LcmTransport, Module, Output}; +use lcm_msgs::geometry_msgs::{Point, Pose, PoseStamped, Quaternion}; use lcm_msgs::nav_msgs::{Odometry, Path}; -use lcm_msgs::sensor_msgs::PointCloud2; +use lcm_msgs::sensor_msgs::{PointCloud2, PointField}; +use lcm_msgs::std_msgs::{Header, Time}; use serde::Deserialize; use tracing::info; -#[allow(dead_code)] // fields populated incrementally as algorithm stages land +use crate::edges::{add_node_edges, edges_to_segments, PlannerGraph}; +use crate::nodes::place_nodes; +use crate::surfaces::extract_surfaces; +use crate::voxel::surface_point_xyz; + #[derive(Debug, Deserialize)] #[serde(deny_unknown_fields)] struct Config { @@ -29,7 +37,6 @@ struct Config { node_step_threshold_m: f32, } -#[allow(dead_code)] // outputs wired up incrementally as algorithm stages land #[derive(Module)] #[module(setup = setup)] struct MlsPlanner { @@ -59,6 +66,8 @@ struct MlsPlanner { height_cells: i32, step_cells: i32, + planner_graph: Option, + latest_start: Option<(f32, f32, f32)>, } impl MlsPlanner { @@ -109,31 +118,310 @@ impl MlsPlanner { } async fn on_global_map(&mut self, msg: PointCloud2) { - let n = (msg.width as usize) * (msg.height as usize); - info!(points = n, "global_map stub: not yet implemented"); + let points = match extract_xyz(&msg) { + Ok(p) => p, + Err(e) => { + warn_throttled!( + Duration::from_secs(1), + error = %e, + "Failed to extract lidar points, dropped a cloud.", + ); + return; + } + }; + if points.is_empty() { + return; + } + + let cfg = &self.config; + let surface_cells = extract_surfaces( + &points, + cfg.voxel_size, + self.height_cells, + cfg.surface_dilation_passes, + cfg.surface_erosion_passes, + ); + let sg = place_nodes( + &surface_cells, + cfg.voxel_size, + self.step_cells, + cfg.node_spacing_m, + cfg.node_wall_buffer_m, + ); + let n_nodes = sg.nodes.len(); + let plg = add_node_edges(sg); + let n_edges = plg.node_edges.len(); + info!( + obstacle_points = points.len(), + surface_cells = surface_cells.len(), + nodes = n_nodes, + edges = n_edges, + "global_map processed", + ); + + let stamp = now(); + let surface_points: Vec<(f32, f32, f32)> = surface_cells + .iter() + .map(|&(ix, iy, iz)| surface_point_xyz(ix, iy, iz, cfg.voxel_size)) + .collect(); + publish_cloud( + &self.surface_map, + &surface_points, + &cfg.world_frame, + stamp.clone(), + ) + .await; + + let node_points: Vec<(f32, f32, f32)> = plg.nodes.iter().map(|n| n.pos).collect(); + publish_cloud(&self.nodes, &node_points, &cfg.world_frame, stamp.clone()).await; + + let edges_path = build_segments_path(&plg, cfg.voxel_size, &cfg.world_frame, stamp.clone()); + publish_path(&self.node_edges, &edges_path).await; + + self.planner_graph = Some(plg); } async fn on_start_pose(&mut self, msg: Odometry) { let p = &msg.pose.pose.position; - info!( - x = p.x, - y = p.y, - z = p.z, - "start_pose stub: not yet implemented" - ); + self.latest_start = Some((p.x as f32, p.y as f32, p.z as f32)); + // Drop any previous plan so the visualizer doesn't show a stale path + // rooted at the old start. + publish_path(&self.path, &empty_path(&self.config.world_frame, now())).await; } async fn on_goal_pose(&mut self, msg: Odometry) { + let Some(start) = self.latest_start else { + tracing::warn!("MLSPlanner received goal before start; skipping"); + return; + }; + let Some(plg) = self.planner_graph.as_ref() else { + tracing::warn!("MLSPlanner received goal before graph was built; skipping"); + return; + }; + let p = &msg.pose.pose.position; - info!( - x = p.x, - y = p.y, - z = p.z, - "goal_pose stub: not yet implemented" + let goal = (p.x as f32, p.y as f32, p.z as f32); + + let waypoints = match plan::plan( + plg, + start, + goal, + self.config.voxel_size, + self.config.robot_height, + ) { + Some(wp) => wp, + None => { + tracing::warn!(?start, ?goal, "no path between start and goal"); + publish_path(&self.path, &empty_path(&self.config.world_frame, now())).await; + return; + } + }; + + let stamp = now(); + let path_msg = build_path_from_waypoints(&waypoints, &self.config.world_frame, stamp); + info!(waypoints = waypoints.len(), "path planned"); + publish_path(&self.path, &path_msg).await; + } +} + +async fn publish_cloud( + out: &Output, + points: &[(f32, f32, f32)], + frame_id: &str, + stamp: Time, +) { + let cloud = build_pc2_xyz(points, frame_id, stamp); + if let Err(e) = out.publish(&cloud).await { + error_throttled!( + Duration::from_secs(1), + error = %e, + topic = %out.topic, + "Cloud failed to publish", ); } } +async fn publish_path(out: &Output, msg: &Path) { + if let Err(e) = out.publish(msg).await { + error_throttled!( + Duration::from_secs(1), + error = %e, + topic = %out.topic, + "Path failed to publish", + ); + } +} + +fn now() -> Time { + let dur = SystemTime::now() + .duration_since(UNIX_EPOCH) + .unwrap_or_default(); + Time { + sec: dur.as_secs() as i32, + nsec: dur.subsec_nanos() as i32, + } +} + +fn header(frame_id: &str, stamp: Time) -> Header { + Header { + seq: 0, + stamp, + frame_id: frame_id.into(), + } +} + +fn pose_at(xyz: (f32, f32, f32), orient_w: f64) -> Pose { + Pose { + position: Point { + x: xyz.0 as f64, + y: xyz.1 as f64, + z: xyz.2 as f64, + }, + orientation: Quaternion { + x: 0.0, + y: 0.0, + z: 0.0, + w: orient_w, + }, + } +} + +fn pose_stamped(xyz: (f32, f32, f32), orient_w: f64, frame_id: &str, stamp: Time) -> PoseStamped { + PoseStamped { + header: header(frame_id, stamp), + pose: pose_at(xyz, orient_w), + } +} + +fn empty_path(frame_id: &str, stamp: Time) -> Path { + Path { + header: header(frame_id, stamp), + poses: Vec::new(), + } +} + +fn build_path_from_waypoints(waypoints: &[(f32, f32, f32)], frame_id: &str, stamp: Time) -> Path { + let poses: Vec = waypoints + .iter() + .map(|&w| pose_stamped(w, 1.0, frame_id, stamp.clone())) + .collect(); + Path { + header: header(frame_id, stamp), + poses, + } +} + +/// Emit edges as alternating PoseStamped pairs (p1, p2, p1', p2', ...) with +/// orientation.w carrying the segment's per-edge cost. This is the +/// nav_msgs/LineSegments3D wire hack the Python side already decodes. +fn build_segments_path(plg: &PlannerGraph, voxel_size: f32, frame_id: &str, stamp: Time) -> Path { + let segments = edges_to_segments(plg, voxel_size); + let mut poses: Vec = Vec::with_capacity(segments.len() * 2); + for (a, b, cost) in segments { + let pa = surface_point_xyz(a.0, a.1, a.2, voxel_size); + let pb = surface_point_xyz(b.0, b.1, b.2, voxel_size); + poses.push(pose_stamped(pa, cost as f64, frame_id, stamp.clone())); + poses.push(pose_stamped(pb, cost as f64, frame_id, stamp.clone())); + } + Path { + header: header(frame_id, stamp), + poses, + } +} + +fn build_pc2_xyz(points: &[(f32, f32, f32)], frame_id: &str, stamp: Time) -> PointCloud2 { + let n = points.len() as i32; + let mut data = Vec::with_capacity(points.len() * 12); + for &(x, y, z) in points { + data.extend_from_slice(&x.to_le_bytes()); + data.extend_from_slice(&y.to_le_bytes()); + data.extend_from_slice(&z.to_le_bytes()); + } + let make_field = |name: &str, off: i32| PointField { + name: name.into(), + offset: off, + datatype: PointField::FLOAT32 as u8, + count: 1, + }; + PointCloud2 { + header: header(frame_id, stamp), + height: 1, + width: n, + fields: vec![make_field("x", 0), make_field("y", 4), make_field("z", 8)], + is_bigendian: false, + point_step: 12, + row_step: 12 * n, + data, + is_dense: true, + } +} + +struct ExtractError(&'static str); +impl std::fmt::Display for ExtractError { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + f.write_str(self.0) + } +} + +fn extract_xyz(msg: &PointCloud2) -> Result, ExtractError> { + let mut x_off: Option = None; + let mut y_off: Option = None; + let mut z_off: Option = None; + for f in &msg.fields { + if f.datatype != PointField::FLOAT32 as u8 { + continue; + } + match f.name.as_str() { + "x" => x_off = Some(f.offset as usize), + "y" => y_off = Some(f.offset as usize), + "z" => z_off = Some(f.offset as usize), + _ => {} + } + } + let xo = x_off.ok_or(ExtractError("missing float32 x field"))?; + let yo = y_off.ok_or(ExtractError("missing float32 y field"))?; + let zo = z_off.ok_or(ExtractError("missing float32 z field"))?; + + let n = (msg.width as usize) * (msg.height as usize); + let step = msg.point_step as usize; + if step == 0 { + return Err(ExtractError("point_step is 0")); + } + if msg.data.len() < n * step { + return Err(ExtractError( + "data buffer shorter than width*height*point_step", + )); + } + if xo + 4 > step || yo + 4 > step || zo + 4 > step { + return Err(ExtractError( + "xyz field offsets do not fit within point_step", + )); + } + if msg.is_bigendian { + return Err(ExtractError("big-endian point data not supported")); + } + + let mut out = Vec::with_capacity(n); + for i in 0..n { + let base = i * step; + let x = read_f32_le(&msg.data, base + xo); + let y = read_f32_le(&msg.data, base + yo); + let z = read_f32_le(&msg.data, base + zo); + if x.is_finite() && y.is_finite() && z.is_finite() { + out.push((x, y, z)); + } + } + Ok(out) +} + +#[inline] +fn read_f32_le(buf: &[u8], off: usize) -> f32 { + let bytes: [u8; 4] = buf[off..off + 4] + .try_into() + .expect("bounds checked by caller"); + f32::from_le_bytes(bytes) +} + #[tokio::main] async fn main() { let transport = LcmTransport::new() diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/plan.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/plan.rs index 0f064d5f4e..bc61f5e60b 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/plan.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/plan.rs @@ -7,7 +7,7 @@ use crate::adjacency::CsrAdjacency; use crate::dijkstra::dijkstra; -use crate::edges::PlannerGraph; +use crate::edges::{walk_preds_to_source, PlannerGraph}; use crate::voxel::{surface_point_xyz, VoxelKey}; /// Snap a query pose to the nearest node by 3D Euclidean distance, rejecting @@ -132,16 +132,6 @@ fn assemble_waypoints( waypoints } -fn walk_preds_to_source(plg: &PlannerGraph, start_cell: u32) -> Vec { - let mut cells = vec![plg.idx_to_cell[start_cell as usize]]; - let mut cur = start_cell as i32; - while plg.cell_predecessors[cur as usize] >= 0 { - cur = plg.cell_predecessors[cur as usize]; - cells.push(plg.idx_to_cell[cur as usize]); - } - cells -} - fn edge_between(plg: &PlannerGraph, a: u32, b: u32) -> Option { for &edge_idx in &plg.node_adj[a as usize] { let edge = &plg.node_edges[edge_idx as usize]; diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index fe5e76c94c..5572354205 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -172,6 +172,7 @@ "mcp-server": "dimos.agents.mcp.mcp_server.McpServer", "memory-module": "dimos.memory2.module.MemoryModule", "mls-planner": "dimos.navigation.nav_stack.modules.mls_planner.mls_planner.MLSPlanner", + "mls-planner-native": "dimos.navigation.nav_stack.modules.mls_planner.mls_planner_native.MLSPlannerNative", "mock-b1-connection-module": "dimos.robot.unitree.b1.connection.MockB1ConnectionModule", "module-a": "dimos.robot.unitree.demo_error_on_name_conflicts.ModuleA", "module-b": "dimos.robot.unitree.demo_error_on_name_conflicts.ModuleB", From 8482e759f68595bd4e97242eda5508dd96797659 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Fri, 29 May 2026 11:57:28 -0700 Subject: [PATCH 36/55] Surface rewrite --- .../modules/mls_planner/rust/src/adjacency.rs | 5 +- .../modules/mls_planner/rust/src/dijkstra.rs | 6 +- .../modules/mls_planner/rust/src/main.rs | 25 ++-- .../modules/mls_planner/rust/src/nodes.rs | 4 +- .../modules/mls_planner/rust/src/surfaces.rs | 119 +++++++++--------- .../modules/mls_planner/rust/src/voxel.rs | 10 ++ 6 files changed, 95 insertions(+), 74 deletions(-) diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/adjacency.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/adjacency.rs index da4f7ce498..057f194dd1 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/adjacency.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/adjacency.rs @@ -28,7 +28,7 @@ pub struct SurfaceAdjacency { pub cell_to_idx: AHashMap, } -/// Group cells by XY column with sorted unique iz per column. +/// Group cells in to xy columns and sort their z indexes. pub fn build_surface_lookup(cells: &[VoxelKey]) -> SurfaceLookup { let mut lookup: SurfaceLookup = AHashMap::new(); for &(ix, iy, iz) in cells { @@ -51,6 +51,7 @@ pub fn build_surface_adjacency( let mut columns: Vec<(i32, i32)> = surface_lookup.keys().copied().collect(); columns.sort_unstable(); + // assign ids to each cell let mut idx_to_cell: Vec = Vec::new(); for &(ix, iy) in &columns { for &iz in &surface_lookup[&(ix, iy)] { @@ -58,6 +59,8 @@ pub fn build_surface_adjacency( } } let n = idx_to_cell.len(); + + // also build the reverse so cells can look up their id let cell_to_idx: AHashMap = idx_to_cell .iter() .enumerate() diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/dijkstra.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/dijkstra.rs index 2940f2617f..376511f0e2 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/dijkstra.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/dijkstra.rs @@ -3,10 +3,8 @@ //! Multi-source Dijkstra over the CSR adjacency. //! -//! Always tracks distance, predecessors, and per-cell source labels. Some -//! callers only need `dist` (e.g. wall-distance in nodes.rs) and ignore the -//! rest. The extra arrays cost two Vec allocations and two writes per -//! heap pop, which is negligible against the algorithmic work. +//! Tracks the path taken and distance for each cell. This can be used to +//! reconstruct shortest paths to any of the source cells. #![allow(dead_code)] diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs index c1b0b79c19..87d2293335 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs @@ -19,10 +19,12 @@ use lcm_msgs::std_msgs::{Header, Time}; use serde::Deserialize; use tracing::info; +use ahash::AHashSet; + use crate::edges::{add_node_edges, edges_to_segments, PlannerGraph}; use crate::nodes::place_nodes; use crate::surfaces::extract_surfaces; -use crate::voxel::surface_point_xyz; +use crate::voxel::{surface_point_xyz, voxelize, VoxelKey}; #[derive(Debug, Deserialize)] #[serde(deny_unknown_fields)] @@ -64,7 +66,7 @@ struct MlsPlanner { #[config] config: Config, - height_cells: i32, + clearance_cells: i32, step_cells: i32, planner_graph: Option, latest_start: Option<(f32, f32, f32)>, @@ -104,14 +106,14 @@ impl MlsPlanner { ); } - self.height_cells = (cfg.robot_height / cfg.voxel_size).ceil() as i32; + self.clearance_cells = (cfg.robot_height / cfg.voxel_size).ceil() as i32; self.step_cells = (cfg.node_step_threshold_m / cfg.voxel_size).floor() as i32; info!( world_frame = %cfg.world_frame, voxel_size = cfg.voxel_size, robot_height = cfg.robot_height, - height_cells = self.height_cells, + clearance_cells = self.clearance_cells, step_cells = self.step_cells, "mls_planner ready", ); @@ -134,13 +136,20 @@ impl MlsPlanner { } let cfg = &self.config; + + // convert whatever map we got in to voxels + let voxel_map: AHashSet = points + .iter() + .map(|&p| voxelize(p, cfg.voxel_size)) + .collect(); + let surface_cells = extract_surfaces( - &points, - cfg.voxel_size, - self.height_cells, + &voxel_map, + self.clearance_cells, cfg.surface_dilation_passes, cfg.surface_erosion_passes, ); + let sg = place_nodes( &surface_cells, cfg.voxel_size, @@ -148,11 +157,13 @@ impl MlsPlanner { cfg.node_spacing_m, cfg.node_wall_buffer_m, ); + let n_nodes = sg.nodes.len(); let plg = add_node_edges(sg); let n_edges = plg.node_edges.len(); info!( obstacle_points = points.len(), + obstacle_voxels = voxel_map.len(), surface_cells = surface_cells.len(), nodes = n_nodes, edges = n_edges, diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs index 5c5c07bcbc..2e5dcdfb3c 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs @@ -31,7 +31,7 @@ pub struct SurfaceGraph { pub fn place_nodes( surface_cells: &[VoxelKey], voxel_size: f32, - step_cells: i32, + maximum_step_cells: i32, node_spacing_m: f32, node_wall_buffer_m: f32, ) -> SurfaceGraph { @@ -40,7 +40,7 @@ pub fn place_nodes( adj, idx_to_cell, cell_to_idx, - } = build_surface_adjacency(&surface_lookup, voxel_size, step_cells); + } = build_surface_adjacency(&surface_lookup, voxel_size, maximum_step_cells); let n = adj.n as usize; if n == 0 { diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs index 6f432d20ee..0545a4fe1f 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs @@ -1,9 +1,9 @@ // Copyright 2026 Dimensional Inc. // SPDX-License-Identifier: Apache-2.0 -//! Surface extraction: voxelize a cloud, mark cells with robot-height clearance -//! above as standable, then morphologically close per-z-level holes without -//! letting closing bridge across walls. +//! Surface extraction: from a voxelized obstacle set, mark cells with +//! robot-height clearance above as standable, then morphologically close +//! per-z-level holes without letting closing bridge across walls. #![allow(dead_code)] // consumed incrementally @@ -17,59 +17,48 @@ use crate::voxel::VoxelKey; const ON: Luma = Luma([255]); const OFF: Luma = Luma([0]); -#[inline] -fn voxelize(p: (f32, f32, f32), voxel_size: f32) -> VoxelKey { - let inv = 1.0 / voxel_size; - ( - (p.0 * inv).floor() as i32, - (p.1 * inv).floor() as i32, - (p.2 * inv).floor() as i32, - ) -} - -/// A cell (ix, iy, iz) is standable iff its column has no obstacle -/// within the height of the robot. +/// A cell is standable if it has at least the robots height of clear +/// space above it. fn is_standable( ix: i32, iy: i32, iz: i32, - obstacles: &AHashSet, - height_cells: i32, + voxel_map: &AHashSet, + clearance_cells: i32, ) -> bool { - for dz in 1..=height_cells { - if obstacles.contains(&(ix, iy, iz + dz)) { + for dz in 1..=clearance_cells { + if voxel_map.contains(&(ix, iy, iz + dz)) { return false; } } true } -/// Extract standable cells from a point cloud, then close small holes. -/// -/// Returns cell indices. +/// Extract standable cells from the voxelized global map, then close small +/// holes. Clearance height given as number of cells. pub fn extract_surfaces( - points: &[(f32, f32, f32)], - voxel_size: f32, - height_cells: i32, + voxel_map: &AHashSet, + clearance_cells: i32, dilation_passes: u32, erosion_passes: u32, ) -> Vec { - if points.is_empty() { + if voxel_map.is_empty() { return Vec::new(); } - let obstacles: AHashSet = points.iter().map(|&p| voxelize(p, voxel_size)).collect(); - + // bin voxels in to their columns let mut by_col: AHashMap<(i32, i32), Vec> = AHashMap::new(); - for &(ix, iy, iz) in &obstacles { + for &(ix, iy, iz) in voxel_map { by_col.entry((ix, iy)).or_default().push(iz); } let mut standable: Vec = Vec::new(); for ((ix, iy), zs) in by_col.iter_mut() { zs.sort_unstable(); + + // find gaps of at least clearance height through the column for w in zs.windows(2) { - if w[1] - w[0] > height_cells { + if w[1] - w[0] > clearance_cells { standable.push((*ix, *iy, w[0])); } } @@ -80,24 +69,27 @@ pub fn extract_surfaces( close_surface_holes( standable, - &obstacles, + voxel_map, dilation_passes, erosion_passes, - height_cells, + clearance_cells, ) } +/// Perform dilation and erosion on all xy slices of the extracted surfaces +/// to fill in small holes. fn close_surface_holes( standable: Vec, obstacles: &AHashSet, dilation_passes: u32, erosion_passes: u32, - height_cells: i32, + clearance_cells: i32, ) -> Vec { if standable.is_empty() || (dilation_passes == 0 && erosion_passes == 0) { return standable; } + // slice the surfaces in to xy planes so we can do the 2d morphology let mut by_z: AHashMap> = AHashMap::new(); for &(ix, iy, iz) in &standable { by_z.entry(iz).or_default().push((ix, iy)); @@ -111,19 +103,20 @@ fn close_surface_holes( obstacles, dilation_passes, erosion_passes, - height_cells, + clearance_cells, )); } out } +/// Close holes on an xy slice of the surfaces. fn close_at_z( xys: &[(i32, i32)], iz: i32, - obstacles: &AHashSet, + voxel_map: &AHashSet, dilation_passes: u32, erosion_passes: u32, - height_cells: i32, + clearance_cells: i32, ) -> Vec { let pad = dilation_passes as i32; let mut min_x = i32::MAX; @@ -142,12 +135,13 @@ fn close_at_z( let x0 = min_x - pad; let y0 = min_y - pad; + // we treat the xy slice as a binary image, either surface (on) or not surface (off) let mut img = GrayImage::from_pixel(w, h, OFF); for &(ix, iy) in xys { img.put_pixel((ix - x0) as u32, (iy - y0) as u32, ON); } - // L1 norm for 4 neighbor connectivity + // use L1 dilation/erosion, expand out in cross shape if dilation_passes > 0 { img = dilate(&img, Norm::L1, dilation_passes as u8); } @@ -163,7 +157,9 @@ fn close_at_z( } let ix = x0 + px as i32; let iy = y0 + py as i32; - if !is_standable(ix, iy, iz, obstacles, height_cells) { + + // filter out if the surface has expanded to any non standable areas + if !is_standable(ix, iy, iz, voxel_map, clearance_cells) { continue; } out.push((ix, iy, iz)); @@ -176,29 +172,32 @@ fn close_at_z( mod tests { use super::*; + fn voxel_map(cells: &[VoxelKey]) -> AHashSet { + cells.iter().copied().collect() + } + #[test] fn empty_input() { - assert!(extract_surfaces(&[], 1.0, 5, 0, 0).is_empty()); + assert!(extract_surfaces(&AHashSet::new(), 5, 0, 0).is_empty()); } #[test] - fn single_point_is_topmost_surface() { - let s = extract_surfaces(&[(0.5, 0.5, 0.5)], 1.0, 5, 0, 0); + fn single_cell_is_topmost_surface() { + let s = extract_surfaces(&voxel_map(&[(0, 0, 0)]), 5, 0, 0); assert_eq!(s, vec![(0, 0, 0)]); } #[test] - fn stacked_points_within_headroom_only_topmost_is_surface() { - let pts: Vec<(f32, f32, f32)> = (0..5).map(|z| (0.5, 0.5, z as f32 + 0.5)).collect(); - let s = extract_surfaces(&pts, 1.0, 5, 0, 0); + fn stacked_cells_within_headroom_only_topmost_is_surface() { + let cells: Vec = (0..5).map(|z| (0, 0, z)).collect(); + let s = extract_surfaces(&voxel_map(&cells), 5, 0, 0); assert_eq!(s, vec![(0, 0, 4)]); } #[test] fn gap_larger_than_headroom_makes_lower_cell_standable() { - // Obstacles at iz=0 and iz=10 with height_cells=5. Lower cell has gap=10 > 5. - let pts: Vec<(f32, f32, f32)> = vec![(0.5, 0.5, 0.5), (0.5, 0.5, 10.5)]; - let mut s = extract_surfaces(&pts, 1.0, 5, 0, 0); + // Obstacles at iz=0 and iz=10 with clearance_cells=5. Lower cell has gap=10 > 5. + let mut s = extract_surfaces(&voxel_map(&[(0, 0, 0), (0, 0, 10)]), 5, 0, 0); s.sort(); assert_eq!(s, vec![(0, 0, 0), (0, 0, 10)]); } @@ -206,8 +205,7 @@ mod tests { #[test] fn morphological_closing_fills_center_hole() { // Ring of 8 cells around (0,0) at iz=0, no obstacles above. - let mut pts: Vec<(f32, f32, f32)> = Vec::new(); - for (dx, dy) in [ + let cells: Vec = [ (-1, -1), (-1, 0), (-1, 1), @@ -216,10 +214,11 @@ mod tests { (1, -1), (1, 0), (1, 1), - ] { - pts.push((dx as f32 + 0.5, dy as f32 + 0.5, 0.5)); - } - let s = extract_surfaces(&pts, 1.0, 5, 3, 3); + ] + .into_iter() + .map(|(dx, dy)| (dx, dy, 0)) + .collect(); + let s = extract_surfaces(&voxel_map(&cells), 5, 3, 3); assert!( s.contains(&(0, 0, 0)), "closing should fill the center hole" @@ -230,8 +229,7 @@ mod tests { fn closing_does_not_bridge_obstacle_in_headroom() { // Ring of 8 cells at iz=0 + an obstacle directly above the hole at (0,0,1). // The hole at (0,0,0) is vetoed because its headroom column has an obstacle. - let mut pts: Vec<(f32, f32, f32)> = Vec::new(); - for (dx, dy) in [ + let mut cells: Vec = [ (-1, -1), (-1, 0), (-1, 1), @@ -240,11 +238,12 @@ mod tests { (1, -1), (1, 0), (1, 1), - ] { - pts.push((dx as f32 + 0.5, dy as f32 + 0.5, 0.5)); - } - pts.push((0.5, 0.5, 1.5)); - let s = extract_surfaces(&pts, 1.0, 5, 3, 3); + ] + .into_iter() + .map(|(dx, dy)| (dx, dy, 0)) + .collect(); + cells.push((0, 0, 1)); + let s = extract_surfaces(&voxel_map(&cells), 5, 3, 3); assert!(!s.contains(&(0, 0, 0))); } } diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/voxel.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/voxel.rs index 302614d7d3..353d728905 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/voxel.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/voxel.rs @@ -7,6 +7,16 @@ pub type VoxelKey = (i32, i32, i32); +#[inline] +pub fn voxelize(p: (f32, f32, f32), voxel_size: f32) -> VoxelKey { + let inv = 1.0 / voxel_size; + ( + (p.0 * inv).floor() as i32, + (p.1 * inv).floor() as i32, + (p.2 * inv).floor() as i32, + ) +} + /// XY centered in the cell, Z at the cell's top face. #[inline] pub fn surface_point_xyz(ix: i32, iy: i32, iz: i32, voxel_size: f32) -> (f32, f32, f32) { From dd74e050e7356fe34df0dcac2491498e9e87a60c Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Fri, 29 May 2026 12:40:08 -0700 Subject: [PATCH 37/55] Delete bad code, implement good code (hashmap for surface adjacency) --- .../modules/mls_planner/rust/src/adjacency.rs | 208 ++++++++---------- .../modules/mls_planner/rust/src/dijkstra.rs | 204 +++++++++-------- .../modules/mls_planner/rust/src/edges.rs | 129 +++++------ .../modules/mls_planner/rust/src/nodes.rs | 161 ++++++-------- .../modules/mls_planner/rust/src/plan.rs | 90 ++++---- 5 files changed, 370 insertions(+), 422 deletions(-) diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/adjacency.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/adjacency.rs index 057f194dd1..4ac3c7081f 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/adjacency.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/adjacency.rs @@ -1,10 +1,6 @@ // Copyright 2026 Dimensional Inc. // SPDX-License-Identifier: Apache-2.0 -//! Per-column surface lookup and 4-connected adjacency over surface cells. -//! -//! Wall-safe cost smoothing in nodes.rs should make paths equivalent. - #![allow(dead_code)] // consumed incrementally by later stage modules use ahash::AHashMap; @@ -15,20 +11,52 @@ pub type SurfaceLookup = AHashMap<(i32, i32), Vec>; const NEIGHBORS_4: [(i32, i32); 4] = [(-1, 0), (1, 0), (0, -1), (0, 1)]; -pub struct CsrAdjacency { - pub indptr: Vec, - pub indices: Vec, - pub data: Vec, - pub n: u32, +#[derive(Clone, Copy, Debug)] +pub struct Edge { + pub dst: VoxelKey, + pub cost: f32, } +#[derive(Default)] pub struct SurfaceAdjacency { - pub adj: CsrAdjacency, - pub idx_to_cell: Vec, - pub cell_to_idx: AHashMap, + cells: AHashMap>, +} + +impl SurfaceAdjacency { + pub fn new() -> Self { + Self::default() + } + + pub fn add_cell(&mut self, cell: VoxelKey) { + self.cells.entry(cell).or_default(); + } + + pub fn add_edge(&mut self, src: VoxelKey, dst: VoxelKey, cost: f32) { + self.cells.entry(src).or_default().push(Edge { dst, cost }); + } + + pub fn neighbors(&self, cell: VoxelKey) -> impl Iterator + '_ { + self.cells.get(&cell).into_iter().flatten().copied() + } + + pub fn cells(&self) -> impl Iterator + '_ { + self.cells.keys().copied() + } + + pub fn contains(&self, cell: VoxelKey) -> bool { + self.cells.contains_key(&cell) + } + + pub fn len(&self) -> usize { + self.cells.len() + } + + pub fn is_empty(&self) -> bool { + self.cells.is_empty() + } } -/// Group cells in to xy columns and sort their z indexes. +/// Group cells by XY column with sorted unique iz per column. pub fn build_surface_lookup(cells: &[VoxelKey]) -> SurfaceLookup { let mut lookup: SurfaceLookup = AHashMap::new(); for &(ix, iy, iz) in cells { @@ -41,66 +69,36 @@ pub fn build_surface_lookup(cells: &[VoxelKey]) -> SurfaceLookup { lookup } -/// 4-connected XY adjacency with per-step dz cap. Cell ordering is -/// lex-sorted by column then iz so the output is deterministic across runs. +/// 4 way connection (L1) and check for step height threshold pub fn build_surface_adjacency( surface_lookup: &SurfaceLookup, voxel_size: f32, step_threshold_cells: i32, ) -> SurfaceAdjacency { - let mut columns: Vec<(i32, i32)> = surface_lookup.keys().copied().collect(); - columns.sort_unstable(); - - // assign ids to each cell - let mut idx_to_cell: Vec = Vec::new(); - for &(ix, iy) in &columns { - for &iz in &surface_lookup[&(ix, iy)] { - idx_to_cell.push((ix, iy, iz)); + let mut adj = SurfaceAdjacency::new(); + for (&(ix, iy), zs) in surface_lookup { + for &iz in zs { + adj.add_cell((ix, iy, iz)); } } - let n = idx_to_cell.len(); - - // also build the reverse so cells can look up their id - let cell_to_idx: AHashMap = idx_to_cell - .iter() - .enumerate() - .map(|(i, &c)| (c, i as u32)) - .collect(); - - let mut indptr: Vec = Vec::with_capacity(n + 1); - indptr.push(0); - let mut indices: Vec = Vec::new(); - let mut data: Vec = Vec::new(); - - for &(ix, iy, iz) in &idx_to_cell { - for (dx, dy) in NEIGHBORS_4 { - let Some(zs) = surface_lookup.get(&(ix + dx, iy + dy)) else { - continue; - }; - for &nz in zs { - let dz = nz - iz; - if dz.abs() > step_threshold_cells { + for (&(ix, iy), zs) in surface_lookup { + for &iz in zs { + for (dx, dy) in NEIGHBORS_4 { + let Some(nzs) = surface_lookup.get(&(ix + dx, iy + dy)) else { continue; + }; + for &nz in nzs { + let dz = nz - iz; + if dz.abs() > step_threshold_cells { + continue; + } + let cost = ((dx * dx + dy * dy + dz * dz) as f32).sqrt() * voxel_size; + adj.add_edge((ix, iy, iz), (ix + dx, iy + dy, nz), cost); } - let dst = cell_to_idx[&(ix + dx, iy + dy, nz)]; - let cost = ((dx * dx + dy * dy + dz * dz) as f32).sqrt() * voxel_size; - indices.push(dst); - data.push(cost); } } - indptr.push(indices.len() as u32); - } - - SurfaceAdjacency { - adj: CsrAdjacency { - indptr, - indices, - data, - n: n as u32, - }, - idx_to_cell, - cell_to_idx, } + adj } #[cfg(test)] @@ -114,102 +112,90 @@ mod tests { assert!((a - b).abs() < eps, "{a} != {b} (eps {eps})"); } - fn edges(sa: &SurfaceAdjacency) -> Vec<(VoxelKey, VoxelKey, f32)> { - let mut out = Vec::new(); - for src in 0..sa.adj.n as usize { - let lo = sa.adj.indptr[src] as usize; - let hi = sa.adj.indptr[src + 1] as usize; - for k in lo..hi { - let dst = sa.adj.indices[k] as usize; - out.push((sa.idx_to_cell[src], sa.idx_to_cell[dst], sa.adj.data[k])); - } - } - out + fn neighbors_of(adj: &SurfaceAdjacency, cell: VoxelKey) -> Vec { + adj.neighbors(cell).collect() } #[test] fn empty_input_yields_empty_adjacency() { let lookup = build_surface_lookup(&[]); - let sa = build_surface_adjacency(&lookup, VOXEL, 2); - assert_eq!(sa.adj.n, 0); - assert_eq!(sa.adj.indptr, vec![0]); - assert!(sa.adj.indices.is_empty()); - assert!(sa.adj.data.is_empty()); - assert!(sa.idx_to_cell.is_empty()); + let adj = build_surface_adjacency(&lookup, VOXEL, 2); + assert_eq!(adj.len(), 0); + assert!(adj.is_empty()); } #[test] fn single_cell_has_no_edges() { let lookup = build_surface_lookup(&[(0, 0, 0)]); - let sa = build_surface_adjacency(&lookup, VOXEL, 2); - assert_eq!(sa.adj.n, 1); - assert_eq!(sa.adj.indptr, vec![0, 0]); - assert!(sa.adj.indices.is_empty()); + let adj = build_surface_adjacency(&lookup, VOXEL, 2); + assert_eq!(adj.len(), 1); + assert!(adj.contains((0, 0, 0))); + assert!(neighbors_of(&adj, (0, 0, 0)).is_empty()); } #[test] fn same_z_neighbors_are_bidirectional() { let lookup = build_surface_lookup(&[(0, 0, 0), (1, 0, 0)]); - let sa = build_surface_adjacency(&lookup, VOXEL, 2); - assert_eq!(sa.adj.indices.len(), 2); - for e in edges(&sa) { - approx_eq(e.2, VOXEL); - } + let adj = build_surface_adjacency(&lookup, VOXEL, 2); + let a = neighbors_of(&adj, (0, 0, 0)); + let b = neighbors_of(&adj, (1, 0, 0)); + assert_eq!(a.len(), 1); + assert_eq!(b.len(), 1); + assert_eq!(a[0].dst, (1, 0, 0)); + assert_eq!(b[0].dst, (0, 0, 0)); + approx_eq(a[0].cost, VOXEL); + approx_eq(b[0].cost, VOXEL); } #[test] fn diagonal_not_connected_under_4_connectivity() { let lookup = build_surface_lookup(&[(0, 0, 0), (1, 1, 0)]); - let sa = build_surface_adjacency(&lookup, VOXEL, 2); - assert!( - sa.adj.indices.is_empty(), - "diagonal must not connect under 4-connectivity" - ); + let adj = build_surface_adjacency(&lookup, VOXEL, 2); + assert!(neighbors_of(&adj, (0, 0, 0)).is_empty()); + assert!(neighbors_of(&adj, (1, 1, 0)).is_empty()); } #[test] fn step_threshold_blocks_large_dz() { let lookup = build_surface_lookup(&[(0, 0, 0), (1, 0, 5)]); - let sa = build_surface_adjacency(&lookup, VOXEL, 2); - assert!( - sa.adj.indices.is_empty(), - "dz=5 must not connect when step_threshold=2" - ); + let adj = build_surface_adjacency(&lookup, VOXEL, 2); + assert!(neighbors_of(&adj, (0, 0, 0)).is_empty()); + assert!(neighbors_of(&adj, (1, 0, 5)).is_empty()); } #[test] fn step_within_threshold_uses_3d_distance() { let lookup = build_surface_lookup(&[(0, 0, 0), (1, 0, 1)]); - let sa = build_surface_adjacency(&lookup, VOXEL, 2); - assert_eq!(sa.adj.indices.len(), 2); + let adj = build_surface_adjacency(&lookup, VOXEL, 2); let expected = (2.0_f32).sqrt() * VOXEL; - for e in edges(&sa) { - approx_eq(e.2, expected); - } + let a = neighbors_of(&adj, (0, 0, 0)); + let b = neighbors_of(&adj, (1, 0, 1)); + assert_eq!(a.len(), 1); + assert_eq!(b.len(), 1); + approx_eq(a[0].cost, expected); + approx_eq(b[0].cost, expected); } #[test] fn same_column_cells_are_not_self_connected() { let lookup = build_surface_lookup(&[(0, 0, 0), (0, 0, 5)]); - let sa = build_surface_adjacency(&lookup, VOXEL, 10); - assert!(sa.adj.indices.is_empty()); + let adj = build_surface_adjacency(&lookup, VOXEL, 10); + assert!(neighbors_of(&adj, (0, 0, 0)).is_empty()); + assert!(neighbors_of(&adj, (0, 0, 5)).is_empty()); } #[test] fn plus_pattern_center_has_four_neighbors() { let cells = vec![(0, 0, 0), (1, 0, 0), (-1, 0, 0), (0, 1, 0), (0, -1, 0)]; let lookup = build_surface_lookup(&cells); - let sa = build_surface_adjacency(&lookup, VOXEL, 2); - let center_idx = sa.cell_to_idx[&(0, 0, 0)] as usize; - let lo = sa.adj.indptr[center_idx] as usize; - let hi = sa.adj.indptr[center_idx + 1] as usize; - assert_eq!(hi - lo, 4); + let adj = build_surface_adjacency(&lookup, VOXEL, 2); + assert_eq!(neighbors_of(&adj, (0, 0, 0)).len(), 4); } #[test] fn deduplicates_repeated_cells() { let lookup = build_surface_lookup(&[(0, 0, 0), (0, 0, 0), (1, 0, 0)]); - let sa = build_surface_adjacency(&lookup, VOXEL, 2); - assert_eq!(sa.adj.n, 2); + let adj = build_surface_adjacency(&lookup, VOXEL, 2); + assert_eq!(adj.len(), 2); } } diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/dijkstra.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/dijkstra.rs index 376511f0e2..a8631999a3 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/dijkstra.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/dijkstra.rs @@ -1,58 +1,62 @@ // Copyright 2026 Dimensional Inc. // SPDX-License-Identifier: Apache-2.0 -//! Multi-source Dijkstra over the CSR adjacency. +//! Multi-source Dijkstra over the cell-keyed surface adjacency. //! -//! Tracks the path taken and distance for each cell. This can be used to -//! reconstruct shortest paths to any of the source cells. +//! Tracks distance, predecessor, and nearest-source label for each reached +//! cell. Cells absent from the result are unreachable from every source. #![allow(dead_code)] use std::cmp::Ordering; use std::collections::BinaryHeap; -use crate::adjacency::CsrAdjacency; +use ahash::AHashMap; + +use crate::adjacency::SurfaceAdjacency; +use crate::voxel::VoxelKey; pub struct DijkstraResult { - pub dist: Vec, - /// Predecessor cell along the shortest path back to a source. -1 marks - /// source cells and unreachable cells. Used downstream to reconstruct - /// cell-by-cell paths lazily. - pub pred: Vec, - /// Index into the caller's `sources` slice. When the caller passes node - /// cells in node-id order this doubles as the nearest-node id, which is - /// the Voronoi partition. -1 for unreachable cells. - pub source: Vec, + pub dist: AHashMap, + /// Predecessor cell along the shortest path back to a source. + pub pred: AHashMap, + /// Id of the cheapest source to return to + pub source: AHashMap, } -pub fn dijkstra(adj: &CsrAdjacency, sources: &[u32]) -> DijkstraResult { - let n = adj.n as usize; - let mut dist = vec![f32::INFINITY; n]; - let mut pred = vec![-1i32; n]; - let mut source = vec![-1i32; n]; +pub fn dijkstra(adj: &SurfaceAdjacency, sources: &[VoxelKey]) -> DijkstraResult { + let mut dist: AHashMap = AHashMap::new(); + let mut pred: AHashMap = AHashMap::new(); + let mut source: AHashMap = AHashMap::new(); let mut heap: BinaryHeap = BinaryHeap::new(); for (label, &s) in sources.iter().enumerate() { - let su = s as usize; - dist[su] = 0.0; - source[su] = label as i32; + if !adj.contains(s) { + continue; + } + dist.insert(s, 0.0); + source.insert(s, label as u32); heap.push(Scored(0.0, s)); } while let Some(Scored(d, u)) = heap.pop() { - if d > dist[u as usize] { - continue; + if let Some(&cur) = dist.get(&u) { + if d > cur { + continue; + } } - let lo = adj.indptr[u as usize] as usize; - let hi = adj.indptr[u as usize + 1] as usize; - for k in lo..hi { - let v = adj.indices[k]; - let nd = d + adj.data[k]; - if nd < dist[v as usize] { - dist[v as usize] = nd; - pred[v as usize] = u as i32; - source[v as usize] = source[u as usize]; - heap.push(Scored(nd, v)); + let su = source[&u]; + for edge in adj.neighbors(u) { + let nd = d + edge.cost; + let should_update = match dist.get(&edge.dst) { + None => true, + Some(&cur) => nd < cur, + }; + if should_update { + dist.insert(edge.dst, nd); + pred.insert(edge.dst, u); + source.insert(edge.dst, su); + heap.push(Scored(nd, edge.dst)); } } } @@ -60,7 +64,7 @@ pub fn dijkstra(adj: &CsrAdjacency, sources: &[u32]) -> DijkstraResult { DijkstraResult { dist, pred, source } } -struct Scored(f32, u32); +struct Scored(f32, VoxelKey); impl PartialEq for Scored { fn eq(&self, other: &Self) -> bool { @@ -84,63 +88,43 @@ impl Ord for Scored { mod tests { use super::*; - /// Build a CSR from a Vec of (src, dst, cost) edges over n cells. - /// Edges are kept directed; caller emits both directions for an undirected graph. - fn csr(n: u32, mut edges: Vec<(u32, u32, f32)>) -> CsrAdjacency { - edges.sort_by_key(|&(s, _, _)| s); - let mut indptr = vec![0u32; (n + 1) as usize]; - for &(s, _, _) in &edges { - indptr[s as usize + 1] += 1; - } - for i in 1..indptr.len() { - indptr[i] += indptr[i - 1]; - } - let indices: Vec = edges.iter().map(|&(_, d, _)| d).collect(); - let data: Vec = edges.iter().map(|&(_, _, w)| w).collect(); - CsrAdjacency { - indptr, - indices, - data, - n, + fn chain(n: i32) -> SurfaceAdjacency { + let mut adj = SurfaceAdjacency::new(); + for i in 0..n { + adj.add_cell((i, 0, 0)); } - } - - /// Bidirectional chain 0 - 1 - 2 - 3 - 4 with unit edge cost. - fn chain(n: u32) -> CsrAdjacency { - let mut edges = Vec::new(); for i in 0..n - 1 { - edges.push((i, i + 1, 1.0)); - edges.push((i + 1, i, 1.0)); + adj.add_edge((i, 0, 0), (i + 1, 0, 0), 1.0); + adj.add_edge((i + 1, 0, 0), (i, 0, 0), 1.0); } - csr(n, edges) + adj } #[test] fn empty_sources_leaves_everything_unreachable() { let adj = chain(4); let r = dijkstra(&adj, &[]); - for i in 0..4 { - assert!(r.dist[i].is_infinite()); - assert_eq!(r.pred[i], -1); - assert_eq!(r.source[i], -1); - } + assert!(r.dist.is_empty()); + assert!(r.pred.is_empty()); + assert!(r.source.is_empty()); } #[test] fn single_source_dist_and_pred() { let adj = chain(5); - let r = dijkstra(&adj, &[0]); - assert_eq!(r.dist, vec![0.0, 1.0, 2.0, 3.0, 4.0]); - assert_eq!(r.source, vec![0, 0, 0, 0, 0]); - // Predecessor chain walks back to the source. - assert_eq!(r.pred[0], -1); - let mut cur: i32 = 4; + let r = dijkstra(&adj, &[(0, 0, 0)]); + for i in 0..5 { + assert_eq!(r.dist[&(i, 0, 0)], i as f32); + assert_eq!(r.source[&(i, 0, 0)], 0); + } + assert!(!r.pred.contains_key(&(0, 0, 0))); + let mut cur = (4, 0, 0); let mut hops = 0; - while r.pred[cur as usize] >= 0 { - cur = r.pred[cur as usize]; + while let Some(&p) = r.pred.get(&cur) { + cur = p; hops += 1; } - assert_eq!(cur, 0); + assert_eq!(cur, (0, 0, 0)); assert_eq!(hops, 4); } @@ -149,46 +133,56 @@ mod tests { // Sources at 0 and 4 on a 5-cell chain. Cells 0-1 closer to source 0, // cells 3-4 closer to source 1. Cell 2 is equidistant. let adj = chain(5); - let r = dijkstra(&adj, &[0, 4]); - assert_eq!(r.source[0], 0); - assert_eq!(r.source[1], 0); - assert_eq!(r.source[3], 1); - assert_eq!(r.source[4], 1); - // Cell 2 labeling is implementation-defined for ties but must be one of the two. - assert!(r.source[2] == 0 || r.source[2] == 1); - assert_eq!(r.dist, vec![0.0, 1.0, 2.0, 1.0, 0.0]); + let r = dijkstra(&adj, &[(0, 0, 0), (4, 0, 0)]); + assert_eq!(r.source[&(0, 0, 0)], 0); + assert_eq!(r.source[&(1, 0, 0)], 0); + assert_eq!(r.source[&(3, 0, 0)], 1); + assert_eq!(r.source[&(4, 0, 0)], 1); + // Tie at cell 2 must resolve to one of the two sources. + let s2 = r.source[&(2, 0, 0)]; + assert!(s2 == 0 || s2 == 1); + assert_eq!(r.dist[&(0, 0, 0)], 0.0); + assert_eq!(r.dist[&(1, 0, 0)], 1.0); + assert_eq!(r.dist[&(2, 0, 0)], 2.0); + assert_eq!(r.dist[&(3, 0, 0)], 1.0); + assert_eq!(r.dist[&(4, 0, 0)], 0.0); } #[test] fn disconnected_cells_stay_unreachable() { // Two separate chains 0-1 and 2-3, source at 0. - let edges = vec![(0, 1, 1.0), (1, 0, 1.0), (2, 3, 1.0), (3, 2, 1.0)]; - let adj = csr(4, edges); - let r = dijkstra(&adj, &[0]); - assert_eq!(r.dist[0], 0.0); - assert_eq!(r.dist[1], 1.0); - assert!(r.dist[2].is_infinite()); - assert!(r.dist[3].is_infinite()); - assert_eq!(r.source[2], -1); - assert_eq!(r.source[3], -1); - assert_eq!(r.pred[2], -1); - assert_eq!(r.pred[3], -1); + let mut adj = SurfaceAdjacency::new(); + for &c in &[(0, 0, 0), (1, 0, 0), (2, 0, 0), (3, 0, 0)] { + adj.add_cell(c); + } + adj.add_edge((0, 0, 0), (1, 0, 0), 1.0); + adj.add_edge((1, 0, 0), (0, 0, 0), 1.0); + adj.add_edge((2, 0, 0), (3, 0, 0), 1.0); + adj.add_edge((3, 0, 0), (2, 0, 0), 1.0); + let r = dijkstra(&adj, &[(0, 0, 0)]); + assert_eq!(r.dist[&(0, 0, 0)], 0.0); + assert_eq!(r.dist[&(1, 0, 0)], 1.0); + assert!(!r.dist.contains_key(&(2, 0, 0))); + assert!(!r.dist.contains_key(&(3, 0, 0))); + assert!(!r.source.contains_key(&(2, 0, 0))); + assert!(!r.pred.contains_key(&(2, 0, 0))); } #[test] fn shorter_path_overrides_longer() { // 0 - 1 with cost 10, 0 - 2 - 1 with cost 1+1=2. - let edges = vec![ - (0, 1, 10.0), - (1, 0, 10.0), - (0, 2, 1.0), - (2, 0, 1.0), - (2, 1, 1.0), - (1, 2, 1.0), - ]; - let adj = csr(3, edges); - let r = dijkstra(&adj, &[0]); - assert_eq!(r.dist[1], 2.0); - assert_eq!(r.pred[1], 2); // came via 2, not directly + let mut adj = SurfaceAdjacency::new(); + for &c in &[(0, 0, 0), (1, 0, 0), (2, 0, 0)] { + adj.add_cell(c); + } + adj.add_edge((0, 0, 0), (1, 0, 0), 10.0); + adj.add_edge((1, 0, 0), (0, 0, 0), 10.0); + adj.add_edge((0, 0, 0), (2, 0, 0), 1.0); + adj.add_edge((2, 0, 0), (0, 0, 0), 1.0); + adj.add_edge((2, 0, 0), (1, 0, 0), 1.0); + adj.add_edge((1, 0, 0), (2, 0, 0), 1.0); + let r = dijkstra(&adj, &[(0, 0, 0)]); + assert_eq!(r.dist[&(1, 0, 0)], 2.0); + assert_eq!(r.pred[&(1, 0, 0)], (2, 0, 0)); } } diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/edges.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/edges.rs index 10ae192203..c8bc24d953 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/edges.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/edges.rs @@ -3,20 +3,16 @@ //! Node-graph edge construction. //! -//! Run multi-source Dijkstra from every node cell to partition the surface -//! into Voronoi regions (one per node). Walk every cell-level edge that -//! straddles a region boundary; for each ordered node pair keep the cheapest -//! crossing as a node-level edge. -//! -//! Each `NodeEdge` stores only the two boundary cells and the total cost. -//! The cell-by-cell path along an edge is reconstructed lazily in plan.rs -//! by walking the saved predecessor array. +//! Build edges by running multi-source Dijkstra from all the start nodes. +//! This labels the surface with each cells closest source, also known as +//! the Voronoi region. We use the boundaries of these regions to build the +//! edges between start nodes. #![allow(dead_code)] use ahash::AHashMap; -use crate::adjacency::{CsrAdjacency, SurfaceLookup}; +use crate::adjacency::{SurfaceAdjacency, SurfaceLookup}; use crate::dijkstra::{dijkstra, DijkstraResult}; use crate::nodes::{NodeData, SurfaceGraph}; use crate::voxel::VoxelKey; @@ -26,52 +22,40 @@ pub struct NodeEdge { pub b: u32, pub cost: f32, /// Cell on a's side of the cheapest Voronoi boundary crossing. - pub boundary_u: u32, + pub boundary_u: VoxelKey, /// Cell on b's side. - pub boundary_v: u32, + pub boundary_v: VoxelKey, } pub struct PlannerGraph { - pub adj: CsrAdjacency, - pub idx_to_cell: Vec, - pub cell_to_idx: AHashMap, + pub adj: SurfaceAdjacency, pub surface_lookup: SurfaceLookup, pub nodes: Vec, pub node_edges: Vec, - /// node_id -> indices into `node_edges`. Adjacency for the node-level - /// shortest-path query in Stage D. pub node_adj: Vec>, - /// Per-cell predecessor along the multi-source Dijkstra from node seeds. - /// -1 marks source cells and unreachable cells. Used by plan.rs to walk - /// cell paths for the edges on a chosen route. - pub cell_predecessors: Vec, + pub cell_predecessors: AHashMap, } pub fn add_node_edges(sg: SurfaceGraph) -> PlannerGraph { let SurfaceGraph { adj, - idx_to_cell, - cell_to_idx, surface_lookup, nodes, } = sg; if nodes.is_empty() { - let n = adj.n as usize; return PlannerGraph { adj, - idx_to_cell, - cell_to_idx, surface_lookup, nodes, node_edges: Vec::new(), node_adj: Vec::new(), - cell_predecessors: vec![-1; n], + cell_predecessors: AHashMap::new(), }; } - let source_indices: Vec = nodes.iter().map(|n| cell_to_idx[&n.cell]).collect(); - let DijkstraResult { dist, pred, source } = dijkstra(&adj, &source_indices); + let source_cells: Vec = nodes.iter().map(|n| n.cell).collect(); + let DijkstraResult { dist, pred, source } = dijkstra(&adj, &source_cells); let node_edges = best_boundary_edges(&adj, &dist, &source); let mut node_adj: Vec> = vec![Vec::new(); nodes.len()]; @@ -82,8 +66,6 @@ pub fn add_node_edges(sg: SurfaceGraph) -> PlannerGraph { PlannerGraph { adj, - idx_to_cell, - cell_to_idx, surface_lookup, nodes, node_edges, @@ -93,8 +75,7 @@ pub fn add_node_edges(sg: SurfaceGraph) -> PlannerGraph { } /// Walk every node-graph edge and emit one segment per consecutive cell pair -/// along the reconstructed cell path. Each segment carries the edge's total -/// cost as its traversability so renderers can color the whole edge uniformly. +/// along the reconstructed cell path. pub fn edges_to_segments(plg: &PlannerGraph, _voxel_size: f32) -> Vec<(VoxelKey, VoxelKey, f32)> { let mut segments = Vec::new(); for edge in &plg.node_edges { @@ -110,49 +91,51 @@ pub fn edges_to_segments(plg: &PlannerGraph, _voxel_size: f32) -> Vec<(VoxelKey, segments } -/// Walk the cell-predecessor array from `start_cell` back to its owning source. -/// Returns cells in walk order (start_cell first, source cell last). -pub fn walk_preds_to_source(plg: &PlannerGraph, start_cell: u32) -> Vec { - let mut cells = vec![plg.idx_to_cell[start_cell as usize]]; - let mut cur = start_cell as i32; - while plg.cell_predecessors[cur as usize] >= 0 { - cur = plg.cell_predecessors[cur as usize]; - cells.push(plg.idx_to_cell[cur as usize]); +pub fn walk_preds_to_source(plg: &PlannerGraph, start_cell: VoxelKey) -> Vec { + let mut cells = vec![start_cell]; + let mut cur = start_cell; + while let Some(&p) = plg.cell_predecessors.get(&cur) { + cur = p; + cells.push(cur); } cells } -fn best_boundary_edges(adj: &CsrAdjacency, dist: &[f32], source: &[i32]) -> Vec { +fn best_boundary_edges( + adj: &SurfaceAdjacency, + dist: &AHashMap, + source: &AHashMap, +) -> Vec { let mut best: AHashMap<(u32, u32), NodeEdge> = AHashMap::new(); - for u in 0..adj.n as usize { - let sa = source[u]; - if sa < 0 { + for u in adj.cells() { + let Some(&sa) = source.get(&u) else { continue; - } - let lo = adj.indptr[u] as usize; - let hi = adj.indptr[u + 1] as usize; - for k in lo..hi { - let v = adj.indices[k] as usize; - let sb = source[v]; - if sb < 0 || sa == sb { + }; + let du = dist[&u]; + for edge in adj.neighbors(u) { + let v = edge.dst; + let Some(&sb) = source.get(&v) else { + continue; + }; + if sa == sb { continue; } - let edge_w = adj.data[k]; - let cost = dist[u] + edge_w + dist[v]; + let dv = dist[&v]; + let cost = du + edge.cost + dv; - let (key_a, key_b, bu, bv) = if (sa as u32) < (sb as u32) { - (sa as u32, sb as u32, u as u32, v as u32) + let (key_a, key_b, bu, bv) = if sa < sb { + (sa, sb, u, v) } else { - (sb as u32, sa as u32, v as u32, u as u32) + (sb, sa, v, u) }; let entry = best.entry((key_a, key_b)).or_insert(NodeEdge { a: key_a, b: key_b, cost: f32::INFINITY, - boundary_u: 0, - boundary_v: 0, + boundary_u: (0, 0, 0), + boundary_v: (0, 0, 0), }); if cost < entry.cost { entry.cost = cost; @@ -170,7 +153,7 @@ fn best_boundary_edges(adj: &CsrAdjacency, dist: &[f32], source: &[i32]) -> Vec< #[cfg(test)] mod tests { use super::*; - use crate::adjacency::{build_surface_adjacency, build_surface_lookup, SurfaceAdjacency}; + use crate::adjacency::{build_surface_adjacency, build_surface_lookup}; use crate::voxel::surface_point_xyz; const VOXEL: f32 = 0.1; @@ -180,11 +163,7 @@ mod tests { /// place_nodes so the test author controls which cells become nodes. fn graph_with_nodes(surface_cells: &[VoxelKey], node_cells: &[VoxelKey]) -> SurfaceGraph { let surface_lookup = build_surface_lookup(surface_cells); - let SurfaceAdjacency { - adj, - idx_to_cell, - cell_to_idx, - } = build_surface_adjacency(&surface_lookup, VOXEL, 2); + let adj = build_surface_adjacency(&surface_lookup, VOXEL, 2); let nodes: Vec = node_cells .iter() .map(|&c| NodeData { @@ -194,8 +173,6 @@ mod tests { .collect(); SurfaceGraph { adj, - idx_to_cell, - cell_to_idx, surface_lookup, nodes, } @@ -263,22 +240,22 @@ mod tests { assert_eq!(pg.node_edges.len(), 1); let e = &pg.node_edges[0]; - let cell_a = pg.cell_to_idx[&pg.nodes[0].cell]; - let cell_b = pg.cell_to_idx[&pg.nodes[1].cell]; + let cell_a = pg.nodes[0].cell; + let cell_b = pg.nodes[1].cell; - let mut cur = e.boundary_u as i32; + let mut cur = e.boundary_u; let mut hops = 0; - while pg.cell_predecessors[cur as usize] >= 0 { - cur = pg.cell_predecessors[cur as usize]; + while let Some(&p) = pg.cell_predecessors.get(&cur) { + cur = p; hops += 1; assert!(hops < 1000, "predecessor walk did not terminate"); } - assert_eq!(cur as u32, cell_a, "u-side preds must reach node a"); + assert_eq!(cur, cell_a, "u-side preds must reach node a"); - let mut cur = e.boundary_v as i32; - while pg.cell_predecessors[cur as usize] >= 0 { - cur = pg.cell_predecessors[cur as usize]; + let mut cur = e.boundary_v; + while let Some(&p) = pg.cell_predecessors.get(&cur) { + cur = p; } - assert_eq!(cur as u32, cell_b, "v-side preds must reach node b"); + assert_eq!(cur, cell_b, "v-side preds must reach node b"); } } diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs index 2e5dcdfb3c..c45dd2c7d5 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs @@ -10,7 +10,7 @@ use ahash::AHashMap; use crate::adjacency::{ - build_surface_adjacency, build_surface_lookup, CsrAdjacency, SurfaceAdjacency, SurfaceLookup, + build_surface_adjacency, build_surface_lookup, SurfaceAdjacency, SurfaceLookup, }; use crate::dijkstra::dijkstra; use crate::voxel::{surface_point_xyz, VoxelKey}; @@ -21,9 +21,7 @@ pub struct NodeData { } pub struct SurfaceGraph { - pub adj: CsrAdjacency, - pub idx_to_cell: Vec, - pub cell_to_idx: AHashMap, + pub adj: SurfaceAdjacency, pub surface_lookup: SurfaceLookup, pub nodes: Vec, } @@ -36,44 +34,38 @@ pub fn place_nodes( node_wall_buffer_m: f32, ) -> SurfaceGraph { let surface_lookup = build_surface_lookup(surface_cells); - let SurfaceAdjacency { - adj, - idx_to_cell, - cell_to_idx, - } = build_surface_adjacency(&surface_lookup, voxel_size, maximum_step_cells); - let n = adj.n as usize; + let adj = build_surface_adjacency(&surface_lookup, voxel_size, maximum_step_cells); - if n == 0 { + if adj.is_empty() { return SurfaceGraph { adj, - idx_to_cell, - cell_to_idx, surface_lookup, nodes: Vec::new(), }; } - let wall_seeds = wall_adjacent_cells(&adj, &idx_to_cell); + let wall_seeds = wall_adjacent_cells(&adj); let dist = dijkstra(&adj, &wall_seeds).dist; - let mut candidates: Vec = (0..n as u32) - .filter(|&i| { - let d = dist[i as usize]; - d.is_finite() && d >= node_wall_buffer_m + let mut candidates: Vec = dist + .iter() + .filter_map(|(&c, &d)| { + if d >= node_wall_buffer_m { + Some(c) + } else { + None + } }) .collect(); - candidates.sort_by(|&a, &b| dist[b as usize].total_cmp(&dist[a as usize])); + candidates.sort_by(|a, b| dist[b].total_cmp(&dist[a]).then(a.cmp(b))); - let survivors = nms_grid(&candidates, &idx_to_cell, voxel_size, node_spacing_m); + let survivors = nms_grid(&candidates, voxel_size, node_spacing_m); let nodes: Vec = survivors .iter() - .map(|&idx| { - let cell = idx_to_cell[idx as usize]; - NodeData { - cell, - pos: surface_point_xyz(cell.0, cell.1, cell.2, voxel_size), - } + .map(|&cell| NodeData { + cell, + pos: surface_point_xyz(cell.0, cell.1, cell.2, voxel_size), }) .collect(); @@ -81,44 +73,32 @@ pub fn place_nodes( SurfaceGraph { adj: scaled_adj, - idx_to_cell, - cell_to_idx, surface_lookup, nodes, } } -/// Cells that are missing any of the 4 neighbors are considered -/// on the edge of walkable terrain. -fn wall_adjacent_cells(adj: &CsrAdjacency, idx_to_cell: &[VoxelKey]) -> Vec { - let n = adj.n as usize; - let mut same_z = vec![0u8; n]; - for u in 0..n { - let iz_u = idx_to_cell[u].2; - let lo = adj.indptr[u] as usize; - let hi = adj.indptr[u + 1] as usize; - for k in lo..hi { - let v = adj.indices[k] as usize; - if idx_to_cell[v].2 == iz_u { - same_z[u] += 1; - } - } - } - let mut wall: Vec = (0..n as u32).filter(|&i| same_z[i as usize] < 4).collect(); +/// Cells missing any of their 4 same-z neighbors are treated as boundaries. +fn wall_adjacent_cells(adj: &SurfaceAdjacency) -> Vec { + let mut wall: Vec = adj + .cells() + .filter(|&c| { + let same_z = adj.neighbors(c).filter(|e| e.dst.2 == c.2).count(); + same_z < 4 + }) + .collect(); + wall.sort(); if wall.is_empty() { - wall.push(0); + if let Some(c) = adj.cells().min() { + wall.push(c); + } } wall } /// Bin placed nodes by node_spacing-sized cells. For each candidate, scan the /// 27 nearby bins for any node within Euclidean node_spacing. -fn nms_grid( - candidates_sorted: &[u32], - idx_to_cell: &[VoxelKey], - voxel_size: f32, - node_spacing_m: f32, -) -> Vec { +fn nms_grid(candidates_sorted: &[VoxelKey], voxel_size: f32, node_spacing_m: f32) -> Vec { let bin_size = ((node_spacing_m / voxel_size) as i32).max(1); let r_sq = (node_spacing_m as f64) * (node_spacing_m as f64); let v = voxel_size as f64; @@ -132,8 +112,7 @@ fn nms_grid( let mut bins: AHashMap<(i32, i32, i32), Vec> = AHashMap::new(); let mut survivors = Vec::new(); - for &cand in candidates_sorted { - let cell = idx_to_cell[cand as usize]; + for &cell in candidates_sorted { let (bx, by, bz) = bin_of(cell); let mut killed = false; 'outer: for dbx in -1..=1 { @@ -154,39 +133,39 @@ fn nms_grid( } } if !killed { - survivors.push(cand); + survivors.push(cell); bins.entry((bx, by, bz)).or_default().push(cell); } } survivors } -/// Linear ramp from penalty=2 at wall to penalty=1 at buffer, capped at 1. -/// Per-edge multiplier averages the two endpoints' penalties. -fn wall_safe_adjacency(adj: &CsrAdjacency, dist: &[f32], buffer_m: f32) -> CsrAdjacency { - let n = adj.n as usize; - let penalty: Vec = (0..n) - .map(|i| (1.0 + (buffer_m - dist[i]) / buffer_m).max(1.0)) - .collect(); - - let mut data = Vec::with_capacity(adj.data.len()); - for u in 0..n { - let lo = adj.indptr[u] as usize; - let hi = adj.indptr[u + 1] as usize; - let pu = penalty[u]; - for k in lo..hi { - let v = adj.indices[k] as usize; - let pv = penalty[v]; - data.push(adj.data[k] * (pu + pv) / 2.0); +/// Penalty adjustments to steer path away from walls and obstacles. +/// Subject to tuning... +fn wall_safe_adjacency( + adj: &SurfaceAdjacency, + dist: &AHashMap, + buffer_m: f32, +) -> SurfaceAdjacency { + let penalty = |cell: VoxelKey| -> f32 { + match dist.get(&cell) { + Some(&d) => (1.0 + (buffer_m - d) / buffer_m).max(1.0), + None => 1.0, } - } + }; - CsrAdjacency { - indptr: adj.indptr.clone(), - indices: adj.indices.clone(), - data, - n: adj.n, + let mut scaled = SurfaceAdjacency::new(); + for cell in adj.cells() { + scaled.add_cell(cell); + } + for cell in adj.cells() { + let pu = penalty(cell); + for edge in adj.neighbors(cell) { + let pv = penalty(edge.dst); + scaled.add_edge(cell, edge.dst, edge.cost * (pu + pv) / 2.0); + } } + scaled } #[cfg(test)] @@ -218,7 +197,7 @@ mod tests { #[test] fn empty_input() { let sg = place_nodes(&[], VOXEL, 2, 1.0, 0.3); - assert_eq!(sg.adj.n, 0); + assert!(sg.adj.is_empty()); assert!(sg.nodes.is_empty()); } @@ -265,13 +244,11 @@ mod tests { // Strip of 10 cells. End cells have 1 same-z neighbor → wall-adjacent → dist=0 → penalty=2. let cells: Vec = (0..10).map(|ix| (ix, 0, 0)).collect(); let sg = place_nodes(&cells, VOXEL, 2, 1.0, 0.3); - let end_idx = sg.cell_to_idx[&(0, 0, 0)] as usize; - let lo = sg.adj.indptr[end_idx] as usize; - let hi = sg.adj.indptr[end_idx + 1] as usize; - assert!(hi > lo); + let outbound: Vec<_> = sg.adj.neighbors((0, 0, 0)).collect(); + assert!(!outbound.is_empty()); // End cell penalty=2, neighbor penalty>=1, so outbound cost >= 1.5 * VOXEL. - for k in lo..hi { - assert!(sg.adj.data[k] >= 1.5 * VOXEL - 1e-5); + for edge in &outbound { + assert!(edge.cost >= 1.5 * VOXEL - 1e-5); } } @@ -279,15 +256,13 @@ mod tests { fn dijkstra_distances_grow_from_seeds() { let cells = open_patch_5x5(); let lookup = build_surface_lookup(&cells); - let SurfaceAdjacency { - adj, idx_to_cell, .. - } = build_surface_adjacency(&lookup, VOXEL, 2); - let seeds = wall_adjacent_cells(&adj, &idx_to_cell); + let adj = build_surface_adjacency(&lookup, VOXEL, 2); + let seeds = wall_adjacent_cells(&adj); let dist = dijkstra(&adj, &seeds).dist; - let center = idx_to_cell.iter().position(|&c| c == (2, 2, 0)).unwrap(); - let corner = idx_to_cell.iter().position(|&c| c == (0, 0, 0)).unwrap(); - assert!(dist[center] > dist[corner]); - assert_eq!(dist[corner], 0.0); + let center = dist[&(2, 2, 0)]; + let corner = dist[&(0, 0, 0)]; + assert!(center > corner); + assert_eq!(corner, 0.0); } } diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/plan.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/plan.rs index bc61f5e60b..5ae5dd4d6a 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/plan.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/plan.rs @@ -5,8 +5,9 @@ //! the node graph, and stitch cached edge cell paths into XYZ waypoints. #![allow(dead_code)] -use crate::adjacency::CsrAdjacency; -use crate::dijkstra::dijkstra; +use std::cmp::Ordering; +use std::collections::BinaryHeap; + use crate::edges::{walk_preds_to_source, PlannerGraph}; use crate::voxel::{surface_point_xyz, VoxelKey}; @@ -56,43 +57,45 @@ pub fn shortest_path_nodes(plg: &PlannerGraph, start: u32, goal: u32) -> Option< if start == goal { return Some(vec![start]); } - let csr = build_node_csr(plg); - let r = dijkstra(&csr, &[start]); - if !r.dist[goal as usize].is_finite() { + let n = plg.nodes.len(); + let mut dist = vec![f32::INFINITY; n]; + let mut pred = vec![-1i32; n]; + dist[start as usize] = 0.0; + let mut heap: BinaryHeap = BinaryHeap::new(); + heap.push(Scored(0.0, start)); + + while let Some(Scored(d, u)) = heap.pop() { + if d > dist[u as usize] { + continue; + } + if u == goal { + break; + } + for &edge_idx in &plg.node_adj[u as usize] { + let edge = &plg.node_edges[edge_idx as usize]; + let neighbor = if edge.a == u { edge.b } else { edge.a }; + let nd = d + edge.cost; + if nd < dist[neighbor as usize] { + dist[neighbor as usize] = nd; + pred[neighbor as usize] = u as i32; + heap.push(Scored(nd, neighbor)); + } + } + } + + if !dist[goal as usize].is_finite() { return None; } let mut path = vec![goal]; let mut cur = goal as i32; - while r.pred[cur as usize] >= 0 { - cur = r.pred[cur as usize]; + while pred[cur as usize] >= 0 { + cur = pred[cur as usize]; path.push(cur as u32); } path.reverse(); Some(path) } -fn build_node_csr(plg: &PlannerGraph) -> CsrAdjacency { - let n = plg.nodes.len(); - let mut indptr = vec![0u32; n + 1]; - let mut indices = Vec::new(); - let mut data = Vec::new(); - for (u, edges) in plg.node_adj.iter().enumerate() { - for &edge_idx in edges { - let edge = &plg.node_edges[edge_idx as usize]; - let neighbor = if edge.a as usize == u { edge.b } else { edge.a }; - indices.push(neighbor); - data.push(edge.cost); - } - indptr[u + 1] = indices.len() as u32; - } - CsrAdjacency { - indptr, - indices, - data, - n: n as u32, - } -} - fn assemble_waypoints( plg: &PlannerGraph, node_seq: &[u32], @@ -143,10 +146,29 @@ fn edge_between(plg: &PlannerGraph, a: u32, b: u32) -> Option { None } +struct Scored(f32, u32); + +impl PartialEq for Scored { + fn eq(&self, other: &Self) -> bool { + self.0.total_cmp(&other.0) == Ordering::Equal && self.1 == other.1 + } +} +impl Eq for Scored {} +impl PartialOrd for Scored { + fn partial_cmp(&self, other: &Self) -> Option { + Some(self.cmp(other)) + } +} +impl Ord for Scored { + fn cmp(&self, other: &Self) -> Ordering { + other.0.total_cmp(&self.0).then(self.1.cmp(&other.1)) + } +} + #[cfg(test)] mod tests { use super::*; - use crate::adjacency::{build_surface_adjacency, build_surface_lookup, SurfaceAdjacency}; + use crate::adjacency::{build_surface_adjacency, build_surface_lookup}; use crate::edges::add_node_edges; use crate::nodes::{NodeData, SurfaceGraph}; @@ -155,11 +177,7 @@ mod tests { fn graph_with_nodes(surface_cells: &[VoxelKey], node_cells: &[VoxelKey]) -> PlannerGraph { let surface_lookup = build_surface_lookup(surface_cells); - let SurfaceAdjacency { - adj, - idx_to_cell, - cell_to_idx, - } = build_surface_adjacency(&surface_lookup, VOXEL, 2); + let adj = build_surface_adjacency(&surface_lookup, VOXEL, 2); let nodes: Vec = node_cells .iter() .map(|&c| NodeData { @@ -169,8 +187,6 @@ mod tests { .collect(); let sg = SurfaceGraph { adj, - idx_to_cell, - cell_to_idx, surface_lookup, nodes, }; From 1080e0ccb89fc53a8fbf94df18c1a544e5307b4a Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Fri, 29 May 2026 13:14:34 -0700 Subject: [PATCH 38/55] Fix edge detection --- .../modules/mls_planner/rust/src/nodes.rs | 26 ++++++++++++++++--- 1 file changed, 22 insertions(+), 4 deletions(-) diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs index c45dd2c7d5..5c9f50e9b8 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs @@ -7,7 +7,7 @@ #![allow(dead_code)] // consumed incrementally -use ahash::AHashMap; +use ahash::{AHashMap, AHashSet}; use crate::adjacency::{ build_surface_adjacency, build_surface_lookup, SurfaceAdjacency, SurfaceLookup, @@ -78,13 +78,16 @@ pub fn place_nodes( } } -/// Cells missing any of their 4 same-z neighbors are treated as boundaries. +/// Cells missing any of their 4 xy-direction neighbors are treated as boundaries. fn wall_adjacent_cells(adj: &SurfaceAdjacency) -> Vec { let mut wall: Vec = adj .cells() .filter(|&c| { - let same_z = adj.neighbors(c).filter(|e| e.dst.2 == c.2).count(); - same_z < 4 + let mut dirs: AHashSet<(i32, i32)> = AHashSet::new(); + for e in adj.neighbors(c) { + dirs.insert((e.dst.0 - c.0, e.dst.1 - c.1)); + } + dirs.len() < 4 }) .collect(); wall.sort(); @@ -219,6 +222,21 @@ mod tests { } } + #[test] + fn sloped_patch_places_interior_nodes() { + // 10x10 plane sloped 1 cell of z per cell of x. With step_threshold=2 + // every interior cell still has all 4 xy-direction neighbors in-graph, + // so it must not be flagged as wall-adjacent. + let mut cells = Vec::new(); + for ix in 0..10 { + for iy in 0..10 { + cells.push((ix, iy, ix)); + } + } + let sg = place_nodes(&cells, VOXEL, 2, 1.0, 0.3); + assert!(!sg.nodes.is_empty()); + } + #[test] fn nms_enforces_spacing() { // Two 10x10 patches separated by 1m gap; each places at least one node, no pair within 1m. From 6c7d42072ed54fe854683608e1bfe45ab3f1e746 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Fri, 29 May 2026 13:16:48 -0700 Subject: [PATCH 39/55] Fix morphology --- .../nav_stack/modules/mls_planner/rust/src/surfaces.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs index 0545a4fe1f..b1f10b777b 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs @@ -118,7 +118,7 @@ fn close_at_z( erosion_passes: u32, clearance_cells: i32, ) -> Vec { - let pad = dilation_passes as i32; + let pad = (dilation_passes + erosion_passes) as i32; let mut min_x = i32::MAX; let mut max_x = i32::MIN; let mut min_y = i32::MAX; From 2be896918382af0bd0637a7727e70217434a3240 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Fri, 29 May 2026 13:32:11 -0700 Subject: [PATCH 40/55] Move publishes and timing --- .../modules/mls_planner/rust/src/main.rs | 54 +++++++++++-------- 1 file changed, 31 insertions(+), 23 deletions(-) diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs index 87d2293335..e36125bca8 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs @@ -9,7 +9,7 @@ mod plan; mod surfaces; mod voxel; -use std::time::{Duration, SystemTime, UNIX_EPOCH}; +use std::time::{Duration, Instant, SystemTime, UNIX_EPOCH}; use dimos_module::{error_throttled, run, warn_throttled, Input, LcmTransport, Module, Output}; use lcm_msgs::geometry_msgs::{Point, Pose, PoseStamped, Quaternion}; @@ -137,19 +137,27 @@ impl MlsPlanner { let cfg = &self.config; + let t_surface = Instant::now(); // convert whatever map we got in to voxels let voxel_map: AHashSet = points .iter() .map(|&p| voxelize(p, cfg.voxel_size)) .collect(); - let surface_cells = extract_surfaces( &voxel_map, self.clearance_cells, cfg.surface_dilation_passes, cfg.surface_erosion_passes, ); + let surface_ms = ms(t_surface.elapsed()); + + let surface_points: Vec<(f32, f32, f32)> = surface_cells + .iter() + .map(|&(ix, iy, iz)| surface_point_xyz(ix, iy, iz, cfg.voxel_size)) + .collect(); + publish_cloud(&self.surface_map, &surface_points, &cfg.world_frame, now()).await; + let t_nodes = Instant::now(); let sg = place_nodes( &surface_cells, cfg.voxel_size, @@ -157,38 +165,32 @@ impl MlsPlanner { cfg.node_spacing_m, cfg.node_wall_buffer_m, ); - + let nodes_ms = ms(t_nodes.elapsed()); let n_nodes = sg.nodes.len(); + + let node_points: Vec<(f32, f32, f32)> = sg.nodes.iter().map(|n| n.pos).collect(); + publish_cloud(&self.nodes, &node_points, &cfg.world_frame, now()).await; + + let t_edges = Instant::now(); let plg = add_node_edges(sg); + let edges_ms = ms(t_edges.elapsed()); let n_edges = plg.node_edges.len(); + + let edges_path = build_segments_path(&plg, cfg.voxel_size, &cfg.world_frame, now()); + publish_path(&self.node_edges, &edges_path).await; + info!( obstacle_points = points.len(), obstacle_voxels = voxel_map.len(), surface_cells = surface_cells.len(), nodes = n_nodes, edges = n_edges, + surface_ms, + nodes_ms, + edges_ms, "global_map processed", ); - let stamp = now(); - let surface_points: Vec<(f32, f32, f32)> = surface_cells - .iter() - .map(|&(ix, iy, iz)| surface_point_xyz(ix, iy, iz, cfg.voxel_size)) - .collect(); - publish_cloud( - &self.surface_map, - &surface_points, - &cfg.world_frame, - stamp.clone(), - ) - .await; - - let node_points: Vec<(f32, f32, f32)> = plg.nodes.iter().map(|n| n.pos).collect(); - publish_cloud(&self.nodes, &node_points, &cfg.world_frame, stamp.clone()).await; - - let edges_path = build_segments_path(&plg, cfg.voxel_size, &cfg.world_frame, stamp.clone()); - publish_path(&self.node_edges, &edges_path).await; - self.planner_graph = Some(plg); } @@ -213,6 +215,7 @@ impl MlsPlanner { let p = &msg.pose.pose.position; let goal = (p.x as f32, p.y as f32, p.z as f32); + let t_plan = Instant::now(); let waypoints = match plan::plan( plg, start, @@ -227,14 +230,19 @@ impl MlsPlanner { return; } }; + let plan_ms = ms(t_plan.elapsed()); let stamp = now(); let path_msg = build_path_from_waypoints(&waypoints, &self.config.world_frame, stamp); - info!(waypoints = waypoints.len(), "path planned"); + info!(waypoints = waypoints.len(), plan_ms, "path planned"); publish_path(&self.path, &path_msg).await; } } +fn ms(d: Duration) -> f64 { + d.as_secs_f64() * 1000.0 +} + async fn publish_cloud( out: &Output, points: &[(f32, f32, f32)], From 8a6f3d142a7eed14b0bf13bceb340068ce151437 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Fri, 29 May 2026 13:52:18 -0700 Subject: [PATCH 41/55] Fix start and goal snapping --- .../modules/mls_planner/rust/src/plan.rs | 164 +++++++++++++----- 1 file changed, 121 insertions(+), 43 deletions(-) diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/plan.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/plan.rs index 5ae5dd4d6a..044dd0caf3 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/plan.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/plan.rs @@ -1,43 +1,79 @@ // Copyright 2026 Dimensional Inc. // SPDX-License-Identifier: Apache-2.0 -//! Query-time planning: snap start/goal poses to nodes, run shortest path on -//! the node graph, and stitch cached edge cell paths into XYZ waypoints. #![allow(dead_code)] use std::cmp::Ordering; use std::collections::BinaryHeap; +use ahash::AHashMap; + +use crate::adjacency::SurfaceLookup; use crate::edges::{walk_preds_to_source, PlannerGraph}; use crate::voxel::{surface_point_xyz, VoxelKey}; -/// Snap a query pose to the nearest node by 3D Euclidean distance, rejecting -/// nodes whose z differs from the pose's z by more than `z_tolerance_m`. -pub fn snap_pose_to_node( - plg: &PlannerGraph, +/// Snap a pose to the best surface cell. +pub fn snap_pose_to_cell( + surface_lookup: &SurfaceLookup, pose: (f32, f32, f32), - z_tolerance_m: f32, -) -> Option { - let mut best: Option<(f32, u32)> = None; - for (i, n) in plg.nodes.iter().enumerate() { - if (pose.2 - n.pos.2).abs() > z_tolerance_m { - continue; + voxel_size: f32, + tolerance_m: f32, +) -> Option { + let ix = (pose.0 / voxel_size).floor() as i32; + let iy = (pose.1 / voxel_size).floor() as i32; + let target_iz = (pose.2 / voxel_size).floor() as i32 - 1; + let tol_cells = (tolerance_m / voxel_size).ceil() as i32; + + if let Some(cell) = best_iz_in_column(surface_lookup, ix, iy, target_iz, tol_cells) { + return Some(cell); + } + + const SEARCH_RADIUS: i32 = 5; + let mut best: Option<(i32, VoxelKey)> = None; + for dix in -SEARCH_RADIUS..=SEARCH_RADIUS { + for diy in -SEARCH_RADIUS..=SEARCH_RADIUS { + if dix == 0 && diy == 0 { + continue; + } + let Some(cell) = + best_iz_in_column(surface_lookup, ix + dix, iy + diy, target_iz, tol_cells) + else { + continue; + }; + let d2 = dix * dix + diy * diy; + if best.is_none_or(|(bd, _)| d2 < bd) { + best = Some((d2, cell)); + } } - let dx = pose.0 - n.pos.0; - let dy = pose.1 - n.pos.1; - let dz = pose.2 - n.pos.2; - let d_sq = dx * dx + dy * dy + dz * dz; - match best { - Some((b, _)) if b <= d_sq => {} - _ => best = Some((d_sq, i as u32)), + } + best.map(|(_, c)| c) +} + +fn best_iz_in_column( + surface_lookup: &SurfaceLookup, + ix: i32, + iy: i32, + target_iz: i32, + tol_cells: i32, +) -> Option { + let zs = surface_lookup.get(&(ix, iy))?; + let mut best: Option<(i32, i32)> = None; + for &iz in zs { + let d = (iz - target_iz).abs(); + if best.is_none_or(|(bd, _)| d < bd) { + best = Some((d, iz)); } } - best.map(|(_, i)| i) + let (bd, iz) = best?; + if bd > tol_cells { + return None; + } + Some((ix, iy, iz)) } -/// Plan an XYZ waypoint sequence from `start_pose` to `goal_pose`. -/// Returns None if either pose can't snap, or if the snapped nodes are -/// disconnected in the node graph. +/// Plan path from start pose to goal pose using the node graph. +/// Returns none if either of the poses can't be snapped to surface or if +/// there is no valid path. pub fn plan( plg: &PlannerGraph, start_pose: (f32, f32, f32), @@ -45,11 +81,30 @@ pub fn plan( voxel_size: f32, z_tolerance_m: f32, ) -> Option> { - let start_node = snap_pose_to_node(plg, start_pose, z_tolerance_m)?; - let goal_node = snap_pose_to_node(plg, goal_pose, z_tolerance_m)?; + let start_cell = snap_pose_to_cell(&plg.surface_lookup, start_pose, voxel_size, z_tolerance_m)?; + let goal_cell = snap_pose_to_cell(&plg.surface_lookup, goal_pose, voxel_size, z_tolerance_m)?; + + let node_cell_to_idx: AHashMap = plg + .nodes + .iter() + .enumerate() + .map(|(i, n)| (n.cell, i as u32)) + .collect(); + + let start_segment = walk_preds_to_source(plg, start_cell); + let goal_segment = walk_preds_to_source(plg, goal_cell); + let start_node = *node_cell_to_idx.get(start_segment.last()?)?; + let goal_node = *node_cell_to_idx.get(goal_segment.last()?)?; + let node_seq = shortest_path_nodes(plg, start_node, goal_node)?; Some(assemble_waypoints( - plg, &node_seq, start_pose, goal_pose, voxel_size, + plg, + &node_seq, + start_pose, + &start_segment, + goal_pose, + &goal_segment, + voxel_size, )) } @@ -100,10 +155,15 @@ fn assemble_waypoints( plg: &PlannerGraph, node_seq: &[u32], start_pose: (f32, f32, f32), + start_segment: &[VoxelKey], goal_pose: (f32, f32, f32), + goal_segment: &[VoxelKey], voxel_size: f32, ) -> Vec<(f32, f32, f32)> { let mut cells: Vec = Vec::new(); + + cells.extend_from_slice(start_segment); + for pair in node_seq.windows(2) { let (a, b) = (pair[0], pair[1]); let edge_idx = @@ -126,6 +186,12 @@ fn assemble_waypoints( } } + for &c in goal_segment.iter().rev() { + if cells.last() != Some(&c) { + cells.push(c); + } + } + let mut waypoints: Vec<(f32, f32, f32)> = Vec::with_capacity(cells.len() + 2); waypoints.push(start_pose); for (ix, iy, iz) in cells { @@ -198,26 +264,27 @@ mod tests { } #[test] - fn snap_returns_none_when_no_nodes() { - let plg = graph_with_nodes(&strip(20), &[]); - assert!(snap_pose_to_node(&plg, (0.5, 0.0, 0.05), Z_TOL).is_none()); + fn snap_picks_in_column_cell() { + let lookup = build_surface_lookup(&strip(20)); + let cell = snap_pose_to_cell(&lookup, (0.5, 0.0, 0.1), VOXEL, Z_TOL).unwrap(); + assert_eq!(cell, (5, 0, 0)); } #[test] - fn snap_returns_nearest_node() { - // Nodes at x=3 and x=15 (XYZ positions 0.35 and 1.55). - let plg = graph_with_nodes(&strip(20), &[(3, 0, 0), (15, 0, 0)]); - let snapped = snap_pose_to_node(&plg, (0.5, 0.0, 0.1), Z_TOL).unwrap(); - assert_eq!(snapped, 0); - let snapped = snap_pose_to_node(&plg, (1.4, 0.0, 0.1), Z_TOL).unwrap(); - assert_eq!(snapped, 1); + fn snap_falls_back_to_nearby_column() { + // Column ix=2 is removed; a pose in that column must snap to an adjacent one. + let mut cells = strip(20); + cells.retain(|c| c.0 != 2); + let lookup = build_surface_lookup(&cells); + let cell = snap_pose_to_cell(&lookup, (0.25, 0.0, 0.1), VOXEL, Z_TOL).unwrap(); + assert!(cell == (1, 0, 0) || cell == (3, 0, 0)); } #[test] fn snap_rejects_outside_z_tolerance() { - let plg = graph_with_nodes(&strip(20), &[(3, 0, 0)]); - // Node's pos.z = (0+1)*0.1 = 0.1. Pose at z=2.0, tolerance=1.5 → 1.9 > 1.5 → reject. - assert!(snap_pose_to_node(&plg, (0.5, 0.0, 2.0), 1.5).is_none()); + let lookup = build_surface_lookup(&strip(20)); + // Surface at iz=0, pose at z=2.0, tolerance=1.5 → out of tolerance in every column. + assert!(snap_pose_to_cell(&lookup, (0.5, 0.0, 2.0), VOXEL, 1.5).is_none()); } #[test] @@ -238,12 +305,23 @@ mod tests { } #[test] - fn plan_same_start_and_goal_returns_two_waypoints() { + fn plan_same_start_and_goal_passes_through_snap_cell() { let plg = graph_with_nodes(&strip(20), &[(10, 0, 0)]); let wp = plan(&plg, (1.0, 0.0, 0.05), (1.0, 0.0, 0.05), VOXEL, Z_TOL).unwrap(); - assert_eq!(wp.len(), 2); - assert_eq!(wp[0], (1.0, 0.0, 0.05)); - assert_eq!(wp[1], (1.0, 0.0, 0.05)); + assert_eq!(wp.first(), Some(&(1.0, 0.0, 0.05))); + assert_eq!(wp.last(), Some(&(1.0, 0.0, 0.05))); + let snap = surface_point_xyz(10, 0, 0, VOXEL); + assert!(wp.contains(&snap)); + } + + #[test] + fn plan_traces_surface_from_pose_to_first_node() { + let plg = graph_with_nodes(&strip(20), &[(3, 0, 0), (15, 0, 0)]); + let wp = plan(&plg, (0.2, 0.0, 0.05), (1.7, 0.0, 0.05), VOXEL, Z_TOL).unwrap(); + let start_cell_pos = surface_point_xyz(2, 0, 0, VOXEL); + let goal_cell_pos = surface_point_xyz(17, 0, 0, VOXEL); + assert_eq!(wp[1], start_cell_pos); + assert_eq!(wp[wp.len() - 2], goal_cell_pos); } #[test] From aca32f4bc345be53e0a887b8686f92da01430248 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Fri, 29 May 2026 14:08:30 -0700 Subject: [PATCH 42/55] Optimizations --- .../modules/mls_planner/rust/src/adjacency.rs | 11 ++ .../modules/mls_planner/rust/src/dijkstra.rs | 116 ++++++++++-------- .../modules/mls_planner/rust/src/edges.rs | 36 +++--- .../modules/mls_planner/rust/src/nodes.rs | 61 ++++----- .../modules/mls_planner/rust/src/surfaces.rs | 33 +++-- 5 files changed, 145 insertions(+), 112 deletions(-) diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/adjacency.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/adjacency.rs index 4ac3c7081f..aeb9e1e0e5 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/adjacency.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/adjacency.rs @@ -43,6 +43,17 @@ impl SurfaceAdjacency { self.cells.keys().copied() } + /// Per-cell iteration that yields the cell and a borrowed view of its + /// edges in a single hashmap probe. + pub fn iter(&self) -> impl Iterator + '_ { + self.cells.iter().map(|(&k, v)| (k, v.as_slice())) + } + + /// Mutable per-cell edge iterator. + pub fn iter_edges_mut(&mut self) -> impl Iterator)> + '_ { + self.cells.iter_mut().map(|(&k, v)| (k, v)) + } + pub fn contains(&self, cell: VoxelKey) -> bool { self.cells.contains_key(&cell) } diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/dijkstra.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/dijkstra.rs index a8631999a3..9cc31476cd 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/dijkstra.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/dijkstra.rs @@ -3,8 +3,6 @@ //! Multi-source Dijkstra over the cell-keyed surface adjacency. //! -//! Tracks distance, predecessor, and nearest-source label for each reached -//! cell. Cells absent from the result are unreachable from every source. #![allow(dead_code)] @@ -16,52 +14,77 @@ use ahash::AHashMap; use crate::adjacency::SurfaceAdjacency; use crate::voxel::VoxelKey; +#[derive(Clone, Copy, Debug)] +pub struct CellState { + pub dist: f32, + /// Predecesor nodes along the shortest path to source + pub pred: Option, + /// Id of cheapest source to return to + pub source: u32, +} + pub struct DijkstraResult { - pub dist: AHashMap, - /// Predecessor cell along the shortest path back to a source. - pub pred: AHashMap, - /// Id of the cheapest source to return to - pub source: AHashMap, + pub state: AHashMap, +} + +impl DijkstraResult { + pub fn dist_map(&self) -> AHashMap { + self.state.iter().map(|(&c, s)| (c, s.dist)).collect() + } } pub fn dijkstra(adj: &SurfaceAdjacency, sources: &[VoxelKey]) -> DijkstraResult { - let mut dist: AHashMap = AHashMap::new(); - let mut pred: AHashMap = AHashMap::new(); - let mut source: AHashMap = AHashMap::new(); + let mut state: AHashMap = AHashMap::new(); let mut heap: BinaryHeap = BinaryHeap::new(); for (label, &s) in sources.iter().enumerate() { if !adj.contains(s) { continue; } - dist.insert(s, 0.0); - source.insert(s, label as u32); + state.insert( + s, + CellState { + dist: 0.0, + pred: None, + source: label as u32, + }, + ); heap.push(Scored(0.0, s)); } while let Some(Scored(d, u)) = heap.pop() { - if let Some(&cur) = dist.get(&u) { - if d > cur { - continue; - } + let Some(&CellState { + dist: cur, + source: su, + .. + }) = state.get(&u) + else { + continue; + }; + if d > cur { + continue; } - let su = source[&u]; for edge in adj.neighbors(u) { let nd = d + edge.cost; - let should_update = match dist.get(&edge.dst) { + let should_update = match state.get(&edge.dst) { None => true, - Some(&cur) => nd < cur, + Some(s) => nd < s.dist, }; if should_update { - dist.insert(edge.dst, nd); - pred.insert(edge.dst, u); - source.insert(edge.dst, su); + state.insert( + edge.dst, + CellState { + dist: nd, + pred: Some(u), + source: su, + }, + ); heap.push(Scored(nd, edge.dst)); } } } - DijkstraResult { dist, pred, source } + DijkstraResult { state } } struct Scored(f32, VoxelKey); @@ -104,9 +127,7 @@ mod tests { fn empty_sources_leaves_everything_unreachable() { let adj = chain(4); let r = dijkstra(&adj, &[]); - assert!(r.dist.is_empty()); - assert!(r.pred.is_empty()); - assert!(r.source.is_empty()); + assert!(r.state.is_empty()); } #[test] @@ -114,13 +135,14 @@ mod tests { let adj = chain(5); let r = dijkstra(&adj, &[(0, 0, 0)]); for i in 0..5 { - assert_eq!(r.dist[&(i, 0, 0)], i as f32); - assert_eq!(r.source[&(i, 0, 0)], 0); + let s = r.state[&(i, 0, 0)]; + assert_eq!(s.dist, i as f32); + assert_eq!(s.source, 0); } - assert!(!r.pred.contains_key(&(0, 0, 0))); + assert!(r.state[&(0, 0, 0)].pred.is_none()); let mut cur = (4, 0, 0); let mut hops = 0; - while let Some(&p) = r.pred.get(&cur) { + while let Some(p) = r.state[&cur].pred { cur = p; hops += 1; } @@ -134,18 +156,18 @@ mod tests { // cells 3-4 closer to source 1. Cell 2 is equidistant. let adj = chain(5); let r = dijkstra(&adj, &[(0, 0, 0), (4, 0, 0)]); - assert_eq!(r.source[&(0, 0, 0)], 0); - assert_eq!(r.source[&(1, 0, 0)], 0); - assert_eq!(r.source[&(3, 0, 0)], 1); - assert_eq!(r.source[&(4, 0, 0)], 1); + assert_eq!(r.state[&(0, 0, 0)].source, 0); + assert_eq!(r.state[&(1, 0, 0)].source, 0); + assert_eq!(r.state[&(3, 0, 0)].source, 1); + assert_eq!(r.state[&(4, 0, 0)].source, 1); // Tie at cell 2 must resolve to one of the two sources. - let s2 = r.source[&(2, 0, 0)]; + let s2 = r.state[&(2, 0, 0)].source; assert!(s2 == 0 || s2 == 1); - assert_eq!(r.dist[&(0, 0, 0)], 0.0); - assert_eq!(r.dist[&(1, 0, 0)], 1.0); - assert_eq!(r.dist[&(2, 0, 0)], 2.0); - assert_eq!(r.dist[&(3, 0, 0)], 1.0); - assert_eq!(r.dist[&(4, 0, 0)], 0.0); + assert_eq!(r.state[&(0, 0, 0)].dist, 0.0); + assert_eq!(r.state[&(1, 0, 0)].dist, 1.0); + assert_eq!(r.state[&(2, 0, 0)].dist, 2.0); + assert_eq!(r.state[&(3, 0, 0)].dist, 1.0); + assert_eq!(r.state[&(4, 0, 0)].dist, 0.0); } #[test] @@ -160,12 +182,10 @@ mod tests { adj.add_edge((2, 0, 0), (3, 0, 0), 1.0); adj.add_edge((3, 0, 0), (2, 0, 0), 1.0); let r = dijkstra(&adj, &[(0, 0, 0)]); - assert_eq!(r.dist[&(0, 0, 0)], 0.0); - assert_eq!(r.dist[&(1, 0, 0)], 1.0); - assert!(!r.dist.contains_key(&(2, 0, 0))); - assert!(!r.dist.contains_key(&(3, 0, 0))); - assert!(!r.source.contains_key(&(2, 0, 0))); - assert!(!r.pred.contains_key(&(2, 0, 0))); + assert_eq!(r.state[&(0, 0, 0)].dist, 0.0); + assert_eq!(r.state[&(1, 0, 0)].dist, 1.0); + assert!(!r.state.contains_key(&(2, 0, 0))); + assert!(!r.state.contains_key(&(3, 0, 0))); } #[test] @@ -182,7 +202,7 @@ mod tests { adj.add_edge((2, 0, 0), (1, 0, 0), 1.0); adj.add_edge((1, 0, 0), (2, 0, 0), 1.0); let r = dijkstra(&adj, &[(0, 0, 0)]); - assert_eq!(r.dist[&(1, 0, 0)], 2.0); - assert_eq!(r.pred[&(1, 0, 0)], (2, 0, 0)); + assert_eq!(r.state[&(1, 0, 0)].dist, 2.0); + assert_eq!(r.state[&(1, 0, 0)].pred, Some((2, 0, 0))); } } diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/edges.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/edges.rs index c8bc24d953..1d708adfa4 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/edges.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/edges.rs @@ -13,7 +13,7 @@ use ahash::AHashMap; use crate::adjacency::{SurfaceAdjacency, SurfaceLookup}; -use crate::dijkstra::{dijkstra, DijkstraResult}; +use crate::dijkstra::{dijkstra, CellState}; use crate::nodes::{NodeData, SurfaceGraph}; use crate::voxel::VoxelKey; @@ -33,7 +33,8 @@ pub struct PlannerGraph { pub nodes: Vec, pub node_edges: Vec, pub node_adj: Vec>, - pub cell_predecessors: AHashMap, + + pub cell_state: AHashMap, } pub fn add_node_edges(sg: SurfaceGraph) -> PlannerGraph { @@ -50,13 +51,13 @@ pub fn add_node_edges(sg: SurfaceGraph) -> PlannerGraph { nodes, node_edges: Vec::new(), node_adj: Vec::new(), - cell_predecessors: AHashMap::new(), + cell_state: AHashMap::new(), }; } let source_cells: Vec = nodes.iter().map(|n| n.cell).collect(); - let DijkstraResult { dist, pred, source } = dijkstra(&adj, &source_cells); - let node_edges = best_boundary_edges(&adj, &dist, &source); + let cell_state = dijkstra(&adj, &source_cells).state; + let node_edges = best_boundary_edges(&adj, &cell_state); let mut node_adj: Vec> = vec![Vec::new(); nodes.len()]; for (edge_idx, edge) in node_edges.iter().enumerate() { @@ -70,7 +71,7 @@ pub fn add_node_edges(sg: SurfaceGraph) -> PlannerGraph { nodes, node_edges, node_adj, - cell_predecessors: pred, + cell_state, } } @@ -94,7 +95,7 @@ pub fn edges_to_segments(plg: &PlannerGraph, _voxel_size: f32) -> Vec<(VoxelKey, pub fn walk_preds_to_source(plg: &PlannerGraph, start_cell: VoxelKey) -> Vec { let mut cells = vec![start_cell]; let mut cur = start_cell; - while let Some(&p) = plg.cell_predecessors.get(&cur) { + while let Some(p) = plg.cell_state.get(&cur).and_then(|s| s.pred) { cur = p; cells.push(cur); } @@ -103,25 +104,26 @@ pub fn walk_preds_to_source(plg: &PlannerGraph, start_cell: VoxelKey) -> Vec, - source: &AHashMap, + state: &AHashMap, ) -> Vec { let mut best: AHashMap<(u32, u32), NodeEdge> = AHashMap::new(); - for u in adj.cells() { - let Some(&sa) = source.get(&u) else { + for (u, edges) in adj.iter() { + let Some(su) = state.get(&u) else { continue; }; - let du = dist[&u]; - for edge in adj.neighbors(u) { + let sa = su.source; + let du = su.dist; + for edge in edges { let v = edge.dst; - let Some(&sb) = source.get(&v) else { + let Some(sv) = state.get(&v) else { continue; }; + let sb = sv.source; if sa == sb { continue; } - let dv = dist[&v]; + let dv = sv.dist; let cost = du + edge.cost + dv; let (key_a, key_b, bu, bv) = if sa < sb { @@ -245,7 +247,7 @@ mod tests { let mut cur = e.boundary_u; let mut hops = 0; - while let Some(&p) = pg.cell_predecessors.get(&cur) { + while let Some(p) = pg.cell_state.get(&cur).and_then(|s| s.pred) { cur = p; hops += 1; assert!(hops < 1000, "predecessor walk did not terminate"); @@ -253,7 +255,7 @@ mod tests { assert_eq!(cur, cell_a, "u-side preds must reach node a"); let mut cur = e.boundary_v; - while let Some(&p) = pg.cell_predecessors.get(&cur) { + while let Some(p) = pg.cell_state.get(&cur).and_then(|s| s.pred) { cur = p; } assert_eq!(cur, cell_b, "v-side preds must reach node b"); diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs index 5c9f50e9b8..ed072dfd52 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs @@ -34,7 +34,7 @@ pub fn place_nodes( node_wall_buffer_m: f32, ) -> SurfaceGraph { let surface_lookup = build_surface_lookup(surface_cells); - let adj = build_surface_adjacency(&surface_lookup, voxel_size, maximum_step_cells); + let mut adj = build_surface_adjacency(&surface_lookup, voxel_size, maximum_step_cells); if adj.is_empty() { return SurfaceGraph { @@ -45,7 +45,7 @@ pub fn place_nodes( } let wall_seeds = wall_adjacent_cells(&adj); - let dist = dijkstra(&adj, &wall_seeds).dist; + let dist = dijkstra(&adj, &wall_seeds).dist_map(); let mut candidates: Vec = dist .iter() @@ -69,10 +69,10 @@ pub fn place_nodes( }) .collect(); - let scaled_adj = wall_safe_adjacency(&adj, &dist, node_wall_buffer_m); + apply_wall_safe_penalty(&mut adj, &dist, node_wall_buffer_m); SurfaceGraph { - adj: scaled_adj, + adj, surface_lookup, nodes, } @@ -81,14 +81,15 @@ pub fn place_nodes( /// Cells missing any of their 4 xy-direction neighbors are treated as boundaries. fn wall_adjacent_cells(adj: &SurfaceAdjacency) -> Vec { let mut wall: Vec = adj - .cells() - .filter(|&c| { + .iter() + .filter(|(c, edges)| { let mut dirs: AHashSet<(i32, i32)> = AHashSet::new(); - for e in adj.neighbors(c) { + for e in *edges { dirs.insert((e.dst.0 - c.0, e.dst.1 - c.1)); } dirs.len() < 4 }) + .map(|(c, _)| c) .collect(); wall.sort(); if wall.is_empty() { @@ -143,32 +144,32 @@ fn nms_grid(candidates_sorted: &[VoxelKey], voxel_size: f32, node_spacing_m: f32 survivors } -/// Penalty adjustments to steer path away from walls and obstacles. +/// Scales every edge cost by the average of its endpoint penalties, which +/// pushes shortest paths away from walls. /// Subject to tuning... -fn wall_safe_adjacency( - adj: &SurfaceAdjacency, +fn apply_wall_safe_penalty( + adj: &mut SurfaceAdjacency, dist: &AHashMap, buffer_m: f32, -) -> SurfaceAdjacency { - let penalty = |cell: VoxelKey| -> f32 { - match dist.get(&cell) { - Some(&d) => (1.0 + (buffer_m - d) / buffer_m).max(1.0), - None => 1.0, - } - }; +) { + let penalty: AHashMap = adj + .cells() + .map(|c| { + let p = match dist.get(&c) { + Some(&d) => (1.0 + (buffer_m - d) / buffer_m).max(1.0), + None => 1.0, + }; + (c, p) + }) + .collect(); - let mut scaled = SurfaceAdjacency::new(); - for cell in adj.cells() { - scaled.add_cell(cell); - } - for cell in adj.cells() { - let pu = penalty(cell); - for edge in adj.neighbors(cell) { - let pv = penalty(edge.dst); - scaled.add_edge(cell, edge.dst, edge.cost * (pu + pv) / 2.0); + for (src, edges) in adj.iter_edges_mut() { + let pu = penalty.get(&src).copied().unwrap_or(1.0); + for edge in edges { + let pv = penalty.get(&edge.dst).copied().unwrap_or(1.0); + edge.cost *= (pu + pv) / 2.0; } } - scaled } #[cfg(test)] @@ -276,10 +277,10 @@ mod tests { let lookup = build_surface_lookup(&cells); let adj = build_surface_adjacency(&lookup, VOXEL, 2); let seeds = wall_adjacent_cells(&adj); - let dist = dijkstra(&adj, &seeds).dist; + let state = dijkstra(&adj, &seeds).state; - let center = dist[&(2, 2, 0)]; - let corner = dist[&(0, 0, 0)]; + let center = state[&(2, 2, 0)].dist; + let corner = state[&(0, 0, 0)].dist; assert!(center > corner); assert_eq!(corner, 0.0); } diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs index b1f10b777b..9c56a4d7be 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs @@ -17,21 +17,20 @@ use crate::voxel::VoxelKey; const ON: Luma = Luma([255]); const OFF: Luma = Luma([0]); +type ColumnIz = AHashMap<(i32, i32), Vec>; + /// A cell is standable if it has at least the robots height of clear /// space above it. -fn is_standable( - ix: i32, - iy: i32, - iz: i32, - voxel_map: &AHashSet, - clearance_cells: i32, -) -> bool { - for dz in 1..=clearance_cells { - if voxel_map.contains(&(ix, iy, iz + dz)) { - return false; - } +fn is_standable(ix: i32, iy: i32, iz: i32, by_col: &ColumnIz, clearance_cells: i32) -> bool { + let Some(zs) = by_col.get(&(ix, iy)) else { + return true; + }; + // first obstacle strictly above iz, if any + let idx = zs.partition_point(|&z| z <= iz); + match zs.get(idx) { + Some(&next) => next - iz > clearance_cells, + None => true, } - true } /// Extract standable cells from the voxelized global map, then close small @@ -69,7 +68,7 @@ pub fn extract_surfaces( close_surface_holes( standable, - voxel_map, + &by_col, dilation_passes, erosion_passes, clearance_cells, @@ -80,7 +79,7 @@ pub fn extract_surfaces( /// to fill in small holes. fn close_surface_holes( standable: Vec, - obstacles: &AHashSet, + by_col: &ColumnIz, dilation_passes: u32, erosion_passes: u32, clearance_cells: i32, @@ -100,7 +99,7 @@ fn close_surface_holes( out.extend(close_at_z( &xys, iz, - obstacles, + by_col, dilation_passes, erosion_passes, clearance_cells, @@ -113,7 +112,7 @@ fn close_surface_holes( fn close_at_z( xys: &[(i32, i32)], iz: i32, - voxel_map: &AHashSet, + by_col: &ColumnIz, dilation_passes: u32, erosion_passes: u32, clearance_cells: i32, @@ -159,7 +158,7 @@ fn close_at_z( let iy = y0 + py as i32; // filter out if the surface has expanded to any non standable areas - if !is_standable(ix, iy, iz, voxel_map, clearance_cells) { + if !is_standable(ix, iy, iz, by_col, clearance_cells) { continue; } out.push((ix, iy, iz)); From 176af2d538df72f060570280af79b1ddbbc6e9d7 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Fri, 29 May 2026 14:18:55 -0700 Subject: [PATCH 43/55] Fix scenarios --- dimos/navigation/nav_stack/evaluator/scenarios.py | 15 +++++++++------ .../modules/mls_planner/rust/src/surfaces.rs | 3 ++- 2 files changed, 11 insertions(+), 7 deletions(-) diff --git a/dimos/navigation/nav_stack/evaluator/scenarios.py b/dimos/navigation/nav_stack/evaluator/scenarios.py index 62cdd50e98..48dfb7e1c6 100644 --- a/dimos/navigation/nav_stack/evaluator/scenarios.py +++ b/dimos/navigation/nav_stack/evaluator/scenarios.py @@ -112,12 +112,15 @@ def _map_with_walls(*walls: np.ndarray) -> PointCloud2: return _cloud(np.vstack([_floor(), *walls])) +_GROUND_SURFACE_Z = 0.1 + + def empty_floor() -> PlannerScenario: return PlannerScenario( name="empty_floor", global_map=_cloud(_floor()), - start_pose=_odom(-1.0, 0.0, 0.2), - goal_pose=_odom(7.0, 0.0, 0.2), + start_pose=_odom(-1.0, 0.0, _GROUND_SURFACE_Z), + goal_pose=_odom(7.0, 0.0, _GROUND_SURFACE_Z), expect_path=True, ) @@ -126,8 +129,8 @@ def blocked_wall() -> PlannerScenario: return PlannerScenario( name="blocked_wall", global_map=_map_with_walls(_wall(3.0, -3.0, 3.0, 3.0)), - start_pose=_odom(-1.0, 0.0, 0.2), - goal_pose=_odom(6.0, 0.0, 0.2), + start_pose=_odom(-1.0, 0.0, _GROUND_SURFACE_Z), + goal_pose=_odom(6.0, 0.0, _GROUND_SURFACE_Z), expect_path=False, ) @@ -139,8 +142,8 @@ def two_rooms_one_door() -> PlannerScenario: _wall(3.0, -3.0, 3.0, -0.75), _wall(3.0, 0.75, 3.0, 3.0), ), - start_pose=_odom(-1.0, 0.0, 0.2), - goal_pose=_odom(6.0, 0.0, 0.2), + start_pose=_odom(-1.0, 0.0, _GROUND_SURFACE_Z), + goal_pose=_odom(6.0, 0.0, _GROUND_SURFACE_Z), expect_path=True, ) diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs index 9c56a4d7be..8c6a20472a 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs @@ -75,7 +75,7 @@ pub fn extract_surfaces( ) } -/// Perform dilation and erosion on all xy slices of the extracted surfaces +/// Dilation and erosion on all xy slices of the extracted surfaces /// to fill in small holes. fn close_surface_holes( standable: Vec, @@ -141,6 +141,7 @@ fn close_at_z( } // use L1 dilation/erosion, expand out in cross shape + // could use alternative methods here as well, subject to tuning if dilation_passes > 0 { img = dilate(&img, Norm::L1, dilation_passes as u8); } From 462585e22dec9cb500b779e99f32ae32e435d0a9 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Fri, 29 May 2026 15:26:57 -0700 Subject: [PATCH 44/55] Remove Python code --- .../nav_stack/evaluator/blueprints.py | 54 +- .../modules/mls_planner/mls_planner.py | 822 ------------------ dimos/robot/all_blueprints.py | 1 - 3 files changed, 4 insertions(+), 873 deletions(-) delete mode 100644 dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py diff --git a/dimos/navigation/nav_stack/evaluator/blueprints.py b/dimos/navigation/nav_stack/evaluator/blueprints.py index 0a7bb28dff..5c3a1a4cab 100644 --- a/dimos/navigation/nav_stack/evaluator/blueprints.py +++ b/dimos/navigation/nav_stack/evaluator/blueprints.py @@ -14,7 +14,7 @@ """Blueprint for the path-planner evaluator. -Wires the Evaluator and MLSPlanner together and bridges all streams to rerun. +Wires the Evaluator and MLSPlannerNative together and bridges all streams to rerun. Run with:: dimos run path-planner-eval @@ -22,25 +22,17 @@ from __future__ import annotations -from functools import partial from typing import Any import numpy as np -from scipy.sparse.csgraph import connected_components from dimos.core.coordination.blueprints import autoconnect from dimos.navigation.nav_stack.evaluator.evaluator import Evaluator from dimos.navigation.nav_stack.modules.click_start_goal_router.click_start_goal_router import ( ClickStartGoalRouter, ) -from dimos.navigation.nav_stack.modules.mls_planner.mls_planner import ( - NODE_STEP_THRESHOLD_M, - build_surface_adjacency, - build_surface_lookup, -) from dimos.navigation.nav_stack.modules.mls_planner.mls_planner_native import ( MLSPlannerNative, - MLSPlannerNativeConfig, ) from dimos.visualization.rerun.bridge import RerunBridgeModule from dimos.visualization.rerun.websocket_server import RerunWebSocketServer @@ -48,21 +40,6 @@ _POSE_MARKER_RADIUS = 0.4 # Small lift so graph artifacts render visibly above the surface points instead of z-fighting. _GRAPH_Z_LIFT = 0.05 -_SURFACE_COMPONENT_PALETTE = np.array( - [ - [245, 140, 150], - [245, 185, 120], - [245, 225, 125], - [170, 220, 135], - [125, 220, 195], - [130, 195, 230], - [170, 160, 230], - [210, 160, 230], - [230, 160, 195], - [225, 200, 145], - ], - dtype=np.uint8, -) def _render_start_pose(msg: Any) -> Any: @@ -89,29 +66,8 @@ def _render_global_map(msg: Any) -> Any: return msg.to_rerun(voxel_size=0.03, colors=[128, 128, 128]) -def _render_surface_map(voxel_size: float, msg: Any) -> Any: - """Render surface points colored by connected component.""" - import rerun as rr - - pts, _ = msg.as_numpy() - if pts is None or len(pts) == 0: - return rr.Points3D([]) - pts = pts.astype(np.float32) - indices = np.floor(pts / voxel_size).astype(np.int64) - ix, iy, iz = indices[:, 0], indices[:, 1], indices[:, 2] - surface_lookup = build_surface_lookup(ix, iy, iz) - step_cells = max(0, int(NODE_STEP_THRESHOLD_M / voxel_size)) - adj, cell_to_idx, _ = build_surface_adjacency(surface_lookup, voxel_size, step_cells) - _, labels = connected_components(adj, directed=False) - point_labels = np.array( - [ - labels[cell_to_idx[cell]] - for cell in zip(ix.tolist(), iy.tolist(), iz.tolist(), strict=True) - ], - dtype=np.int64, - ) - colors = _SURFACE_COMPONENT_PALETTE[point_labels % len(_SURFACE_COMPONENT_PALETTE)] - return rr.Points3D(positions=pts, colors=colors, radii=[0.05]) +def _render_surface_map(msg: Any) -> Any: + return msg.to_rerun(voxel_size=0.1, colors=[40, 75, 130]) def _render_nodes(msg: Any) -> Any: @@ -150,8 +106,6 @@ def _render_node_edges(msg: Any) -> Any: return rr.LineStrips3D(strips, colors=colors, radii=[0.04] * len(strips)) -_planner_voxel = MLSPlannerNativeConfig().voxel_size - path_planner_eval = autoconnect( Evaluator.blueprint(), MLSPlannerNative.blueprint(), @@ -162,7 +116,7 @@ def _render_node_edges(msg: Any) -> Any: "world/start_pose": _render_start_pose, "world/goal_pose": _render_goal_pose, "world/global_map": _render_global_map, - "world/surface_map": partial(_render_surface_map, _planner_voxel), + "world/surface_map": _render_surface_map, "world/nodes": _render_nodes, "world/node_edges": _render_node_edges, } diff --git a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py b/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py deleted file mode 100644 index 88727a6bce..0000000000 --- a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner.py +++ /dev/null @@ -1,822 +0,0 @@ -# Copyright 2026 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. - -"""Multi-level surface path planner.""" - -from __future__ import annotations - -from dataclasses import dataclass, field -import time -from typing import Any - -from dimos_lcm.geometry_msgs import ( - Point as LCMPoint, - Pose as LCMPose, - PoseStamped as LCMPoseStamped, - Quaternion as LCMQuaternion, -) -from dimos_lcm.nav_msgs import Path as LCMPath -from dimos_lcm.std_msgs import Header as LCMHeader, Time as LCMTime -import networkx as nx -import numpy as np -from scipy import ndimage -from scipy.sparse import csr_matrix -from scipy.sparse.csgraph import dijkstra -from scipy.spatial import cKDTree - -from dimos.core.module import Module, ModuleConfig -from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.nav_msgs.LineSegments3D import LineSegments3D -from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.nav_msgs.Path import Path, sec_nsec -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.utils.logging_config import setup_logger - -logger = setup_logger() - -SURFACE_DILATION_PASSES = 3 -SURFACE_EROSION_PASSES = 3 - -NODE_SPACING_M = 1.0 -NODE_WALL_BUFFER_M = 0.3 -NODE_STEP_THRESHOLD_M = 0.25 - - -class MLSPlannerConfig(ModuleConfig): - world_frame: str = "map" - voxel_size: float = 0.1 - robot_height: float = 1.5 - - -def surface_point_xyz(ix: int, iy: int, iz: int, voxel_size: float) -> tuple[float, float, float]: - """Standing-surface coord for one cell. XY centered in the cell, Z at the cell's top face.""" - return ((ix + 0.5) * voxel_size, (iy + 0.5) * voxel_size, (iz + 1.0) * voxel_size) - - -def surface_points_xyz(cells: np.ndarray, voxel_size: float) -> np.ndarray: - """Vectorized surface_point_xyz. (N, 3) int cell indices to (N, 3) world XYZ.""" - offset = np.array([0.5 * voxel_size, 0.5 * voxel_size, voxel_size]) - return cells * voxel_size + offset - - -@dataclass -class SurfaceGraph: - """Surface-cell grid plus its waypoint-node overlay. - - place_nodes populates the first five fields. add_node_edges fills source_cells - and cell_idx_to_nid and adds edges to graph. - """ - - graph: nx.Graph - adj: csr_matrix - cell_to_idx: dict[tuple[int, int, int], int] - idx_to_cell: list[tuple[int, int, int]] - surface_lookup: dict[tuple[int, int], np.ndarray] - source_cells: np.ndarray = field(default_factory=lambda: np.zeros(0, dtype=np.int64)) - cell_idx_to_nid: dict[int, int] = field(default_factory=dict) - - -def _extract_surfaces(points: np.ndarray, voxel_size: float, robot_height: float) -> np.ndarray: - """For each XY column, mark cells with at least robot_height of free space above as surfaces.""" - if len(points) == 0: - return np.zeros((0, 3), dtype=np.float32) - - indices = np.floor(points / voxel_size).astype(np.int64) - ix, iy, iz = indices[:, 0], indices[:, 1], indices[:, 2] - - order = np.lexsort((iz, iy, ix)) - ix_sorted, iy_sorted, iz_sorted = ix[order], iy[order], iz[order] - - height_cells = int(np.ceil(robot_height / voxel_size)) - - next_same_col = np.zeros(len(ix_sorted), dtype=bool) - next_same_col[:-1] = (ix_sorted[:-1] == ix_sorted[1:]) & (iy_sorted[:-1] == iy_sorted[1:]) - - gap = np.empty(len(ix_sorted), dtype=np.int64) - gap[:-1] = iz_sorted[1:] - iz_sorted[:-1] - gap[-1] = 0 - - is_surface = (~next_same_col) | (gap > height_cells) - - surface_ix = ix_sorted[is_surface] - surface_iy = iy_sorted[is_surface] - surface_iz = iz_sorted[is_surface] - - surface_ix, surface_iy, surface_iz = _close_surface_holes( - surface_ix, - surface_iy, - surface_iz, - SURFACE_DILATION_PASSES, - SURFACE_EROSION_PASSES, - ix, - iy, - iz, - height_cells, - ) - - cells = np.column_stack([surface_ix, surface_iy, surface_iz]) - return surface_points_xyz(cells, voxel_size).astype(np.float32) - - -def _close_surface_holes( - surface_ix: np.ndarray, - surface_iy: np.ndarray, - surface_iz: np.ndarray, - dilation_passes: int, - erosion_passes: int, - obstacle_ix: np.ndarray, - obstacle_iy: np.ndarray, - obstacle_iz: np.ndarray, - height_cells: int, -) -> tuple[np.ndarray, np.ndarray, np.ndarray]: - """Dilate then erode the surface map at each z level, without bridging walls. - - Fills small holes from missing lidar points, then rejects dilated cells whose - column has an obstacle point at z' in (level_iz, level_iz + height_cells]. - That's the same clearance check the surface extractor uses, applied to the - dilated cells so morphology can't bridge across a wall column. - """ - if len(surface_ix) == 0 or (dilation_passes <= 0 and erosion_passes <= 0): - return surface_ix, surface_iy, surface_iz - - pad = max(dilation_passes, 0) - new_ix: list[np.ndarray] = [] - new_iy: list[np.ndarray] = [] - new_iz: list[np.ndarray] = [] - for level_iz in np.unique(surface_iz): - sel = surface_iz == level_iz - lx = surface_ix[sel] - ly = surface_iy[sel] - x0, x1 = int(lx.min()), int(lx.max()) - y0, y1 = int(ly.min()), int(ly.max()) - w = x1 - x0 + 1 + 2 * pad - h = y1 - y0 + 1 + 2 * pad - # numpy mask is indexed (row, col) = (y, x). - mask = np.zeros((h, w), dtype=bool) - mask[ly - y0 + pad, lx - x0 + pad] = True - if dilation_passes > 0: - mask = ndimage.binary_dilation(mask, iterations=dilation_passes) - if erosion_passes > 0: - mask = ndimage.binary_erosion(mask, iterations=erosion_passes) - - blocking = ( - (obstacle_iz > level_iz) - & (obstacle_iz <= level_iz + height_cells) - & (obstacle_ix >= x0 - pad) - & (obstacle_ix <= x1 + pad) - & (obstacle_iy >= y0 - pad) - & (obstacle_iy <= y1 + pad) - ) - if blocking.any(): - blocked = np.zeros((h, w), dtype=bool) - blocked[ - obstacle_iy[blocking] - y0 + pad, - obstacle_ix[blocking] - x0 + pad, - ] = True - mask = mask & ~blocked - - ys, xs = np.where(mask) - new_ix.append(xs.astype(np.int64) + x0 - pad) - new_iy.append(ys.astype(np.int64) + y0 - pad) - new_iz.append(np.full(len(xs), level_iz, dtype=np.int64)) - - return ( - np.concatenate(new_ix), - np.concatenate(new_iy), - np.concatenate(new_iz), - ) - - -def build_surface_lookup( - ix: np.ndarray, iy: np.ndarray, iz: np.ndarray -) -> dict[tuple[int, int], np.ndarray]: - """Group surface cells by XY column, deduping z values per column. - - Public so downstream code can recompute the same lookup the planner uses. - """ - by_column: dict[tuple[int, int], list[int]] = {} - for ix_, iy_, iz_ in zip(ix.tolist(), iy.tolist(), iz.tolist(), strict=True): - by_column.setdefault((ix_, iy_), []).append(iz_) - return {key: np.unique(np.array(vs, dtype=np.int64)) for key, vs in by_column.items()} - - -def build_surface_adjacency( - surface_lookup: dict[tuple[int, int], np.ndarray], - voxel_size: float, - step_threshold_cells: int, -) -> tuple[csr_matrix, dict[tuple[int, int, int], int], list[tuple[int, int, int]]]: - """Sparse 8-connected adjacency over surface cells, with a per-step dz cap. - - Public so downstream code can recompute the same adjacency the planner uses. - """ - n = sum(len(zs) for zs in surface_lookup.values()) - if n == 0: - return csr_matrix((0, 0), dtype=np.float64), {}, [] - - ix = np.empty(n, dtype=np.int64) - iy = np.empty(n, dtype=np.int64) - iz = np.empty(n, dtype=np.int64) - cursor = 0 - for (ix_col, iy_col), zs in surface_lookup.items(): - k = len(zs) - ix[cursor : cursor + k] = int(ix_col) - iy[cursor : cursor + k] = int(iy_col) - iz[cursor : cursor + k] = zs - cursor += k - - idx_to_cell: list[tuple[int, int, int]] = list( - zip(ix.tolist(), iy.tolist(), iz.tolist(), strict=True) - ) - cell_to_idx: dict[tuple[int, int, int], int] = {cell: i for i, cell in enumerate(idx_to_cell)} - - # Pack (ix, iy) into one int64 key. Offsets leave room for dx, dy in -1, 0, 1. - ix_offset = ix - ix.min() + 1 - iy_offset = iy - iy.min() + 1 - y_stride = int(iy_offset.max()) + 2 - col_key = ix_offset * y_stride + iy_offset - - sort_order = np.lexsort((iz, col_key)) - sorted_col_key = col_key[sort_order] - sorted_iz = iz[sort_order] - - row_chunks: list[np.ndarray] = [] - col_chunks: list[np.ndarray] = [] - data_chunks: list[np.ndarray] = [] - for dx, dy in ((-1, -1), (-1, 0), (-1, 1), (0, -1), (0, 1), (1, -1), (1, 0), (1, 1)): - # For each source cell, range-scan the (dx, dy) neighbor column for its surface cells, - # then keep only the dz transitions that are within the per-step limit. - neighbor_key = (ix_offset + dx) * y_stride + (iy_offset + dy) - lo = np.searchsorted(sorted_col_key, neighbor_key, side="left") - hi = np.searchsorted(sorted_col_key, neighbor_key, side="right") - n_per_src = hi - lo - total = int(n_per_src.sum()) - if total == 0: - continue - src_per_candidate = np.repeat(np.arange(n), n_per_src) - starts = np.zeros(n, dtype=np.int64) - starts[1:] = np.cumsum(n_per_src[:-1]) - candidate_in_sorted = lo[src_per_candidate] + (np.arange(total) - starts[src_per_candidate]) - dz = sorted_iz[candidate_in_sorted] - iz[src_per_candidate] - valid = np.abs(dz) <= step_threshold_cells - if not valid.any(): - continue - src_valid = src_per_candidate[valid] - dst_valid = sort_order[candidate_in_sorted[valid]] - dz_valid = dz[valid] - step_cost = np.sqrt(dx * dx + dy * dy + dz_valid * dz_valid) * voxel_size - row_chunks.append(src_valid) - col_chunks.append(dst_valid) - data_chunks.append(step_cost.astype(np.float64)) - - if not row_chunks: - return csr_matrix((n, n), dtype=np.float64), cell_to_idx, idx_to_cell - - rows = np.concatenate(row_chunks) - cols = np.concatenate(col_chunks) - data = np.concatenate(data_chunks) - return csr_matrix((data, (rows, cols)), shape=(n, n)), cell_to_idx, idx_to_cell - - -def _walk_predecessors( - predecessors: np.ndarray, end_idx: int, idx_to_cell: list[tuple[int, int, int]] -) -> list[tuple[int, int, int]]: - """Walk predecessors from end_idx back to its scipy Dijkstra source (neg predecessor).""" - cells: list[tuple[int, int, int]] = [idx_to_cell[end_idx]] - cur = end_idx - while True: - nxt = int(predecessors[cur]) - if nxt < 0: - break - cells.append(idx_to_cell[nxt]) - cur = nxt - return cells - - -def _wall_safe_adjacency( - adj: csr_matrix, - dist_to_wall: np.ndarray, - missing_neighbors: np.ndarray, - buffer_m: float, - voxel_size: float, -) -> csr_matrix: - """Scale adjacency edge costs up for cells near walls or with high edge exposure. - - Two multiplicative penalties combine per cell: - - Distance penalty (buffer_m / dist)^4 clamped to >= 1, with a small dist - floor so cells exactly on an edge stay distinguishable from cells one - voxel in. - - Exposure penalty 1 + missing/2 from the count of missing same-z neighbors, - gated by proximity to a wall. Far from walls, a missing neighbor is almost - always just a small hole in the surface map (mesh-sampling noise), not a - real wall edge -- so the exposure factor fades to 1 as dist exceeds buffer. - Near walls, the full corner-avoidance behavior kicks in. - - Per-edge cost averages the two endpoints' penalties. - """ - safe_dist = np.maximum(dist_to_wall, voxel_size / 10.0) - dist_penalty = np.maximum(1.0, (buffer_m / safe_dist) ** 4) - near_wall = np.clip(1.0 - dist_to_wall / buffer_m, 0.0, 1.0) - exposure_penalty = 1.0 + (missing_neighbors / 2.0) * near_wall - penalty = dist_penalty * exposure_penalty - src = np.repeat(np.arange(adj.shape[0]), np.diff(adj.indptr)) - dst = adj.indices - scaled = adj.data * (penalty[src] + penalty[dst]) / 2.0 - return csr_matrix((scaled, adj.indices.copy(), adj.indptr.copy()), shape=adj.shape) - - -def place_nodes( - surface_points: np.ndarray, - voxel_size: float, - *, - node_spacing: float, - wall_buffer: float, - step_threshold: float, -) -> SurfaceGraph: - """Place nodes by greedy NMS on the dist-to-wall field from one Dijkstra. - - Run multisource Dijkstra with the surface edges as sources to find distance from wall. - Then place nodes spaced throughout based on that distance. The returned adjacency - is wall-safe: edge costs scale up within wall_buffer of any wall so add_node_edges - routes through corridor centers. - """ - graph = nx.Graph() - if len(surface_points) == 0: - return SurfaceGraph( - graph=graph, - adj=csr_matrix((0, 0), dtype=np.float64), - cell_to_idx={}, - idx_to_cell=[], - surface_lookup={}, - ) - - sx = np.floor(surface_points[:, 0] / voxel_size).astype(np.int64) - sy = np.floor(surface_points[:, 1] / voxel_size).astype(np.int64) - sz = np.floor(surface_points[:, 2] / voxel_size).astype(np.int64) - surface_lookup = build_surface_lookup(sx, sy, sz) - - step_cells = max(0, int(step_threshold / voxel_size)) - adj, cell_to_idx, idx_to_cell = build_surface_adjacency(surface_lookup, voxel_size, step_cells) - - n_cells = adj.shape[0] - if n_cells == 0: - return SurfaceGraph( - graph=graph, - adj=adj, - cell_to_idx=cell_to_idx, - idx_to_cell=idx_to_cell, - surface_lookup=surface_lookup, - ) - - neighbor_count = np.diff(adj.indptr) - wall_adjacent_indices = np.where(neighbor_count < 8)[0] - if len(wall_adjacent_indices) == 0: - wall_adjacent_indices = np.array([0], dtype=np.int64) - - dist = dijkstra(adj, indices=wall_adjacent_indices, min_only=True) - - # Same-z adjacency feeds the exposure penalty only. - adj_xy, _, _ = build_surface_adjacency(surface_lookup, voxel_size, 0) - xy_neighbor_count = np.diff(adj_xy.indptr) - - cells_arr = np.array(idx_to_cell, dtype=np.float64) - cell_positions = surface_points_xyz(cells_arr, voxel_size) - - candidate_mask = np.isfinite(dist) & (dist >= wall_buffer) - candidate_indices = np.where(candidate_mask)[0] - if len(candidate_indices) == 0: - return SurfaceGraph( - graph=graph, - adj=adj, - cell_to_idx=cell_to_idx, - idx_to_cell=idx_to_cell, - surface_lookup=surface_lookup, - ) - order = candidate_indices[np.argsort(-dist[candidate_indices])] - positions = cell_positions[order] - - tree = cKDTree(positions) - killed = np.zeros(len(order), dtype=bool) - - for i in range(len(order)): - if killed[i]: - continue - cix, ciy, ciz = idx_to_cell[int(order[i])] - nid = graph.number_of_nodes() - graph.add_node( - nid, - pos=surface_point_xyz(cix, ciy, ciz, voxel_size), - cell=(cix, ciy, ciz), - ) - nearby = tree.query_ball_point(positions[i], r=node_spacing) - killed[np.asarray(nearby, dtype=np.int64)] = True - - missing_neighbors = 8 - xy_neighbor_count - adj_safe = _wall_safe_adjacency(adj, dist, missing_neighbors, wall_buffer, voxel_size) - return SurfaceGraph( - graph=graph, - adj=adj_safe, - cell_to_idx=cell_to_idx, - idx_to_cell=idx_to_cell, - surface_lookup=surface_lookup, - ) - - -def add_node_edges(sg: SurfaceGraph) -> None: - """Add Voronoi-adjacency edges between placed nodes, mutating sg in place. - - One multi-source Dijkstra labels each cell with its nearest node. The cheapest - boundary crossing per node-pair becomes an edge with the cell path on data["path"]. - Also fills sg.source_cells (per-cell owner) and sg.cell_idx_to_nid for pose-snapping. - """ - if sg.graph.number_of_nodes() == 0: - sg.source_cells = np.full(sg.adj.shape[0], -9999, dtype=np.int64) - sg.cell_idx_to_nid = {} - return - - node_ids = list(sg.graph.nodes()) - source_cell_indices = np.empty(len(node_ids), dtype=np.int64) - cell_idx_to_nid: dict[int, int] = {} - for nid in node_ids: - cell_idx = sg.cell_to_idx[sg.graph.nodes[nid]["cell"]] - source_cell_indices[nid] = cell_idx - cell_idx_to_nid[cell_idx] = nid - - dist, predecessors, source_cells = dijkstra( - sg.adj, - indices=source_cell_indices, - min_only=True, - return_predecessors=True, - ) - - rows = np.repeat(np.arange(sg.adj.shape[0]), np.diff(sg.adj.indptr)) - cols = sg.adj.indices - weights = sg.adj.data - src_u = source_cells[rows] - src_v = source_cells[cols] - boundary = (src_u != src_v) & (src_u >= 0) & (src_v >= 0) - sg.source_cells = source_cells - sg.cell_idx_to_nid = cell_idx_to_nid - if not boundary.any(): - return - - b_rows = rows[boundary] - b_cols = cols[boundary] - b_costs = dist[b_rows] + weights[boundary] + dist[b_cols] - b_src_u = src_u[boundary] - b_src_v = src_v[boundary] - - # Keep the min-cost boundary crossing per node pair. - best: dict[tuple[int, int], tuple[float, int, int]] = {} - for i in range(len(b_costs)): - nid_a = cell_idx_to_nid[int(b_src_u[i])] - nid_b = cell_idx_to_nid[int(b_src_v[i])] - u_a = int(b_rows[i]) - u_b = int(b_cols[i]) - if nid_a > nid_b: - nid_a, nid_b = nid_b, nid_a - u_a, u_b = u_b, u_a - cost = float(b_costs[i]) - existing = best.get((nid_a, nid_b)) - if existing is None or existing[0] > cost: - best[(nid_a, nid_b)] = (cost, u_a, u_b) - - for (nid_a, nid_b), (cost, u_a, u_b) in best.items(): - path_a = _walk_predecessors(predecessors, u_a, sg.idx_to_cell) - path_b = _walk_predecessors(predecessors, u_b, sg.idx_to_cell) - path_a.reverse() - full_path = np.array(path_a + path_b, dtype=np.int64) - sg.graph.add_edge(nid_a, nid_b, weight=cost, path=full_path) - - -class _LineSegmentsAsPath(LineSegments3D): - """Pack LineSegments3D into nav_msgs/Path until a dedicated message exists. - - Segment endpoints alternate as poses (p1, p2, p1, p2, ...) and traversability rides - on each pose's orientation.w. - """ - - def lcm_encode(self) -> bytes: - lcm_msg = LCMPath() - sec, nsec = sec_nsec(self.ts) - lcm_poses = [] - for (p1, p2), trav in zip(self._segments, self._traversability, strict=False): - for pt in (p1, p2): - lp = LCMPoseStamped() - lp.pose = LCMPose() - lp.pose.position = LCMPoint() - lp.pose.orientation = LCMQuaternion() - lp.pose.position.x = pt[0] - lp.pose.position.y = pt[1] - lp.pose.position.z = pt[2] - lp.pose.orientation.w = trav - lp.header = LCMHeader() - lp.header.stamp = LCMTime() - lp.header.stamp.sec = sec - lp.header.stamp.nsec = nsec - lp.header.frame_id = self.frame_id - lcm_poses.append(lp) - lcm_msg.poses_length = len(lcm_poses) - lcm_msg.poses = lcm_poses - lcm_msg.header.stamp.sec = sec - lcm_msg.header.stamp.nsec = nsec - lcm_msg.header.frame_id = self.frame_id - return lcm_msg.lcm_encode() # type: ignore[no-any-return] - - -def _nodes_to_cloud(graph: nx.Graph) -> np.ndarray: - if graph.number_of_nodes() == 0: - return np.zeros((0, 3), dtype=np.float32) - return np.array([graph.nodes[n]["pos"] for n in graph.nodes()], dtype=np.float32) - - -def _edges_to_segments( - sg: SurfaceGraph, voxel_size: float -) -> tuple[ - list[tuple[tuple[float, float, float], tuple[float, float, float]]], - list[float], -]: - """Emit one segment per consecutive cell pair on each edge. - - All segments belonging to the same graph edge share the edge's total weight - so the renderer can color the whole edge uniformly. - """ - segments: list[tuple[tuple[float, float, float], tuple[float, float, float]]] = [] - weights: list[float] = [] - for _, _, edge_data in sg.graph.edges(data=True): - path_cells: np.ndarray = edge_data["path"] - edge_weight = float(edge_data.get("weight", 0.0)) - for i in range(len(path_cells) - 1): - a = path_cells[i] - b = path_cells[i + 1] - segments.append( - ( - surface_point_xyz(int(a[0]), int(a[1]), int(a[2]), voxel_size), - surface_point_xyz(int(b[0]), int(b[1]), int(b[2]), voxel_size), - ) - ) - weights.append(edge_weight) - return segments, weights - - -class MLSPlanner(Module): - config: MLSPlannerConfig - - global_map: In[PointCloud2] - start_pose: In[Odometry] - goal_pose: In[Odometry] - path: Out[Path] - surface_map: Out[PointCloud2] - nodes: Out[PointCloud2] - node_edges: Out[LineSegments3D] - - def __init__(self, **kwargs: Any) -> None: - super().__init__(**kwargs) - self._latest_start: tuple[float, float, float] | None = None - self._surface_graph: SurfaceGraph | None = None - - async def handle_global_map(self, msg: PointCloud2) -> None: - points, _ = msg.as_numpy() - if points is None or len(points) == 0: - return - - # 1. Surface extraction - t0 = time.perf_counter() - surface_points = _extract_surfaces(points, self.config.voxel_size, self.config.robot_height) - surfaces_ms = (time.perf_counter() - t0) * 1000 - self.surface_map.publish( - PointCloud2.from_numpy( - surface_points, frame_id=self.config.world_frame, timestamp=time.time() - ) - ) - logger.info( - "Surfaces ready", - surfaces=len(surface_points), - surface_ms=round(surfaces_ms, 1), - ) - - # 2. Node placement - logger.info("Placing nodes", spacing_m=NODE_SPACING_M) - t1 = time.perf_counter() - sg = place_nodes( - surface_points, - self.config.voxel_size, - node_spacing=NODE_SPACING_M, - wall_buffer=NODE_WALL_BUFFER_M, - step_threshold=NODE_STEP_THRESHOLD_M, - ) - place_ms = (time.perf_counter() - t1) * 1000 - self.nodes.publish( - PointCloud2.from_numpy( - _nodes_to_cloud(sg.graph), - frame_id=self.config.world_frame, - timestamp=time.time(), - ) - ) - logger.info( - "Nodes placed", - nodes=sg.graph.number_of_nodes(), - place_ms=round(place_ms, 1), - ) - - # 3. Edge construction - logger.info("Building edges") - t2 = time.perf_counter() - add_node_edges(sg) - edges_ms = (time.perf_counter() - t2) * 1000 - logger.info( - "Edges built", - edges=sg.graph.number_of_edges(), - edges_ms=round(edges_ms, 1), - ) - - self._surface_graph = sg - segments, segment_weights = _edges_to_segments(sg, self.config.voxel_size) - self.node_edges.publish( - _LineSegmentsAsPath( - ts=time.time(), - frame_id=self.config.world_frame, - segments=segments, - traversability=segment_weights, - ) - ) - - async def handle_start_pose(self, msg: Odometry) -> None: - self._latest_start = (msg.x, msg.y, msg.z) - self._publish_empty_path() - - def _publish_empty_path(self) -> None: - """Clear any previously published path so the visualizer drops the stale plan.""" - self.path.publish(Path(ts=time.time(), frame_id=self.config.world_frame, poses=[])) - - async def handle_goal_pose(self, msg: Odometry) -> None: - self._plan_to((msg.x, msg.y, msg.z)) - - def _plan_to(self, goal: tuple[float, float, float]) -> None: - if self._latest_start is None: - logger.warning("MLSPlanner received goal before start; skipping") - return - sg = self._surface_graph - if sg is None or sg.graph.number_of_nodes() == 0: - logger.warning("MLSPlanner received goal before graph was built; skipping") - return - - t0 = time.perf_counter() - start = self._latest_start - - start_node = self._snap_pose_to_node(sg, start) - goal_node = self._snap_pose_to_node(sg, goal) - if start_node is None or goal_node is None: - logger.warning( - "Could not snap pose to graph", - start=start, - goal=goal, - start_node=start_node, - goal_node=goal_node, - ) - self._publish_empty_path() - return - logger.info( - "Snapped poses to graph nodes", - start_pose=start, - start_node=start_node, - start_node_pos=sg.graph.nodes[start_node]["pos"], - goal_pose=goal, - goal_node=goal_node, - goal_node_pos=sg.graph.nodes[goal_node]["pos"], - ) - - try: - node_seq = nx.shortest_path( - sg.graph, source=start_node, target=goal_node, weight="weight" - ) - except nx.NetworkXNoPath: - logger.warning( - "No path between start and goal nodes", - start_node=start_node, - goal_node=goal_node, - ) - self._publish_empty_path() - return - - waypoints = self._assemble_waypoints(sg, node_seq, start, goal) - plan_ms = (time.perf_counter() - t0) * 1000 - - now = time.time() - path_msg = Path( - ts=now, - frame_id=self.config.world_frame, - poses=[ - PoseStamped( - ts=now, - frame_id=self.config.world_frame, - position=[float(x), float(y), float(z)], - orientation=[0.0, 0.0, 0.0, 1.0], - ) - for x, y, z in waypoints - ], - ) - self.path.publish(path_msg) - logger.info( - "Path planned", - waypoints=len(waypoints), - nodes_traversed=len(node_seq), - plan_ms=round(plan_ms, 1), - ) - - def _snap_pose_to_node( - self, sg: SurfaceGraph, pose_xyz: tuple[float, float, float] - ) -> int | None: - """Snap pose to its owning node via the precomputed Voronoi labels. - - Finds the surface cell in the pose's column, looks up its owner in source_cells. - Falls back to nearby columns if the pose's own column has no surface. - """ - if sg.graph.number_of_nodes() == 0: - return None - - voxel = self.config.voxel_size - x, y, z = pose_xyz - ix = int(np.floor(x / voxel)) - iy = int(np.floor(y / voxel)) - target_iz = int(np.floor(z / voxel)) - 1 - tolerance_cells = int(np.ceil(self.config.robot_height / voxel)) - - cell = self._best_iz_in_column(sg, ix, iy, target_iz, tolerance_cells) - if cell is None: - # Pose's column has no surface. Try nearby columns. - search_radius = 5 - best_cell = None - best_d2 = -1 - for dix in range(-search_radius, search_radius + 1): - for diy in range(-search_radius, search_radius + 1): - if dix == 0 and diy == 0: - continue - c = self._best_iz_in_column(sg, ix + dix, iy + diy, target_iz, tolerance_cells) - if c is None: - continue - d2 = dix * dix + diy * diy - if best_d2 < 0 or d2 < best_d2: - best_d2 = d2 - best_cell = c - cell = best_cell - if cell is None: - return None - - cell_idx = sg.cell_to_idx.get(cell) - if cell_idx is None: - return None - owner_cell_idx = int(sg.source_cells[cell_idx]) - if owner_cell_idx < 0: - return None - return sg.cell_idx_to_nid.get(owner_cell_idx) - - def _best_iz_in_column( - self, sg: SurfaceGraph, ix: int, iy: int, target_iz: int, tolerance_cells: int - ) -> tuple[int, int, int] | None: - """Surface iz in column (ix, iy) closest to target_iz, within tolerance.""" - zs = sg.surface_lookup.get((ix, iy)) - if zs is None or len(zs) == 0: - return None - distances = np.abs(zs - target_iz) - best = int(distances.argmin()) - if int(distances[best]) > tolerance_cells: - return None - return (ix, iy, int(zs[best])) - - def _assemble_waypoints( - self, - sg: SurfaceGraph, - node_seq: list[int], - start_pose: tuple[float, float, float], - goal_pose: tuple[float, float, float], - ) -> list[tuple[float, float, float]]: - """Chain cached edge paths into a continuous waypoint list, bracketed by the actual poses.""" - voxel = self.config.voxel_size - cells: list[tuple[int, int, int]] = [] - for i in range(len(node_seq) - 1): - a, b = node_seq[i], node_seq[i + 1] - edge_path: np.ndarray = sg.graph[a][b]["path"] - # Stored path runs from min(a, b) to max(a, b). Reverse if traversing the other way. - if a > b: - edge_path = edge_path[::-1] - tail = edge_path if i == 0 else edge_path[1:] - for c in tail: - cells.append((int(c[0]), int(c[1]), int(c[2]))) - - waypoints: list[tuple[float, float, float]] = [start_pose] - for cix, ciy, ciz in cells: - waypoints.append(surface_point_xyz(cix, ciy, ciz, voxel)) - waypoints.append(goal_pose) - return waypoints diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index d253df414d..a1099800f9 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -173,7 +173,6 @@ "mcp-client": "dimos.agents.mcp.mcp_client.McpClient", "mcp-server": "dimos.agents.mcp.mcp_server.McpServer", "memory-module": "dimos.memory2.module.MemoryModule", - "mls-planner": "dimos.navigation.nav_stack.modules.mls_planner.mls_planner.MLSPlanner", "mls-planner-native": "dimos.navigation.nav_stack.modules.mls_planner.mls_planner_native.MLSPlannerNative", "mock-b1-connection-module": "dimos.robot.unitree.b1.connection.MockB1ConnectionModule", "module-a": "dimos.robot.unitree.demo_error_on_name_conflicts.ModuleA", From c9168e5a8e0f2389396d1ef1b21399c663660f31 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Fri, 29 May 2026 15:46:35 -0700 Subject: [PATCH 45/55] Remove unused code --- .../modules/mls_planner/rust/src/adjacency.rs | 11 ++--------- .../modules/mls_planner/rust/src/dijkstra.rs | 3 --- .../nav_stack/modules/mls_planner/rust/src/edges.rs | 5 ----- .../nav_stack/modules/mls_planner/rust/src/nodes.rs | 2 -- .../nav_stack/modules/mls_planner/rust/src/plan.rs | 2 -- .../modules/mls_planner/rust/src/surfaces.rs | 2 -- .../nav_stack/modules/mls_planner/rust/src/voxel.rs | 2 -- 7 files changed, 2 insertions(+), 25 deletions(-) diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/adjacency.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/adjacency.rs index aeb9e1e0e5..8ba3f07797 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/adjacency.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/adjacency.rs @@ -1,8 +1,6 @@ // Copyright 2026 Dimensional Inc. // SPDX-License-Identifier: Apache-2.0 -#![allow(dead_code)] // consumed incrementally by later stage modules - use ahash::AHashMap; use crate::voxel::VoxelKey; @@ -58,10 +56,6 @@ impl SurfaceAdjacency { self.cells.contains_key(&cell) } - pub fn len(&self) -> usize { - self.cells.len() - } - pub fn is_empty(&self) -> bool { self.cells.is_empty() } @@ -131,7 +125,6 @@ mod tests { fn empty_input_yields_empty_adjacency() { let lookup = build_surface_lookup(&[]); let adj = build_surface_adjacency(&lookup, VOXEL, 2); - assert_eq!(adj.len(), 0); assert!(adj.is_empty()); } @@ -139,7 +132,7 @@ mod tests { fn single_cell_has_no_edges() { let lookup = build_surface_lookup(&[(0, 0, 0)]); let adj = build_surface_adjacency(&lookup, VOXEL, 2); - assert_eq!(adj.len(), 1); + assert_eq!(adj.cells().count(), 1); assert!(adj.contains((0, 0, 0))); assert!(neighbors_of(&adj, (0, 0, 0)).is_empty()); } @@ -207,6 +200,6 @@ mod tests { fn deduplicates_repeated_cells() { let lookup = build_surface_lookup(&[(0, 0, 0), (0, 0, 0), (1, 0, 0)]); let adj = build_surface_adjacency(&lookup, VOXEL, 2); - assert_eq!(adj.len(), 2); + assert_eq!(adj.cells().count(), 2); } } diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/dijkstra.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/dijkstra.rs index 9cc31476cd..9b2222a894 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/dijkstra.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/dijkstra.rs @@ -2,9 +2,6 @@ // SPDX-License-Identifier: Apache-2.0 //! Multi-source Dijkstra over the cell-keyed surface adjacency. -//! - -#![allow(dead_code)] use std::cmp::Ordering; use std::collections::BinaryHeap; diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/edges.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/edges.rs index 1d708adfa4..9afdec5aa1 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/edges.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/edges.rs @@ -8,8 +8,6 @@ //! the Voronoi region. We use the boundaries of these regions to build the //! edges between start nodes. -#![allow(dead_code)] - use ahash::AHashMap; use crate::adjacency::{SurfaceAdjacency, SurfaceLookup}; @@ -28,7 +26,6 @@ pub struct NodeEdge { } pub struct PlannerGraph { - pub adj: SurfaceAdjacency, pub surface_lookup: SurfaceLookup, pub nodes: Vec, pub node_edges: Vec, @@ -46,7 +43,6 @@ pub fn add_node_edges(sg: SurfaceGraph) -> PlannerGraph { if nodes.is_empty() { return PlannerGraph { - adj, surface_lookup, nodes, node_edges: Vec::new(), @@ -66,7 +62,6 @@ pub fn add_node_edges(sg: SurfaceGraph) -> PlannerGraph { } PlannerGraph { - adj, surface_lookup, nodes, node_edges, diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs index ed072dfd52..2d61a3d20f 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs @@ -5,8 +5,6 @@ //! nodes at local maxima via NMS, and rescale cell-edge costs to push paths //! toward corridor centers. -#![allow(dead_code)] // consumed incrementally - use ahash::{AHashMap, AHashSet}; use crate::adjacency::{ diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/plan.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/plan.rs index 044dd0caf3..0cc8d61687 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/plan.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/plan.rs @@ -1,8 +1,6 @@ // Copyright 2026 Dimensional Inc. // SPDX-License-Identifier: Apache-2.0 -#![allow(dead_code)] - use std::cmp::Ordering; use std::collections::BinaryHeap; diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs index 8c6a20472a..30f11e573e 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs @@ -5,8 +5,6 @@ //! robot-height clearance above as standable, then morphologically close //! per-z-level holes without letting closing bridge across walls. -#![allow(dead_code)] // consumed incrementally - use ahash::{AHashMap, AHashSet}; use image::{GrayImage, Luma}; use imageproc::distance_transform::Norm; diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/voxel.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/voxel.rs index 353d728905..69eb9e32b0 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/voxel.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/voxel.rs @@ -3,8 +3,6 @@ //! Voxel-grid coordinate math. -#![allow(dead_code)] // consumed incrementally by later stage modules - pub type VoxelKey = (i32, i32, i32); #[inline] From 6424a0571e92154957ecf90780b638f70826fbd5 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Fri, 29 May 2026 16:26:30 -0700 Subject: [PATCH 46/55] Remove unused code --- .../modules/mls_planner/rust/src/adjacency.rs | 33 +---------- .../modules/mls_planner/rust/src/dijkstra.rs | 11 +--- .../modules/mls_planner/rust/src/edges.rs | 21 ------- .../modules/mls_planner/rust/src/main.rs | 56 +++++++------------ .../modules/mls_planner/rust/src/nodes.rs | 43 +------------- .../modules/mls_planner/rust/src/plan.rs | 15 +---- .../modules/mls_planner/rust/src/surfaces.rs | 23 +++----- .../modules/mls_planner/rust/src/voxel.rs | 34 ----------- 8 files changed, 35 insertions(+), 201 deletions(-) diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/adjacency.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/adjacency.rs index 8ba3f07797..6e07697a1d 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/adjacency.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/adjacency.rs @@ -74,7 +74,7 @@ pub fn build_surface_lookup(cells: &[VoxelKey]) -> SurfaceLookup { lookup } -/// 4 way connection (L1) and check for step height threshold +/// 4 way L1 adjacency, limited by step height threshold. pub fn build_surface_adjacency( surface_lookup: &SurfaceLookup, voxel_size: f32, @@ -121,22 +121,6 @@ mod tests { adj.neighbors(cell).collect() } - #[test] - fn empty_input_yields_empty_adjacency() { - let lookup = build_surface_lookup(&[]); - let adj = build_surface_adjacency(&lookup, VOXEL, 2); - assert!(adj.is_empty()); - } - - #[test] - fn single_cell_has_no_edges() { - let lookup = build_surface_lookup(&[(0, 0, 0)]); - let adj = build_surface_adjacency(&lookup, VOXEL, 2); - assert_eq!(adj.cells().count(), 1); - assert!(adj.contains((0, 0, 0))); - assert!(neighbors_of(&adj, (0, 0, 0)).is_empty()); - } - #[test] fn same_z_neighbors_are_bidirectional() { let lookup = build_surface_lookup(&[(0, 0, 0), (1, 0, 0)]); @@ -180,14 +164,6 @@ mod tests { approx_eq(b[0].cost, expected); } - #[test] - fn same_column_cells_are_not_self_connected() { - let lookup = build_surface_lookup(&[(0, 0, 0), (0, 0, 5)]); - let adj = build_surface_adjacency(&lookup, VOXEL, 10); - assert!(neighbors_of(&adj, (0, 0, 0)).is_empty()); - assert!(neighbors_of(&adj, (0, 0, 5)).is_empty()); - } - #[test] fn plus_pattern_center_has_four_neighbors() { let cells = vec![(0, 0, 0), (1, 0, 0), (-1, 0, 0), (0, 1, 0), (0, -1, 0)]; @@ -195,11 +171,4 @@ mod tests { let adj = build_surface_adjacency(&lookup, VOXEL, 2); assert_eq!(neighbors_of(&adj, (0, 0, 0)).len(), 4); } - - #[test] - fn deduplicates_repeated_cells() { - let lookup = build_surface_lookup(&[(0, 0, 0), (0, 0, 0), (1, 0, 0)]); - let adj = build_surface_adjacency(&lookup, VOXEL, 2); - assert_eq!(adj.cells().count(), 2); - } } diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/dijkstra.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/dijkstra.rs index 9b2222a894..15e13c96c7 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/dijkstra.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/dijkstra.rs @@ -14,9 +14,9 @@ use crate::voxel::VoxelKey; #[derive(Clone, Copy, Debug)] pub struct CellState { pub dist: f32, - /// Predecesor nodes along the shortest path to source + /// Predecessor node along the shortest path to source. pub pred: Option, - /// Id of cheapest source to return to + /// Id of cheapest source to return to. pub source: u32, } @@ -120,13 +120,6 @@ mod tests { adj } - #[test] - fn empty_sources_leaves_everything_unreachable() { - let adj = chain(4); - let r = dijkstra(&adj, &[]); - assert!(r.state.is_empty()); - } - #[test] fn single_source_dist_and_pred() { let adj = chain(5); diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/edges.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/edges.rs index 9afdec5aa1..221d8b1ded 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/edges.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/edges.rs @@ -155,9 +155,6 @@ mod tests { const VOXEL: f32 = 0.1; - /// Build a SurfaceGraph from a list of surface cells and a list of node - /// cells (which must be a subset of the surface cells). Bypasses - /// place_nodes so the test author controls which cells become nodes. fn graph_with_nodes(surface_cells: &[VoxelKey], node_cells: &[VoxelKey]) -> SurfaceGraph { let surface_lookup = build_surface_lookup(surface_cells); let adj = build_surface_adjacency(&surface_lookup, VOXEL, 2); @@ -175,28 +172,10 @@ mod tests { } } - /// 20-cell strip along x at iz=0. fn strip_cells() -> Vec { (0..20).map(|x| (x, 0, 0)).collect() } - #[test] - fn no_nodes_yields_no_edges() { - let sg = graph_with_nodes(&strip_cells(), &[]); - let pg = add_node_edges(sg); - assert!(pg.node_edges.is_empty()); - assert!(pg.node_adj.is_empty()); - } - - #[test] - fn single_node_has_no_edges() { - let sg = graph_with_nodes(&strip_cells(), &[(10, 0, 0)]); - let pg = add_node_edges(sg); - assert!(pg.node_edges.is_empty()); - assert_eq!(pg.node_adj.len(), 1); - assert!(pg.node_adj[0].is_empty()); - } - #[test] fn two_nodes_on_strip_have_one_edge() { let sg = graph_with_nodes(&strip_cells(), &[(3, 0, 0), (15, 0, 0)]); diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs index e36125bca8..95ba00aaed 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs @@ -75,36 +75,11 @@ struct MlsPlanner { impl MlsPlanner { async fn setup(&mut self) { let cfg = &self.config; - if !cfg.voxel_size.is_finite() || cfg.voxel_size <= 0.0 { - panic!( - "mls_planner: voxel_size must be > 0, got {}", - cfg.voxel_size - ); - } - if !cfg.robot_height.is_finite() || cfg.robot_height <= 0.0 { - panic!( - "mls_planner: robot_height must be > 0, got {}", - cfg.robot_height - ); - } - if !cfg.node_spacing_m.is_finite() || cfg.node_spacing_m <= 0.0 { - panic!( - "mls_planner: node_spacing_m must be > 0, got {}", - cfg.node_spacing_m - ); - } - if !cfg.node_wall_buffer_m.is_finite() || cfg.node_wall_buffer_m < 0.0 { - panic!( - "mls_planner: node_wall_buffer_m must be >= 0, got {}", - cfg.node_wall_buffer_m - ); - } - if !cfg.node_step_threshold_m.is_finite() || cfg.node_step_threshold_m < 0.0 { - panic!( - "mls_planner: node_step_threshold_m must be >= 0, got {}", - cfg.node_step_threshold_m - ); - } + require_positive("voxel_size", cfg.voxel_size); + require_positive("robot_height", cfg.robot_height); + require_positive("node_spacing_m", cfg.node_spacing_m); + require_non_negative("node_wall_buffer_m", cfg.node_wall_buffer_m); + require_non_negative("node_step_threshold_m", cfg.node_step_threshold_m); self.clearance_cells = (cfg.robot_height / cfg.voxel_size).ceil() as i32; self.step_cells = (cfg.node_step_threshold_m / cfg.voxel_size).floor() as i32; @@ -180,8 +155,8 @@ impl MlsPlanner { publish_path(&self.node_edges, &edges_path).await; info!( - obstacle_points = points.len(), - obstacle_voxels = voxel_map.len(), + global_map_points = points.len(), + voxels = voxel_map.len(), surface_cells = surface_cells.len(), nodes = n_nodes, edges = n_edges, @@ -243,6 +218,18 @@ fn ms(d: Duration) -> f64 { d.as_secs_f64() * 1000.0 } +fn require_positive(name: &str, v: f32) { + if !v.is_finite() || v <= 0.0 { + panic!("mls_planner: {name} must be > 0, got {v}"); + } +} + +fn require_non_negative(name: &str, v: f32) { + if !v.is_finite() || v < 0.0 { + panic!("mls_planner: {name} must be >= 0, got {v}"); + } +} + async fn publish_cloud( out: &Output, points: &[(f32, f32, f32)], @@ -330,9 +317,8 @@ fn build_path_from_waypoints(waypoints: &[(f32, f32, f32)], frame_id: &str, stam } } -/// Emit edges as alternating PoseStamped pairs (p1, p2, p1', p2', ...) with -/// orientation.w carrying the segment's per-edge cost. This is the -/// nav_msgs/LineSegments3D wire hack the Python side already decodes. +/// Emit edges as alternating PoseStamped pairs with orientation.w carrying +/// the per-edge cost. fn build_segments_path(plg: &PlannerGraph, voxel_size: f32, frame_id: &str, stamp: Time) -> Path { let segments = edges_to_segments(plg, voxel_size); let mut poses: Vec = Vec::with_capacity(segments.len() * 2); diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs index 2d61a3d20f..ddce9fc12f 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs @@ -98,8 +98,7 @@ fn wall_adjacent_cells(adj: &SurfaceAdjacency) -> Vec { wall } -/// Bin placed nodes by node_spacing-sized cells. For each candidate, scan the -/// 27 nearby bins for any node within Euclidean node_spacing. +/// Space out nodes based on minimum distance fn nms_grid(candidates_sorted: &[VoxelKey], voxel_size: f32, node_spacing_m: f32) -> Vec { let bin_size = ((node_spacing_m / voxel_size) as i32).max(1); let r_sq = (node_spacing_m as f64) * (node_spacing_m as f64); @@ -144,7 +143,6 @@ fn nms_grid(candidates_sorted: &[VoxelKey], voxel_size: f32, node_spacing_m: f32 /// Scales every edge cost by the average of its endpoint penalties, which /// pushes shortest paths away from walls. -/// Subject to tuning... fn apply_wall_safe_penalty( adj: &mut SurfaceAdjacency, dist: &AHashMap, @@ -176,16 +174,6 @@ mod tests { const VOXEL: f32 = 0.1; - fn open_patch_5x5() -> Vec { - let mut c = Vec::new(); - for ix in 0..5 { - for iy in 0..5 { - c.push((ix, iy, 0)); - } - } - c - } - fn open_patch(ix0: i32, iy0: i32, size: i32) -> Vec { let mut c = Vec::new(); for dx in 0..size { @@ -196,20 +184,6 @@ mod tests { c } - #[test] - fn empty_input() { - let sg = place_nodes(&[], VOXEL, 2, 1.0, 0.3); - assert!(sg.adj.is_empty()); - assert!(sg.nodes.is_empty()); - } - - #[test] - fn isolated_cell_places_no_node() { - // Single cell has 0 neighbors, is wall-adjacent, dist=0, below buffer. - let sg = place_nodes(&[(0, 0, 0)], VOXEL, 2, 1.0, 0.3); - assert!(sg.nodes.is_empty()); - } - #[test] fn open_patch_places_at_least_one_node() { // 10x10 at voxel=0.1 is 1m x 1m. Center is ~0.5m from any wall, well above buffer=0.3m. @@ -258,7 +232,6 @@ mod tests { #[test] fn wall_cells_scale_outbound_cost() { - // Strip of 10 cells. End cells have 1 same-z neighbor → wall-adjacent → dist=0 → penalty=2. let cells: Vec = (0..10).map(|ix| (ix, 0, 0)).collect(); let sg = place_nodes(&cells, VOXEL, 2, 1.0, 0.3); let outbound: Vec<_> = sg.adj.neighbors((0, 0, 0)).collect(); @@ -268,18 +241,4 @@ mod tests { assert!(edge.cost >= 1.5 * VOXEL - 1e-5); } } - - #[test] - fn dijkstra_distances_grow_from_seeds() { - let cells = open_patch_5x5(); - let lookup = build_surface_lookup(&cells); - let adj = build_surface_adjacency(&lookup, VOXEL, 2); - let seeds = wall_adjacent_cells(&adj); - let state = dijkstra(&adj, &seeds).state; - - let center = state[&(2, 2, 0)].dist; - let corner = state[&(0, 0, 0)].dist; - assert!(center > corner); - assert_eq!(corner, 0.0); - } } diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/plan.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/plan.rs index 0cc8d61687..7c4a093f1f 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/plan.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/plan.rs @@ -281,7 +281,7 @@ mod tests { #[test] fn snap_rejects_outside_z_tolerance() { let lookup = build_surface_lookup(&strip(20)); - // Surface at iz=0, pose at z=2.0, tolerance=1.5 → out of tolerance in every column. + // Surface at iz=0, pose at z=2.0, tolerance=1.5 is out of tolerance in every column. assert!(snap_pose_to_cell(&lookup, (0.5, 0.0, 2.0), VOXEL, 1.5).is_none()); } @@ -322,19 +322,6 @@ mod tests { assert_eq!(wp[wp.len() - 2], goal_cell_pos); } - #[test] - fn plan_produces_monotonic_xy_along_strip() { - let plg = graph_with_nodes(&strip(20), &[(3, 0, 0), (15, 0, 0)]); - let wp = plan(&plg, (0.2, 0.0, 0.05), (1.7, 0.0, 0.05), VOXEL, Z_TOL).unwrap(); - // First waypoint is start_pose, last is goal_pose. - assert_eq!(wp.first(), Some(&(0.2, 0.0, 0.05))); - assert_eq!(wp.last(), Some(&(1.7, 0.0, 0.05))); - // Interior waypoints monotonically increase in x. - for w in wp.windows(2).skip(1).take(wp.len() - 3) { - assert!(w[1].0 >= w[0].0 - 1e-5); - } - } - #[test] fn plan_three_nodes_visits_them_all() { let plg = graph_with_nodes(&strip(20), &[(3, 0, 0), (10, 0, 0), (17, 0, 0)]); diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs index 30f11e573e..b5d5299233 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs @@ -1,9 +1,9 @@ // Copyright 2026 Dimensional Inc. // SPDX-License-Identifier: Apache-2.0 -//! Surface extraction: from a voxelized obstacle set, mark cells with -//! robot-height clearance above as standable, then morphologically close -//! per-z-level holes without letting closing bridge across walls. +//! Surface extraction: from a voxel map, mark cells with robot-height +//! clearance above as standable, then morphologically close per-z-level +//! holes without letting closing bridge across walls. use ahash::{AHashMap, AHashSet}; use image::{GrayImage, Luma}; @@ -17,13 +17,12 @@ const OFF: Luma = Luma([0]); type ColumnIz = AHashMap<(i32, i32), Vec>; -/// A cell is standable if it has at least the robots height of clear +/// A cell is standable if it has at least the robot's height of clear /// space above it. fn is_standable(ix: i32, iy: i32, iz: i32, by_col: &ColumnIz, clearance_cells: i32) -> bool { let Some(zs) = by_col.get(&(ix, iy)) else { return true; }; - // first obstacle strictly above iz, if any let idx = zs.partition_point(|&z| z <= iz); match zs.get(idx) { Some(&next) => next - iz > clearance_cells, @@ -132,14 +131,11 @@ fn close_at_z( let x0 = min_x - pad; let y0 = min_y - pad; - // we treat the xy slice as a binary image, either surface (on) or not surface (off) let mut img = GrayImage::from_pixel(w, h, OFF); for &(ix, iy) in xys { img.put_pixel((ix - x0) as u32, (iy - y0) as u32, ON); } - // use L1 dilation/erosion, expand out in cross shape - // could use alternative methods here as well, subject to tuning if dilation_passes > 0 { img = dilate(&img, Norm::L1, dilation_passes as u8); } @@ -156,7 +152,6 @@ fn close_at_z( let ix = x0 + px as i32; let iy = y0 + py as i32; - // filter out if the surface has expanded to any non standable areas if !is_standable(ix, iy, iz, by_col, clearance_cells) { continue; } @@ -194,7 +189,7 @@ mod tests { #[test] fn gap_larger_than_headroom_makes_lower_cell_standable() { - // Obstacles at iz=0 and iz=10 with clearance_cells=5. Lower cell has gap=10 > 5. + // Voxel map points at iz=0 and iz=10 with clearance_cells=5. Lower cell has gap=10 > 5. let mut s = extract_surfaces(&voxel_map(&[(0, 0, 0), (0, 0, 10)]), 5, 0, 0); s.sort(); assert_eq!(s, vec![(0, 0, 0), (0, 0, 10)]); @@ -202,7 +197,7 @@ mod tests { #[test] fn morphological_closing_fills_center_hole() { - // Ring of 8 cells around (0,0) at iz=0, no obstacles above. + // Ring of 8 cells around (0,0) at iz=0, nothing above. let cells: Vec = [ (-1, -1), (-1, 0), @@ -224,9 +219,9 @@ mod tests { } #[test] - fn closing_does_not_bridge_obstacle_in_headroom() { - // Ring of 8 cells at iz=0 + an obstacle directly above the hole at (0,0,1). - // The hole at (0,0,0) is vetoed because its headroom column has an obstacle. + fn closing_does_not_bridge_voxel_in_headroom() { + // Ring of 8 cells at iz=0 plus a voxel directly above the hole at (0,0,1). + // The hole at (0,0,0) is vetoed because its headroom column is occupied. let mut cells: Vec = [ (-1, -1), (-1, 0), diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/voxel.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/voxel.rs index 69eb9e32b0..2fca61c048 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/voxel.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/voxel.rs @@ -24,37 +24,3 @@ pub fn surface_point_xyz(ix: i32, iy: i32, iz: i32, voxel_size: f32) -> (f32, f3 (iz as f32 + 1.0) * voxel_size, ) } - -#[cfg(test)] -mod tests { - use super::*; - - fn approx_eq(a: f32, b: f32) { - let eps = 1e-6; - assert!((a - b).abs() < eps, "{a} != {b} (eps {eps})"); - } - - #[test] - fn origin_cell_at_voxel_1() { - let (x, y, z) = surface_point_xyz(0, 0, 0, 1.0); - approx_eq(x, 0.5); - approx_eq(y, 0.5); - approx_eq(z, 1.0); - } - - #[test] - fn positive_cell_at_voxel_0_1() { - let (x, y, z) = surface_point_xyz(3, 2, 5, 0.1); - approx_eq(x, 0.35); - approx_eq(y, 0.25); - approx_eq(z, 0.6); - } - - #[test] - fn negative_cell() { - let (x, y, z) = surface_point_xyz(-2, -1, -3, 1.0); - approx_eq(x, -1.5); - approx_eq(y, -0.5); - approx_eq(z, -2.0); - } -} From 0a7e3bf481aed81de33cda0e6718d9740670bb92 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Fri, 29 May 2026 16:38:16 -0700 Subject: [PATCH 47/55] Rename file --- .../navigation/nav_stack/modules/mls_planner/rust/src/main.rs | 4 ++-- .../modules/mls_planner/rust/src/{plan.rs => planner.rs} | 0 2 files changed, 2 insertions(+), 2 deletions(-) rename dimos/navigation/nav_stack/modules/mls_planner/rust/src/{plan.rs => planner.rs} (100%) diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs index 95ba00aaed..9627d42449 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs @@ -5,7 +5,7 @@ mod adjacency; mod dijkstra; mod edges; mod nodes; -mod plan; +mod planner; mod surfaces; mod voxel; @@ -191,7 +191,7 @@ impl MlsPlanner { let goal = (p.x as f32, p.y as f32, p.z as f32); let t_plan = Instant::now(); - let waypoints = match plan::plan( + let waypoints = match planner::plan( plg, start, goal, diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/plan.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/planner.rs similarity index 100% rename from dimos/navigation/nav_stack/modules/mls_planner/rust/src/plan.rs rename to dimos/navigation/nav_stack/modules/mls_planner/rust/src/planner.rs From 8deb4d1cc12dd3c2e0070dd6a9715ecf94e706f0 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Fri, 29 May 2026 16:43:58 -0700 Subject: [PATCH 48/55] Remove old gitignores for other rust crates --- dimos/mapping/ray_tracing/rust/.gitignore | 1 - examples/native-modules/rust/.gitignore | 1 - native/rust/.gitignore | 2 -- 3 files changed, 4 deletions(-) delete mode 100644 dimos/mapping/ray_tracing/rust/.gitignore delete mode 100644 examples/native-modules/rust/.gitignore delete mode 100644 native/rust/.gitignore diff --git a/dimos/mapping/ray_tracing/rust/.gitignore b/dimos/mapping/ray_tracing/rust/.gitignore deleted file mode 100644 index 2f7896d1d1..0000000000 --- a/dimos/mapping/ray_tracing/rust/.gitignore +++ /dev/null @@ -1 +0,0 @@ -target/ diff --git a/examples/native-modules/rust/.gitignore b/examples/native-modules/rust/.gitignore deleted file mode 100644 index 2f7896d1d1..0000000000 --- a/examples/native-modules/rust/.gitignore +++ /dev/null @@ -1 +0,0 @@ -target/ diff --git a/native/rust/.gitignore b/native/rust/.gitignore deleted file mode 100644 index eccd7b4ab8..0000000000 --- a/native/rust/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -/target/ -**/*.rs.bk From db7f3b0e09997201b4500a4fec9d1f28d4b11a68 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Fri, 29 May 2026 18:05:26 -0700 Subject: [PATCH 49/55] Rayon --- .../modules/mls_planner/rust/Cargo.lock | 46 ++++++++++++++ .../modules/mls_planner/rust/Cargo.toml | 1 + .../modules/mls_planner/rust/src/edges.rs | 27 ++++---- .../modules/mls_planner/rust/src/nodes.rs | 36 ++++++----- .../modules/mls_planner/rust/src/surfaces.rs | 62 +++++++++++-------- 5 files changed, 121 insertions(+), 51 deletions(-) diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/Cargo.lock b/dimos/navigation/nav_stack/modules/mls_planner/rust/Cargo.lock index 77f0f06e98..bb89daed36 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/Cargo.lock +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/Cargo.lock @@ -91,6 +91,31 @@ version = "1.0.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "9330f8b2ff13f34540b44e946ef35111825727b38d33286ef986142615121801" +[[package]] +name = "crossbeam-deque" +version = "0.8.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9dd111b7b7f7d55b72c0a6ae361660ee5853c9af73f70c3c2ef6858b950e2e51" +dependencies = [ + "crossbeam-epoch", + "crossbeam-utils", +] + +[[package]] +name = "crossbeam-epoch" +version = "0.9.18" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5b82ac4a3c2ca9c3460964f020e1402edd5753411d7737aa39c3714ad1b5420e" +dependencies = [ + "crossbeam-utils", +] + +[[package]] +name = "crossbeam-utils" +version = "0.8.21" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d0a5c400df2834b80a4c3327b3aad3a4c4cd4de0629063962b03235697506a28" + [[package]] name = "dimos-lcm" version = "0.1.0" @@ -110,6 +135,7 @@ dependencies = [ "image", "imageproc", "lcm-msgs", + "rayon", "serde", "tokio", "tracing", @@ -546,6 +572,26 @@ version = "0.2.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "60a357793950651c4ed0f3f52338f53b2f809f32d83a07f72909fa13e4c6c1e3" +[[package]] +name = "rayon" +version = "1.12.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fb39b166781f92d482534ef4b4b1b2568f42613b53e5b6c160e24cfbfa30926d" +dependencies = [ + "either", + "rayon-core", +] + +[[package]] +name = "rayon-core" +version = "1.13.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "22e18b0f0062d30d4230b2e85ff77fdfe4326feb054b9783a3460d8435c8ab91" +dependencies = [ + "crossbeam-deque", + "crossbeam-utils", +] + [[package]] name = "regex-automata" version = "0.4.14" diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/Cargo.toml b/dimos/navigation/nav_stack/modules/mls_planner/rust/Cargo.toml index a64e56cb40..71744de8fd 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/Cargo.toml +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/Cargo.toml @@ -18,6 +18,7 @@ ahash = "0.8" tracing = "0.1" image = { version = "0.25", default-features = false } imageproc = { version = "0.25", default-features = false } +rayon = "1" [profile.release] lto = "thin" diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/edges.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/edges.rs index 221d8b1ded..7c039bd975 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/edges.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/edges.rs @@ -9,6 +9,7 @@ //! edges between start nodes. use ahash::AHashMap; +use rayon::prelude::*; use crate::adjacency::{SurfaceAdjacency, SurfaceLookup}; use crate::dijkstra::{dijkstra, CellState}; @@ -73,18 +74,20 @@ pub fn add_node_edges(sg: SurfaceGraph) -> PlannerGraph { /// Walk every node-graph edge and emit one segment per consecutive cell pair /// along the reconstructed cell path. pub fn edges_to_segments(plg: &PlannerGraph, _voxel_size: f32) -> Vec<(VoxelKey, VoxelKey, f32)> { - let mut segments = Vec::new(); - for edge in &plg.node_edges { - let mut from_a = walk_preds_to_source(plg, edge.boundary_u); - from_a.reverse(); - let to_b = walk_preds_to_source(plg, edge.boundary_v); - let mut path: Vec = from_a; - path.extend(to_b); - for pair in path.windows(2) { - segments.push((pair[0], pair[1], edge.cost)); - } - } - segments + plg.node_edges + .par_iter() + .flat_map_iter(|edge| { + let mut from_a = walk_preds_to_source(plg, edge.boundary_u); + from_a.reverse(); + let to_b = walk_preds_to_source(plg, edge.boundary_v); + let mut path: Vec = from_a; + path.extend(to_b); + let cost = edge.cost; + path.windows(2) + .map(|pair| (pair[0], pair[1], cost)) + .collect::>() + }) + .collect() } pub fn walk_preds_to_source(plg: &PlannerGraph, start_cell: VoxelKey) -> Vec { diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs index ddce9fc12f..633a64668c 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs @@ -6,9 +6,10 @@ //! toward corridor centers. use ahash::{AHashMap, AHashSet}; +use rayon::prelude::*; use crate::adjacency::{ - build_surface_adjacency, build_surface_lookup, SurfaceAdjacency, SurfaceLookup, + build_surface_adjacency, build_surface_lookup, Edge, SurfaceAdjacency, SurfaceLookup, }; use crate::dijkstra::dijkstra; use crate::voxel::{surface_point_xyz, VoxelKey}; @@ -78,18 +79,22 @@ pub fn place_nodes( /// Cells missing any of their 4 xy-direction neighbors are treated as boundaries. fn wall_adjacent_cells(adj: &SurfaceAdjacency) -> Vec { - let mut wall: Vec = adj - .iter() - .filter(|(c, edges)| { + let entries: Vec<(VoxelKey, &[Edge])> = adj.iter().collect(); + let mut wall: Vec = entries + .par_iter() + .filter_map(|(c, edges)| { let mut dirs: AHashSet<(i32, i32)> = AHashSet::new(); for e in *edges { dirs.insert((e.dst.0 - c.0, e.dst.1 - c.1)); } - dirs.len() < 4 + if dirs.len() < 4 { + Some(*c) + } else { + None + } }) - .map(|(c, _)| c) .collect(); - wall.sort(); + wall.par_sort_unstable(); if wall.is_empty() { if let Some(c) = adj.cells().min() { wall.push(c); @@ -148,9 +153,10 @@ fn apply_wall_safe_penalty( dist: &AHashMap, buffer_m: f32, ) { - let penalty: AHashMap = adj - .cells() - .map(|c| { + let cells: Vec = adj.cells().collect(); + let penalty_pairs: Vec<(VoxelKey, f32)> = cells + .par_iter() + .map(|&c| { let p = match dist.get(&c) { Some(&d) => (1.0 + (buffer_m - d) / buffer_m).max(1.0), None => 1.0, @@ -158,14 +164,16 @@ fn apply_wall_safe_penalty( (c, p) }) .collect(); + let penalty: AHashMap = penalty_pairs.into_iter().collect(); - for (src, edges) in adj.iter_edges_mut() { - let pu = penalty.get(&src).copied().unwrap_or(1.0); - for edge in edges { + let mut edge_lists: Vec<(VoxelKey, &mut Vec)> = adj.iter_edges_mut().collect(); + edge_lists.par_iter_mut().for_each(|(src, edges)| { + let pu = penalty.get(src).copied().unwrap_or(1.0); + for edge in edges.iter_mut() { let pv = penalty.get(&edge.dst).copied().unwrap_or(1.0); edge.cost *= (pu + pv) / 2.0; } - } + }); } #[cfg(test)] diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs index b5d5299233..c5499ee6ce 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs @@ -9,6 +9,7 @@ use ahash::{AHashMap, AHashSet}; use image::{GrayImage, Luma}; use imageproc::distance_transform::Norm; use imageproc::morphology::{dilate, erode}; +use rayon::prelude::*; use crate::voxel::VoxelKey; @@ -48,20 +49,29 @@ pub fn extract_surfaces( by_col.entry((ix, iy)).or_default().push(iz); } - let mut standable: Vec = Vec::new(); - for ((ix, iy), zs) in by_col.iter_mut() { - zs.sort_unstable(); - - // find gaps of at least clearance height through the column - for w in zs.windows(2) { - if w[1] - w[0] > clearance_cells { - standable.push((*ix, *iy, w[0])); + let mut entries: Vec<((i32, i32), &mut Vec)> = + by_col.iter_mut().map(|(&k, v)| (k, v)).collect(); + entries + .par_iter_mut() + .for_each(|(_, zs)| zs.sort_unstable()); + + let standable: Vec = entries + .par_iter() + .flat_map_iter(|((ix, iy), zs)| { + let mut out: Vec = Vec::new(); + // find gaps of at least clearance height through the column + for w in zs.windows(2) { + if w[1] - w[0] > clearance_cells { + out.push((*ix, *iy, w[0])); + } } - } - if let Some(&last_iz) = zs.last() { - standable.push((*ix, *iy, last_iz)); - } - } + if let Some(&last_iz) = zs.last() { + out.push((*ix, *iy, last_iz)); + } + out + }) + .collect(); + drop(entries); close_surface_holes( standable, @@ -91,18 +101,20 @@ fn close_surface_holes( by_z.entry(iz).or_default().push((ix, iy)); } - let mut out = Vec::new(); - for (iz, xys) in by_z { - out.extend(close_at_z( - &xys, - iz, - by_col, - dilation_passes, - erosion_passes, - clearance_cells, - )); - } - out + let slices: Vec<(i32, Vec<(i32, i32)>)> = by_z.into_iter().collect(); + slices + .par_iter() + .flat_map_iter(|(iz, xys)| { + close_at_z( + xys, + *iz, + by_col, + dilation_passes, + erosion_passes, + clearance_cells, + ) + }) + .collect() } /// Close holes on an xy slice of the surfaces. From 734382e951b41126a90933a26da7bc9be6c7991c Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Mon, 1 Jun 2026 09:54:35 -0700 Subject: [PATCH 50/55] Fix typing --- dimos/navigation/nav_stack/evaluator/blueprints.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/dimos/navigation/nav_stack/evaluator/blueprints.py b/dimos/navigation/nav_stack/evaluator/blueprints.py index 5c3a1a4cab..fc750958eb 100644 --- a/dimos/navigation/nav_stack/evaluator/blueprints.py +++ b/dimos/navigation/nav_stack/evaluator/blueprints.py @@ -22,11 +22,12 @@ from __future__ import annotations -from typing import Any +from typing import TYPE_CHECKING, Any import numpy as np from dimos.core.coordination.blueprints import autoconnect +from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.navigation.nav_stack.evaluator.evaluator import Evaluator from dimos.navigation.nav_stack.modules.click_start_goal_router.click_start_goal_router import ( ClickStartGoalRouter, @@ -37,6 +38,9 @@ from dimos.visualization.rerun.bridge import RerunBridgeModule from dimos.visualization.rerun.websocket_server import RerunWebSocketServer +if TYPE_CHECKING: + from rerun._baseclasses import Archetype + _POSE_MARKER_RADIUS = 0.4 # Small lift so graph artifacts render visibly above the surface points instead of z-fighting. _GRAPH_Z_LIFT = 0.05 @@ -52,7 +56,7 @@ def _render_start_pose(msg: Any) -> Any: ) -def _render_goal_pose(msg: Any) -> Any: +def _render_goal_pose(msg: Odometry) -> Archetype: import rerun as rr return rr.Points3D( From 0f3149fcc15cc958f9e2e49ceef4bacbfa1e140d Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Fri, 29 May 2026 18:57:43 -0700 Subject: [PATCH 51/55] Switch to vectors and ids --- .../modules/mls_planner/rust/src/adjacency.rs | 302 ++++++++++++++---- .../modules/mls_planner/rust/src/dijkstra.rs | 245 +++++++------- .../modules/mls_planner/rust/src/edges.rs | 292 ++++++++--------- .../modules/mls_planner/rust/src/main.rs | 102 ++++-- .../modules/mls_planner/rust/src/nodes.rs | 269 ++++++++-------- .../modules/mls_planner/rust/src/planner.rs | 87 ++--- .../modules/mls_planner/rust/src/surfaces.rs | 83 ++--- 7 files changed, 798 insertions(+), 582 deletions(-) diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/adjacency.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/adjacency.rs index 6e07697a1d..395c268cda 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/adjacency.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/adjacency.rs @@ -1,93 +1,201 @@ // Copyright 2026 Dimensional Inc. // SPDX-License-Identifier: Apache-2.0 +//! Surface cells indexed by dense CellId. Hot-path Dijkstra / NMS / edge work +//! operates on CellId; VoxelKey is only translated at the boundary with the +//! voxel map and at output (publishing, waypoint emission). +//! +//! The structure is a slot map. Inserting allocates a fresh id (or recycles +//! a freed one). Deleting tombstones the slot and walks the cell's neighbors +//! to drop back-edges, leaving every surviving cell's CellId unchanged. + use ahash::AHashMap; +use rayon::prelude::*; use crate::voxel::VoxelKey; pub type SurfaceLookup = AHashMap<(i32, i32), Vec>; +pub type CellId = u32; +pub const NO_CELL: CellId = u32::MAX; + +const TOMBSTONE: VoxelKey = (i32::MIN, i32::MIN, i32::MIN); const NEIGHBORS_4: [(i32, i32); 4] = [(-1, 0), (1, 0), (0, -1), (0, 1)]; #[derive(Clone, Copy, Debug)] pub struct Edge { - pub dst: VoxelKey, + pub dst: CellId, pub cost: f32, } #[derive(Default)] -pub struct SurfaceAdjacency { - cells: AHashMap>, +pub struct SurfaceCells { + coord: Vec, + edges: Vec>, + by_coord: AHashMap, + free: Vec, } -impl SurfaceAdjacency { - pub fn new() -> Self { - Self::default() +impl SurfaceCells { + pub fn is_empty(&self) -> bool { + self.by_coord.is_empty() } - pub fn add_cell(&mut self, cell: VoxelKey) { - self.cells.entry(cell).or_default(); + /// Total slot count, including tombstoned ones. Use as the size for + /// CellId-indexed scratch buffers. + pub fn slot_capacity(&self) -> usize { + self.coord.len() } - pub fn add_edge(&mut self, src: VoxelKey, dst: VoxelKey, cost: f32) { - self.cells.entry(src).or_default().push(Edge { dst, cost }); + /// Drop all identity and edges while keeping allocation capacity. + pub fn clear(&mut self) { + self.coord.clear(); + self.by_coord.clear(); + self.free.clear(); + for e in self.edges.iter_mut() { + e.clear(); + } } - pub fn neighbors(&self, cell: VoxelKey) -> impl Iterator + '_ { - self.cells.get(&cell).into_iter().flatten().copied() + #[inline] + pub fn is_live(&self, id: CellId) -> bool { + self.coord[id as usize] != TOMBSTONE } - pub fn cells(&self) -> impl Iterator + '_ { - self.cells.keys().copied() + /// Get-or-insert: O(1) amortized. Returns the CellId for `k`. + pub fn alloc(&mut self, k: VoxelKey) -> CellId { + debug_assert_ne!(k, TOMBSTONE, "voxel coord collides with tombstone sentinel"); + if let Some(&id) = self.by_coord.get(&k) { + return id; + } + let id = if let Some(free_id) = self.free.pop() { + self.coord[free_id as usize] = k; + free_id + } else { + let id = self.coord.len() as CellId; + self.coord.push(k); + self.edges.push(Vec::new()); + id + }; + self.by_coord.insert(k, id); + id } - /// Per-cell iteration that yields the cell and a borrowed view of its - /// edges in a single hashmap probe. - pub fn iter(&self) -> impl Iterator + '_ { - self.cells.iter().map(|(&k, v)| (k, v.as_slice())) + /// Remove cell `k`, dropping all edges that reference it. CellIds of + /// other live cells are preserved. Required for incremental updates. + #[allow(dead_code)] + pub fn remove(&mut self, k: VoxelKey) -> Option { + let id = self.by_coord.remove(&k)?; + let outbound = std::mem::take(&mut self.edges[id as usize]); + for e in &outbound { + let neigh = &mut self.edges[e.dst as usize]; + neigh.retain(|x| x.dst != id); + } + self.coord[id as usize] = TOMBSTONE; + self.free.push(id); + Some(id) } - /// Mutable per-cell edge iterator. - pub fn iter_edges_mut(&mut self) -> impl Iterator)> + '_ { - self.cells.iter_mut().map(|(&k, v)| (k, v)) + #[inline] + pub fn id(&self, k: VoxelKey) -> Option { + self.by_coord.get(&k).copied() } - pub fn contains(&self, cell: VoxelKey) -> bool { - self.cells.contains_key(&cell) + #[inline] + pub fn coord(&self, id: CellId) -> VoxelKey { + self.coord[id as usize] } - pub fn is_empty(&self) -> bool { - self.cells.is_empty() + #[inline] + pub fn neighbors(&self, id: CellId) -> &[Edge] { + &self.edges[id as usize] + } + + #[allow(dead_code)] + pub fn add_edge(&mut self, src: CellId, dst: CellId, cost: f32) { + self.edges[src as usize].push(Edge { dst, cost }); + } + + /// Iterate live cells: (id, outgoing edges). + pub fn iter(&self) -> impl Iterator + '_ { + self.coord.iter().enumerate().filter_map(move |(i, c)| { + if *c != TOMBSTONE { + Some((i as CellId, self.edges[i].as_slice())) + } else { + None + } + }) + } + + /// Mutable per-cell edge iterator over live cells. + pub fn iter_edges_mut(&mut self) -> impl Iterator)> + '_ { + self.coord + .iter() + .zip(self.edges.iter_mut()) + .enumerate() + .filter_map(|(i, (c, e))| { + if *c != TOMBSTONE { + Some((i as CellId, e)) + } else { + None + } + }) + } + + pub fn ids(&self) -> impl Iterator + '_ { + self.coord.iter().enumerate().filter_map(|(i, c)| { + if *c != TOMBSTONE { + Some(i as CellId) + } else { + None + } + }) } } /// Group cells by XY column with sorted unique iz per column. -pub fn build_surface_lookup(cells: &[VoxelKey]) -> SurfaceLookup { - let mut lookup: SurfaceLookup = AHashMap::new(); +pub fn build_surface_lookup(cells: &[VoxelKey], out: &mut SurfaceLookup) { + out.clear(); for &(ix, iy, iz) in cells { - lookup.entry((ix, iy)).or_default().push(iz); + out.entry((ix, iy)).or_default().push(iz); } - for zs in lookup.values_mut() { + for zs in out.values_mut() { zs.sort_unstable(); zs.dedup(); } - lookup } -/// 4 way L1 adjacency, limited by step height threshold. -pub fn build_surface_adjacency( +/// Populate `cells` with surface adjacency from the lookup. Existing +/// contents are dropped. CellIds are assigned in a deterministic column +/// order so debug logs and tests reproduce across runs. The edge pass is +/// parallel: each cell's outbound edges are computed independently by +/// reading the immutable coord array and by_coord map. +pub fn build_surface_cells( + cells: &mut SurfaceCells, surface_lookup: &SurfaceLookup, voxel_size: f32, step_threshold_cells: i32, -) -> SurfaceAdjacency { - let mut adj = SurfaceAdjacency::new(); - for (&(ix, iy), zs) in surface_lookup { - for &iz in zs { - adj.add_cell((ix, iy, iz)); +) { + cells.clear(); + + let mut keys: Vec<(i32, i32)> = surface_lookup.keys().copied().collect(); + keys.sort_unstable(); + for &(ix, iy) in &keys { + for &iz in &surface_lookup[&(ix, iy)] { + cells.alloc((ix, iy, iz)); } } - for (&(ix, iy), zs) in surface_lookup { - for &iz in zs { + + let n = cells.coord.len(); + cells.edges.resize_with(n, Vec::new); + let coord: &[VoxelKey] = &cells.coord; + let by_coord: &AHashMap = &cells.by_coord; + cells + .edges + .par_iter_mut() + .enumerate() + .for_each(|(src_id, local)| { + let (ix, iy, iz) = coord[src_id]; for (dx, dy) in NEIGHBORS_4 { let Some(nzs) = surface_lookup.get(&(ix + dx, iy + dy)) else { continue; @@ -97,13 +205,14 @@ pub fn build_surface_adjacency( if dz.abs() > step_threshold_cells { continue; } + let dst = *by_coord + .get(&(ix + dx, iy + dy, nz)) + .expect("neighbor cell exists in lookup"); let cost = ((dx * dx + dy * dy + dz * dz) as f32).sqrt() * voxel_size; - adj.add_edge((ix, iy, iz), (ix + dx, iy + dy, nz), cost); + local.push(Edge { dst, cost }); } } - } - } - adj + }); } #[cfg(test)] @@ -117,58 +226,111 @@ mod tests { assert!((a - b).abs() < eps, "{a} != {b} (eps {eps})"); } - fn neighbors_of(adj: &SurfaceAdjacency, cell: VoxelKey) -> Vec { - adj.neighbors(cell).collect() + fn build(cells: &[VoxelKey]) -> (SurfaceLookup, SurfaceCells) { + let mut lookup = SurfaceLookup::new(); + build_surface_lookup(cells, &mut lookup); + let mut sc = SurfaceCells::default(); + build_surface_cells(&mut sc, &lookup, VOXEL, 2); + (lookup, sc) + } + + fn neighbors_of(sc: &SurfaceCells, k: VoxelKey) -> Vec<(VoxelKey, f32)> { + let id = sc.id(k).expect("cell should exist"); + sc.neighbors(id) + .iter() + .map(|e| (sc.coord(e.dst), e.cost)) + .collect() } #[test] fn same_z_neighbors_are_bidirectional() { - let lookup = build_surface_lookup(&[(0, 0, 0), (1, 0, 0)]); - let adj = build_surface_adjacency(&lookup, VOXEL, 2); - let a = neighbors_of(&adj, (0, 0, 0)); - let b = neighbors_of(&adj, (1, 0, 0)); + let (_, sc) = build(&[(0, 0, 0), (1, 0, 0)]); + let a = neighbors_of(&sc, (0, 0, 0)); + let b = neighbors_of(&sc, (1, 0, 0)); assert_eq!(a.len(), 1); assert_eq!(b.len(), 1); - assert_eq!(a[0].dst, (1, 0, 0)); - assert_eq!(b[0].dst, (0, 0, 0)); - approx_eq(a[0].cost, VOXEL); - approx_eq(b[0].cost, VOXEL); + assert_eq!(a[0].0, (1, 0, 0)); + assert_eq!(b[0].0, (0, 0, 0)); + approx_eq(a[0].1, VOXEL); + approx_eq(b[0].1, VOXEL); } #[test] fn diagonal_not_connected_under_4_connectivity() { - let lookup = build_surface_lookup(&[(0, 0, 0), (1, 1, 0)]); - let adj = build_surface_adjacency(&lookup, VOXEL, 2); - assert!(neighbors_of(&adj, (0, 0, 0)).is_empty()); - assert!(neighbors_of(&adj, (1, 1, 0)).is_empty()); + let (_, sc) = build(&[(0, 0, 0), (1, 1, 0)]); + assert!(neighbors_of(&sc, (0, 0, 0)).is_empty()); + assert!(neighbors_of(&sc, (1, 1, 0)).is_empty()); } #[test] fn step_threshold_blocks_large_dz() { - let lookup = build_surface_lookup(&[(0, 0, 0), (1, 0, 5)]); - let adj = build_surface_adjacency(&lookup, VOXEL, 2); - assert!(neighbors_of(&adj, (0, 0, 0)).is_empty()); - assert!(neighbors_of(&adj, (1, 0, 5)).is_empty()); + let (_, sc) = build(&[(0, 0, 0), (1, 0, 5)]); + assert!(neighbors_of(&sc, (0, 0, 0)).is_empty()); + assert!(neighbors_of(&sc, (1, 0, 5)).is_empty()); } #[test] fn step_within_threshold_uses_3d_distance() { - let lookup = build_surface_lookup(&[(0, 0, 0), (1, 0, 1)]); - let adj = build_surface_adjacency(&lookup, VOXEL, 2); + let (_, sc) = build(&[(0, 0, 0), (1, 0, 1)]); let expected = (2.0_f32).sqrt() * VOXEL; - let a = neighbors_of(&adj, (0, 0, 0)); - let b = neighbors_of(&adj, (1, 0, 1)); + let a = neighbors_of(&sc, (0, 0, 0)); + let b = neighbors_of(&sc, (1, 0, 1)); assert_eq!(a.len(), 1); assert_eq!(b.len(), 1); - approx_eq(a[0].cost, expected); - approx_eq(b[0].cost, expected); + approx_eq(a[0].1, expected); + approx_eq(b[0].1, expected); } #[test] fn plus_pattern_center_has_four_neighbors() { let cells = vec![(0, 0, 0), (1, 0, 0), (-1, 0, 0), (0, 1, 0), (0, -1, 0)]; - let lookup = build_surface_lookup(&cells); - let adj = build_surface_adjacency(&lookup, VOXEL, 2); - assert_eq!(neighbors_of(&adj, (0, 0, 0)).len(), 4); + let (_, sc) = build(&cells); + assert_eq!(neighbors_of(&sc, (0, 0, 0)).len(), 4); + } + + #[test] + fn clear_keeps_edge_vec_capacity() { + let (_, mut sc) = build(&[(0, 0, 0), (1, 0, 0), (2, 0, 0)]); + let edge_vec_count = sc.edges.len(); + sc.clear(); + assert!(sc.is_empty()); + assert_eq!(sc.edges.len(), edge_vec_count); + } + + #[test] + fn remove_keeps_neighbor_cell_ids_stable() { + let (_, mut sc) = build(&[(0, 0, 0), (1, 0, 0), (2, 0, 0)]); + let id0 = sc.id((0, 0, 0)).unwrap(); + let id2 = sc.id((2, 0, 0)).unwrap(); + sc.remove((1, 0, 0)); + assert_eq!(sc.id((0, 0, 0)), Some(id0)); + assert_eq!(sc.id((2, 0, 0)), Some(id2)); + assert_eq!(sc.id((1, 0, 0)), None); + assert!( + sc.neighbors(id0).is_empty(), + "back-edge from 0 to 1 must be dropped" + ); + assert!( + sc.neighbors(id2).is_empty(), + "back-edge from 2 to 1 must be dropped" + ); + } + + #[test] + fn alloc_after_remove_reuses_freed_slot() { + let (_, mut sc) = build(&[(0, 0, 0), (1, 0, 0)]); + let removed_id = sc.remove((1, 0, 0)).unwrap(); + let new_id = sc.alloc((5, 5, 0)); + assert_eq!(new_id, removed_id); + assert_eq!(sc.coord(new_id), (5, 5, 0)); + assert!(sc.is_live(new_id)); + } + + #[test] + fn live_iter_skips_tombstones() { + let (_, mut sc) = build(&[(0, 0, 0), (1, 0, 0), (2, 0, 0)]); + sc.remove((1, 0, 0)); + let live: Vec = sc.ids().map(|id| sc.coord(id)).collect(); + assert_eq!(live, vec![(0, 0, 0), (2, 0, 0)]); } } diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/dijkstra.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/dijkstra.rs index 15e13c96c7..28a11bccdd 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/dijkstra.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/dijkstra.rs @@ -1,90 +1,85 @@ // Copyright 2026 Dimensional Inc. // SPDX-License-Identifier: Apache-2.0 -//! Multi-source Dijkstra over the cell-keyed surface adjacency. +//! Multi-source Dijkstra over the CellId-indexed surface graph. State and +//! the heap live in a reusable struct so the inner loop never allocates. use std::cmp::Ordering; use std::collections::BinaryHeap; -use ahash::AHashMap; +use crate::adjacency::{CellId, SurfaceCells, NO_CELL}; -use crate::adjacency::SurfaceAdjacency; -use crate::voxel::VoxelKey; - -#[derive(Clone, Copy, Debug)] -pub struct CellState { - pub dist: f32, - /// Predecessor node along the shortest path to source. - pub pred: Option, - /// Id of cheapest source to return to. - pub source: u32, -} - -pub struct DijkstraResult { - pub state: AHashMap, +#[derive(Default)] +pub struct DijkstraState { + pub dist: Vec, + pub pred: Vec, + pub source: Vec, + heap: BinaryHeap, } -impl DijkstraResult { - pub fn dist_map(&self) -> AHashMap { - self.state.iter().map(|(&c, s)| (c, s.dist)).collect() +impl DijkstraState { + /// Resize parallel vecs to `n` slots and reset their contents to the + /// "unreached" sentinels. Capacity is preserved across calls. + pub fn reset(&mut self, n: usize) { + self.dist.clear(); + self.dist.resize(n, f32::INFINITY); + self.pred.clear(); + self.pred.resize(n, NO_CELL); + self.source.clear(); + self.source.resize(n, 0); + self.heap.clear(); } } -pub fn dijkstra(adj: &SurfaceAdjacency, sources: &[VoxelKey]) -> DijkstraResult { - let mut state: AHashMap = AHashMap::new(); - let mut heap: BinaryHeap = BinaryHeap::new(); +pub fn dijkstra(cells: &SurfaceCells, sources: &[CellId], state: &mut DijkstraState) { + state.reset(cells.slot_capacity()); for (label, &s) in sources.iter().enumerate() { - if !adj.contains(s) { + if !cells.is_live(s) { continue; } - state.insert( - s, - CellState { - dist: 0.0, - pred: None, - source: label as u32, - }, - ); - heap.push(Scored(0.0, s)); + state.dist[s as usize] = 0.0; + state.source[s as usize] = label as u32; + state.heap.push(Scored(0.0, s)); } - while let Some(Scored(d, u)) = heap.pop() { - let Some(&CellState { - dist: cur, - source: su, - .. - }) = state.get(&u) - else { - continue; - }; + while let Some(Scored(d, u)) = state.heap.pop() { + let cur = state.dist[u as usize]; if d > cur { continue; } - for edge in adj.neighbors(u) { + let su = state.source[u as usize]; + for edge in cells.neighbors(u) { let nd = d + edge.cost; - let should_update = match state.get(&edge.dst) { - None => true, - Some(s) => nd < s.dist, - }; - if should_update { - state.insert( - edge.dst, - CellState { - dist: nd, - pred: Some(u), - source: su, - }, - ); - heap.push(Scored(nd, edge.dst)); + let v = edge.dst as usize; + if nd < state.dist[v] { + state.dist[v] = nd; + state.pred[v] = u; + state.source[v] = su; + state.heap.push(Scored(nd, edge.dst)); } } } +} - DijkstraResult { state } +/// Walk predecessors from `start` back to its source, returning the chain +/// inclusive of both ends. Returns just [start] for source cells and for +/// unreached cells. +pub fn walk_preds(state: &DijkstraState, start: CellId) -> Vec { + let mut cells = vec![start]; + let mut cur = start; + loop { + let p = state.pred[cur as usize]; + if p == NO_CELL { + break; + } + cur = p; + cells.push(cur); + } + cells } -struct Scored(f32, VoxelKey); +struct Scored(f32, CellId); impl PartialEq for Scored { fn eq(&self, other: &Self) -> bool { @@ -99,7 +94,7 @@ impl PartialOrd for Scored { } impl Ord for Scored { fn cmp(&self, other: &Self) -> Ordering { - // Min-heap on f32 score. + // Min-heap on f32 score with CellId tie-break for determinism. other.0.total_cmp(&self.0).then(self.1.cmp(&other.1)) } } @@ -107,92 +102,102 @@ impl Ord for Scored { #[cfg(test)] mod tests { use super::*; + use crate::adjacency::SurfaceCells; - fn chain(n: i32) -> SurfaceAdjacency { - let mut adj = SurfaceAdjacency::new(); - for i in 0..n { - adj.add_cell((i, 0, 0)); - } + fn chain(n: i32) -> (SurfaceCells, Vec) { + let mut sc = SurfaceCells::default(); + let ids: Vec = (0..n).map(|i| sc.alloc((i, 0, 0))).collect(); for i in 0..n - 1 { - adj.add_edge((i, 0, 0), (i + 1, 0, 0), 1.0); - adj.add_edge((i + 1, 0, 0), (i, 0, 0), 1.0); + sc.add_edge(ids[i as usize], ids[(i + 1) as usize], 1.0); + sc.add_edge(ids[(i + 1) as usize], ids[i as usize], 1.0); } - adj + (sc, ids) } #[test] fn single_source_dist_and_pred() { - let adj = chain(5); - let r = dijkstra(&adj, &[(0, 0, 0)]); - for i in 0..5 { - let s = r.state[&(i, 0, 0)]; - assert_eq!(s.dist, i as f32); - assert_eq!(s.source, 0); + let (sc, ids) = chain(5); + let mut st = DijkstraState::default(); + dijkstra(&sc, &[ids[0]], &mut st); + for (i, &id) in ids.iter().enumerate().take(5) { + assert_eq!(st.dist[id as usize], i as f32); + assert_eq!(st.source[id as usize], 0); } - assert!(r.state[&(0, 0, 0)].pred.is_none()); - let mut cur = (4, 0, 0); + assert_eq!(st.pred[ids[0] as usize], NO_CELL); + let mut cur = ids[4]; let mut hops = 0; - while let Some(p) = r.state[&cur].pred { - cur = p; + while st.pred[cur as usize] != NO_CELL { + cur = st.pred[cur as usize]; hops += 1; } - assert_eq!(cur, (0, 0, 0)); + assert_eq!(cur, ids[0]); assert_eq!(hops, 4); } #[test] fn multi_source_labels_by_nearest() { - // Sources at 0 and 4 on a 5-cell chain. Cells 0-1 closer to source 0, - // cells 3-4 closer to source 1. Cell 2 is equidistant. - let adj = chain(5); - let r = dijkstra(&adj, &[(0, 0, 0), (4, 0, 0)]); - assert_eq!(r.state[&(0, 0, 0)].source, 0); - assert_eq!(r.state[&(1, 0, 0)].source, 0); - assert_eq!(r.state[&(3, 0, 0)].source, 1); - assert_eq!(r.state[&(4, 0, 0)].source, 1); - // Tie at cell 2 must resolve to one of the two sources. - let s2 = r.state[&(2, 0, 0)].source; + let (sc, ids) = chain(5); + let mut st = DijkstraState::default(); + dijkstra(&sc, &[ids[0], ids[4]], &mut st); + assert_eq!(st.source[ids[0] as usize], 0); + assert_eq!(st.source[ids[1] as usize], 0); + assert_eq!(st.source[ids[3] as usize], 1); + assert_eq!(st.source[ids[4] as usize], 1); + let s2 = st.source[ids[2] as usize]; assert!(s2 == 0 || s2 == 1); - assert_eq!(r.state[&(0, 0, 0)].dist, 0.0); - assert_eq!(r.state[&(1, 0, 0)].dist, 1.0); - assert_eq!(r.state[&(2, 0, 0)].dist, 2.0); - assert_eq!(r.state[&(3, 0, 0)].dist, 1.0); - assert_eq!(r.state[&(4, 0, 0)].dist, 0.0); + assert_eq!(st.dist[ids[0] as usize], 0.0); + assert_eq!(st.dist[ids[1] as usize], 1.0); + assert_eq!(st.dist[ids[2] as usize], 2.0); + assert_eq!(st.dist[ids[3] as usize], 1.0); + assert_eq!(st.dist[ids[4] as usize], 0.0); } #[test] fn disconnected_cells_stay_unreachable() { - // Two separate chains 0-1 and 2-3, source at 0. - let mut adj = SurfaceAdjacency::new(); - for &c in &[(0, 0, 0), (1, 0, 0), (2, 0, 0), (3, 0, 0)] { - adj.add_cell(c); - } - adj.add_edge((0, 0, 0), (1, 0, 0), 1.0); - adj.add_edge((1, 0, 0), (0, 0, 0), 1.0); - adj.add_edge((2, 0, 0), (3, 0, 0), 1.0); - adj.add_edge((3, 0, 0), (2, 0, 0), 1.0); - let r = dijkstra(&adj, &[(0, 0, 0)]); - assert_eq!(r.state[&(0, 0, 0)].dist, 0.0); - assert_eq!(r.state[&(1, 0, 0)].dist, 1.0); - assert!(!r.state.contains_key(&(2, 0, 0))); - assert!(!r.state.contains_key(&(3, 0, 0))); + let mut sc = SurfaceCells::default(); + let a = sc.alloc((0, 0, 0)); + let b = sc.alloc((1, 0, 0)); + let c = sc.alloc((2, 0, 0)); + let d = sc.alloc((3, 0, 0)); + sc.add_edge(a, b, 1.0); + sc.add_edge(b, a, 1.0); + sc.add_edge(c, d, 1.0); + sc.add_edge(d, c, 1.0); + let mut st = DijkstraState::default(); + dijkstra(&sc, &[a], &mut st); + assert_eq!(st.dist[a as usize], 0.0); + assert_eq!(st.dist[b as usize], 1.0); + assert!(!st.dist[c as usize].is_finite()); + assert!(!st.dist[d as usize].is_finite()); } #[test] fn shorter_path_overrides_longer() { - // 0 - 1 with cost 10, 0 - 2 - 1 with cost 1+1=2. - let mut adj = SurfaceAdjacency::new(); - for &c in &[(0, 0, 0), (1, 0, 0), (2, 0, 0)] { - adj.add_cell(c); + let mut sc = SurfaceCells::default(); + let a = sc.alloc((0, 0, 0)); + let b = sc.alloc((1, 0, 0)); + let c = sc.alloc((2, 0, 0)); + sc.add_edge(a, b, 10.0); + sc.add_edge(b, a, 10.0); + sc.add_edge(a, c, 1.0); + sc.add_edge(c, a, 1.0); + sc.add_edge(c, b, 1.0); + sc.add_edge(b, c, 1.0); + let mut st = DijkstraState::default(); + dijkstra(&sc, &[a], &mut st); + assert_eq!(st.dist[b as usize], 2.0); + assert_eq!(st.pred[b as usize], c); + } + + #[test] + fn buffer_reuse_does_not_leak_prior_state() { + let (sc1, ids1) = chain(5); + let mut st = DijkstraState::default(); + dijkstra(&sc1, &[ids1[0]], &mut st); + let (sc2, ids2) = chain(3); + dijkstra(&sc2, &[ids2[0]], &mut st); + for (i, &id) in ids2.iter().enumerate().take(3) { + assert_eq!(st.dist[id as usize], i as f32); } - adj.add_edge((0, 0, 0), (1, 0, 0), 10.0); - adj.add_edge((1, 0, 0), (0, 0, 0), 10.0); - adj.add_edge((0, 0, 0), (2, 0, 0), 1.0); - adj.add_edge((2, 0, 0), (0, 0, 0), 1.0); - adj.add_edge((2, 0, 0), (1, 0, 0), 1.0); - adj.add_edge((1, 0, 0), (2, 0, 0), 1.0); - let r = dijkstra(&adj, &[(0, 0, 0)]); - assert_eq!(r.state[&(1, 0, 0)].dist, 2.0); - assert_eq!(r.state[&(1, 0, 0)].pred, Some((2, 0, 0))); } } diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/edges.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/edges.rs index 7c039bd975..91331f9b85 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/edges.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/edges.rs @@ -11,168 +11,189 @@ use ahash::AHashMap; use rayon::prelude::*; -use crate::adjacency::{SurfaceAdjacency, SurfaceLookup}; -use crate::dijkstra::{dijkstra, CellState}; -use crate::nodes::{NodeData, SurfaceGraph}; +use crate::adjacency::{CellId, Edge, SurfaceCells, SurfaceLookup, NO_CELL}; +use crate::dijkstra::{dijkstra, walk_preds, DijkstraState}; +use crate::nodes::NodeData; use crate::voxel::VoxelKey; +#[derive(Clone, Copy, Debug)] pub struct NodeEdge { pub a: u32, pub b: u32, pub cost: f32, /// Cell on a's side of the cheapest Voronoi boundary crossing. - pub boundary_u: VoxelKey, + pub boundary_u: CellId, /// Cell on b's side. - pub boundary_v: VoxelKey, + pub boundary_v: CellId, } +/// Long-lived planner graph. Buffers are reused across rebuilds. +#[derive(Default)] pub struct PlannerGraph { + pub cells: SurfaceCells, pub surface_lookup: SurfaceLookup, pub nodes: Vec, pub node_edges: Vec, pub node_adj: Vec>, + pub cell_state: DijkstraState, +} - pub cell_state: AHashMap, +impl PlannerGraph { + pub fn new() -> Self { + Self::default() + } } -pub fn add_node_edges(sg: SurfaceGraph) -> PlannerGraph { - let SurfaceGraph { - adj, - surface_lookup, - nodes, - } = sg; +/// Run multi-source Dijkstra from all node cells, then derive the cheapest +/// inter-node edge per Voronoi-region boundary pair. Reuses `state` as the +/// scratch buffer. The state is left holding the node-source Dijkstra +/// result so callers can walk preds for waypoint reconstruction. +pub fn build_node_edges( + cells: &SurfaceCells, + nodes: &[NodeData], + state: &mut DijkstraState, + out_edges: &mut Vec, + out_adj: &mut Vec>, +) { + out_edges.clear(); + out_adj.clear(); if nodes.is_empty() { - return PlannerGraph { - surface_lookup, - nodes, - node_edges: Vec::new(), - node_adj: Vec::new(), - cell_state: AHashMap::new(), - }; + state.reset(cells.slot_capacity()); + return; } - let source_cells: Vec = nodes.iter().map(|n| n.cell).collect(); - let cell_state = dijkstra(&adj, &source_cells).state; - let node_edges = best_boundary_edges(&adj, &cell_state); + let source_cells: Vec = nodes.iter().map(|n| n.cell_id).collect(); + dijkstra(cells, &source_cells, state); - let mut node_adj: Vec> = vec![Vec::new(); nodes.len()]; - for (edge_idx, edge) in node_edges.iter().enumerate() { - node_adj[edge.a as usize].push(edge_idx as u32); - node_adj[edge.b as usize].push(edge_idx as u32); - } + best_boundary_edges(cells, state, out_edges); - PlannerGraph { - surface_lookup, - nodes, - node_edges, - node_adj, - cell_state, + out_adj.resize_with(nodes.len(), Vec::new); + for v in out_adj.iter_mut() { + v.clear(); } + for (edge_idx, edge) in out_edges.iter().enumerate() { + out_adj[edge.a as usize].push(edge_idx as u32); + out_adj[edge.b as usize].push(edge_idx as u32); + } +} + +fn best_boundary_edges(cells: &SurfaceCells, state: &DijkstraState, out: &mut Vec) { + let cell_entries: Vec<(CellId, &[Edge])> = cells.iter().collect(); + + let merged: AHashMap<(u32, u32), NodeEdge> = cell_entries + .par_iter() + .fold( + AHashMap::<(u32, u32), NodeEdge>::new, + |mut local, (u, edges)| { + let du = state.dist[*u as usize]; + if !du.is_finite() { + return local; + } + let sa = state.source[*u as usize]; + for edge in *edges { + let v = edge.dst; + let dv = state.dist[v as usize]; + if !dv.is_finite() { + continue; + } + let sb = state.source[v as usize]; + if sa == sb { + continue; + } + let cost = du + edge.cost + dv; + + let (key_a, key_b, bu, bv) = if sa < sb { + (sa, sb, *u, v) + } else { + (sb, sa, v, *u) + }; + + let entry = local.entry((key_a, key_b)).or_insert(NodeEdge { + a: key_a, + b: key_b, + cost: f32::INFINITY, + boundary_u: NO_CELL, + boundary_v: NO_CELL, + }); + if cost < entry.cost { + entry.cost = cost; + entry.boundary_u = bu; + entry.boundary_v = bv; + } + } + local + }, + ) + .reduce(AHashMap::<(u32, u32), NodeEdge>::new, |mut a, b| { + for (k, v_edge) in b { + let entry = a.entry(k).or_insert(v_edge); + if v_edge.cost < entry.cost { + *entry = v_edge; + } + } + a + }); + + out.clear(); + out.extend(merged.into_values()); + out.par_sort_unstable_by_key(|e| (e.a, e.b)); } -/// Walk every node-graph edge and emit one segment per consecutive cell pair -/// along the reconstructed cell path. -pub fn edges_to_segments(plg: &PlannerGraph, _voxel_size: f32) -> Vec<(VoxelKey, VoxelKey, f32)> { - plg.node_edges +/// Walk every node-graph edge and emit one segment per consecutive cell +/// pair along the reconstructed cell path. Output coords are in VoxelKey +/// space. +pub fn edges_to_segments( + cells: &SurfaceCells, + state: &DijkstraState, + node_edges: &[NodeEdge], +) -> Vec<(VoxelKey, VoxelKey, f32)> { + node_edges .par_iter() .flat_map_iter(|edge| { - let mut from_a = walk_preds_to_source(plg, edge.boundary_u); + let mut from_a = walk_preds(state, edge.boundary_u); from_a.reverse(); - let to_b = walk_preds_to_source(plg, edge.boundary_v); - let mut path: Vec = from_a; - path.extend(to_b); + let to_b = walk_preds(state, edge.boundary_v); + let path: Vec = from_a.into_iter().chain(to_b).collect(); let cost = edge.cost; path.windows(2) - .map(|pair| (pair[0], pair[1], cost)) + .map(|pair| (cells.coord(pair[0]), cells.coord(pair[1]), cost)) .collect::>() }) .collect() } -pub fn walk_preds_to_source(plg: &PlannerGraph, start_cell: VoxelKey) -> Vec { - let mut cells = vec![start_cell]; - let mut cur = start_cell; - while let Some(p) = plg.cell_state.get(&cur).and_then(|s| s.pred) { - cur = p; - cells.push(cur); - } - cells -} - -fn best_boundary_edges( - adj: &SurfaceAdjacency, - state: &AHashMap, -) -> Vec { - let mut best: AHashMap<(u32, u32), NodeEdge> = AHashMap::new(); - - for (u, edges) in adj.iter() { - let Some(su) = state.get(&u) else { - continue; - }; - let sa = su.source; - let du = su.dist; - for edge in edges { - let v = edge.dst; - let Some(sv) = state.get(&v) else { - continue; - }; - let sb = sv.source; - if sa == sb { - continue; - } - let dv = sv.dist; - let cost = du + edge.cost + dv; - - let (key_a, key_b, bu, bv) = if sa < sb { - (sa, sb, u, v) - } else { - (sb, sa, v, u) - }; - - let entry = best.entry((key_a, key_b)).or_insert(NodeEdge { - a: key_a, - b: key_b, - cost: f32::INFINITY, - boundary_u: (0, 0, 0), - boundary_v: (0, 0, 0), - }); - if cost < entry.cost { - entry.cost = cost; - entry.boundary_u = bu; - entry.boundary_v = bv; - } - } - } - - let mut out: Vec = best.into_values().collect(); - out.sort_by_key(|e| (e.a, e.b)); - out -} - #[cfg(test)] mod tests { use super::*; - use crate::adjacency::{build_surface_adjacency, build_surface_lookup}; + use crate::adjacency::{build_surface_cells, build_surface_lookup}; + use crate::nodes::NodeData; use crate::voxel::surface_point_xyz; const VOXEL: f32 = 0.1; - fn graph_with_nodes(surface_cells: &[VoxelKey], node_cells: &[VoxelKey]) -> SurfaceGraph { - let surface_lookup = build_surface_lookup(surface_cells); - let adj = build_surface_adjacency(&surface_lookup, VOXEL, 2); - let nodes: Vec = node_cells + fn setup(surface: &[VoxelKey], node_cells: &[VoxelKey]) -> PlannerGraph { + let mut plg = PlannerGraph::new(); + build_surface_lookup(surface, &mut plg.surface_lookup); + build_surface_cells(&mut plg.cells, &plg.surface_lookup, VOXEL, 2); + plg.nodes = node_cells .iter() - .map(|&c| NodeData { - cell: c, - pos: surface_point_xyz(c.0, c.1, c.2, VOXEL), + .map(|&c| { + let id = plg.cells.id(c).expect("node cell must be in surface"); + NodeData { + cell_id: id, + pos: surface_point_xyz(c.0, c.1, c.2, VOXEL), + } }) .collect(); - SurfaceGraph { - adj, - surface_lookup, - nodes, - } + build_node_edges( + &plg.cells, + &plg.nodes, + &mut plg.cell_state, + &mut plg.node_edges, + &mut plg.node_adj, + ); + plg } fn strip_cells() -> Vec { @@ -181,8 +202,7 @@ mod tests { #[test] fn two_nodes_on_strip_have_one_edge() { - let sg = graph_with_nodes(&strip_cells(), &[(3, 0, 0), (15, 0, 0)]); - let pg = add_node_edges(sg); + let pg = setup(&strip_cells(), &[(3, 0, 0), (15, 0, 0)]); assert_eq!(pg.node_edges.len(), 1); let e = &pg.node_edges[0]; assert_eq!((e.a, e.b), (0, 1)); @@ -192,49 +212,31 @@ mod tests { #[test] fn three_nodes_in_line_form_a_chain() { - // Nodes at 3, 10, 17 in a strip 0..20. Voronoi boundaries are - // around 6-7 and 13-14, so we get edges (0,1) and (1,2) but no (0,2). - let sg = graph_with_nodes(&strip_cells(), &[(3, 0, 0), (10, 0, 0), (17, 0, 0)]); - let pg = add_node_edges(sg); + let pg = setup(&strip_cells(), &[(3, 0, 0), (10, 0, 0), (17, 0, 0)]); let pairs: Vec<(u32, u32)> = pg.node_edges.iter().map(|e| (e.a, e.b)).collect(); assert_eq!(pairs, vec![(0, 1), (1, 2)]); } #[test] fn disconnected_components_have_no_edge() { - // Two strips with a gap, one node in each. let mut cells: Vec = (0..5).map(|x| (x, 0, 0)).collect(); cells.extend((10..15).map(|x| (x, 0, 0))); - let sg = graph_with_nodes(&cells, &[(2, 0, 0), (12, 0, 0)]); - let pg = add_node_edges(sg); + let pg = setup(&cells, &[(2, 0, 0), (12, 0, 0)]); assert!(pg.node_edges.is_empty()); } #[test] fn predecessor_walk_recovers_cell_path() { - // Two nodes at strip ends. Walk preds from each boundary cell back to - // its owning node cell and verify the chain reaches the node. - let sg = graph_with_nodes(&strip_cells(), &[(0, 0, 0), (19, 0, 0)]); - let pg = add_node_edges(sg); + let pg = setup(&strip_cells(), &[(0, 0, 0), (19, 0, 0)]); assert_eq!(pg.node_edges.len(), 1); let e = &pg.node_edges[0]; - let cell_a = pg.nodes[0].cell; - let cell_b = pg.nodes[1].cell; - - let mut cur = e.boundary_u; - let mut hops = 0; - while let Some(p) = pg.cell_state.get(&cur).and_then(|s| s.pred) { - cur = p; - hops += 1; - assert!(hops < 1000, "predecessor walk did not terminate"); - } - assert_eq!(cur, cell_a, "u-side preds must reach node a"); - - let mut cur = e.boundary_v; - while let Some(p) = pg.cell_state.get(&cur).and_then(|s| s.pred) { - cur = p; - } - assert_eq!(cur, cell_b, "v-side preds must reach node b"); + let cell_a = pg.nodes[0].cell_id; + let cell_b = pg.nodes[1].cell_id; + + let chain_u = walk_preds(&pg.cell_state, e.boundary_u); + let chain_v = walk_preds(&pg.cell_state, e.boundary_v); + assert_eq!(chain_u.last(), Some(&cell_a)); + assert_eq!(chain_v.last(), Some(&cell_b)); } } diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs index 9627d42449..eead046324 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs @@ -21,9 +21,10 @@ use tracing::info; use ahash::AHashSet; -use crate::edges::{add_node_edges, edges_to_segments, PlannerGraph}; +use crate::adjacency::{build_surface_cells, build_surface_lookup}; +use crate::edges::{build_node_edges, edges_to_segments, PlannerGraph}; use crate::nodes::place_nodes; -use crate::surfaces::extract_surfaces; +use crate::surfaces::{extract_surfaces, ColumnIz}; use crate::voxel::{surface_point_xyz, voxelize, VoxelKey}; #[derive(Debug, Deserialize)] @@ -70,6 +71,10 @@ struct MlsPlanner { step_cells: i32, planner_graph: Option, latest_start: Option<(f32, f32, f32)>, + + voxel_map_buf: AHashSet, + by_col_buf: ColumnIz, + surface_buf: Vec, } impl MlsPlanner { @@ -110,63 +115,86 @@ impl MlsPlanner { return; } - let cfg = &self.config; + let voxel_size = self.config.voxel_size; + let step_cells = self.step_cells; + let clearance_cells = self.clearance_cells; + let dil = self.config.surface_dilation_passes; + let ero = self.config.surface_erosion_passes; + let spacing = self.config.node_spacing_m; + let wall_buf = self.config.node_wall_buffer_m; + let frame = self.config.world_frame.clone(); let t_surface = Instant::now(); - // convert whatever map we got in to voxels - let voxel_map: AHashSet = points - .iter() - .map(|&p| voxelize(p, cfg.voxel_size)) - .collect(); - let surface_cells = extract_surfaces( - &voxel_map, - self.clearance_cells, - cfg.surface_dilation_passes, - cfg.surface_erosion_passes, + self.voxel_map_buf.clear(); + for &p in &points { + self.voxel_map_buf.insert(voxelize(p, voxel_size)); + } + let voxels_count = self.voxel_map_buf.len(); + + extract_surfaces( + &self.voxel_map_buf, + clearance_cells, + dil, + ero, + &mut self.by_col_buf, + &mut self.surface_buf, ); + let surface_count = self.surface_buf.len(); + + let plg = self.planner_graph.get_or_insert_with(PlannerGraph::new); + build_surface_lookup(&self.surface_buf, &mut plg.surface_lookup); + build_surface_cells(&mut plg.cells, &plg.surface_lookup, voxel_size, step_cells); let surface_ms = ms(t_surface.elapsed()); - let surface_points: Vec<(f32, f32, f32)> = surface_cells + let surface_points: Vec<(f32, f32, f32)> = self + .surface_buf .iter() - .map(|&(ix, iy, iz)| surface_point_xyz(ix, iy, iz, cfg.voxel_size)) + .map(|&(ix, iy, iz)| surface_point_xyz(ix, iy, iz, voxel_size)) .collect(); - publish_cloud(&self.surface_map, &surface_points, &cfg.world_frame, now()).await; + publish_cloud(&self.surface_map, &surface_points, &frame, now()).await; let t_nodes = Instant::now(); - let sg = place_nodes( - &surface_cells, - cfg.voxel_size, - self.step_cells, - cfg.node_spacing_m, - cfg.node_wall_buffer_m, + let plg = self.planner_graph.as_mut().expect("just inserted"); + place_nodes( + &mut plg.cells, + voxel_size, + spacing, + wall_buf, + &mut plg.cell_state, + &mut plg.nodes, ); let nodes_ms = ms(t_nodes.elapsed()); - let n_nodes = sg.nodes.len(); + let nodes_count = plg.nodes.len(); - let node_points: Vec<(f32, f32, f32)> = sg.nodes.iter().map(|n| n.pos).collect(); - publish_cloud(&self.nodes, &node_points, &cfg.world_frame, now()).await; + let node_points: Vec<(f32, f32, f32)> = plg.nodes.iter().map(|n| n.pos).collect(); + publish_cloud(&self.nodes, &node_points, &frame, now()).await; let t_edges = Instant::now(); - let plg = add_node_edges(sg); + let plg = self.planner_graph.as_mut().expect("just inserted"); + build_node_edges( + &plg.cells, + &plg.nodes, + &mut plg.cell_state, + &mut plg.node_edges, + &mut plg.node_adj, + ); let edges_ms = ms(t_edges.elapsed()); - let n_edges = plg.node_edges.len(); + let edges_count = plg.node_edges.len(); - let edges_path = build_segments_path(&plg, cfg.voxel_size, &cfg.world_frame, now()); + let edges_path = build_segments_path(plg, voxel_size, &frame, now()); publish_path(&self.node_edges, &edges_path).await; info!( global_map_points = points.len(), - voxels = voxel_map.len(), - surface_cells = surface_cells.len(), - nodes = n_nodes, - edges = n_edges, + voxels = voxels_count, + surface_cells = surface_count, + nodes = nodes_count, + edges = edges_count, surface_ms, nodes_ms, edges_ms, "global_map processed", ); - - self.planner_graph = Some(plg); } async fn on_start_pose(&mut self, msg: Odometry) { @@ -186,6 +214,10 @@ impl MlsPlanner { tracing::warn!("MLSPlanner received goal before graph was built; skipping"); return; }; + if plg.nodes.is_empty() { + tracing::warn!("MLSPlanner received goal before graph had nodes; skipping"); + return; + } let p = &msg.pose.pose.position; let goal = (p.x as f32, p.y as f32, p.z as f32); @@ -320,7 +352,7 @@ fn build_path_from_waypoints(waypoints: &[(f32, f32, f32)], frame_id: &str, stam /// Emit edges as alternating PoseStamped pairs with orientation.w carrying /// the per-edge cost. fn build_segments_path(plg: &PlannerGraph, voxel_size: f32, frame_id: &str, stamp: Time) -> Path { - let segments = edges_to_segments(plg, voxel_size); + let segments = edges_to_segments(&plg.cells, &plg.cell_state, &plg.node_edges); let mut poses: Vec = Vec::with_capacity(segments.len() * 2); for (a, b, cost) in segments { let pa = surface_point_xyz(a.0, a.1, a.2, voxel_size); diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs index 633a64668c..cbb1ba8d03 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs @@ -5,106 +5,100 @@ //! nodes at local maxima via NMS, and rescale cell-edge costs to push paths //! toward corridor centers. -use ahash::{AHashMap, AHashSet}; +use ahash::AHashMap; use rayon::prelude::*; -use crate::adjacency::{ - build_surface_adjacency, build_surface_lookup, Edge, SurfaceAdjacency, SurfaceLookup, -}; -use crate::dijkstra::dijkstra; +use crate::adjacency::{CellId, Edge, SurfaceCells}; +use crate::dijkstra::{dijkstra, DijkstraState}; use crate::voxel::{surface_point_xyz, VoxelKey}; +#[derive(Clone, Copy, Debug)] pub struct NodeData { - pub cell: VoxelKey, + pub cell_id: CellId, pub pos: (f32, f32, f32), } -pub struct SurfaceGraph { - pub adj: SurfaceAdjacency, - pub surface_lookup: SurfaceLookup, - pub nodes: Vec, -} - +/// Run wall-distance Dijkstra, NMS-pick node cells, write them into +/// `out_nodes`, and apply wall-safe edge-cost scaling to `cells`. The +/// DijkstraState is left holding the wall-distance result so callers can +/// reuse it before the next Dijkstra overwrites it. pub fn place_nodes( - surface_cells: &[VoxelKey], + cells: &mut SurfaceCells, voxel_size: f32, - maximum_step_cells: i32, node_spacing_m: f32, node_wall_buffer_m: f32, -) -> SurfaceGraph { - let surface_lookup = build_surface_lookup(surface_cells); - let mut adj = build_surface_adjacency(&surface_lookup, voxel_size, maximum_step_cells); - - if adj.is_empty() { - return SurfaceGraph { - adj, - surface_lookup, - nodes: Vec::new(), - }; + state: &mut DijkstraState, + out_nodes: &mut Vec, +) { + out_nodes.clear(); + if cells.is_empty() { + return; } - let wall_seeds = wall_adjacent_cells(&adj); - let dist = dijkstra(&adj, &wall_seeds).dist_map(); - - let mut candidates: Vec = dist - .iter() - .filter_map(|(&c, &d)| { - if d >= node_wall_buffer_m { - Some(c) - } else { - None - } - }) - .collect(); - candidates.sort_by(|a, b| dist[b].total_cmp(&dist[a]).then(a.cmp(b))); - - let survivors = nms_grid(&candidates, voxel_size, node_spacing_m); + let mut wall_seeds: Vec = Vec::new(); + collect_wall_adjacent_cells(cells, &mut wall_seeds); + dijkstra(cells, &wall_seeds, state); - let nodes: Vec = survivors - .iter() - .map(|&cell| NodeData { - cell, - pos: surface_point_xyz(cell.0, cell.1, cell.2, voxel_size), - }) + let mut candidates: Vec = cells + .ids() + .filter(|&id| state.dist[id as usize] >= node_wall_buffer_m) .collect(); + candidates.par_sort_unstable_by(|&a, &b| { + state.dist[b as usize] + .total_cmp(&state.dist[a as usize]) + .then(a.cmp(&b)) + }); - apply_wall_safe_penalty(&mut adj, &dist, node_wall_buffer_m); + let survivors = nms_grid(cells, &candidates, voxel_size, node_spacing_m); - SurfaceGraph { - adj, - surface_lookup, - nodes, + out_nodes.reserve(survivors.len()); + for &id in &survivors { + let (ix, iy, iz) = cells.coord(id); + out_nodes.push(NodeData { + cell_id: id, + pos: surface_point_xyz(ix, iy, iz, voxel_size), + }); } + + apply_wall_safe_penalty(cells, &state.dist, node_wall_buffer_m); } -/// Cells missing any of their 4 xy-direction neighbors are treated as boundaries. -fn wall_adjacent_cells(adj: &SurfaceAdjacency) -> Vec { - let entries: Vec<(VoxelKey, &[Edge])> = adj.iter().collect(); - let mut wall: Vec = entries - .par_iter() - .filter_map(|(c, edges)| { - let mut dirs: AHashSet<(i32, i32)> = AHashSet::new(); - for e in *edges { - dirs.insert((e.dst.0 - c.0, e.dst.1 - c.1)); - } - if dirs.len() < 4 { - Some(*c) - } else { - None - } - }) - .collect(); - wall.par_sort_unstable(); - if wall.is_empty() { - if let Some(c) = adj.cells().min() { - wall.push(c); +/// Cells missing any of their 4 xy-direction neighbors are treated as +/// boundaries. Direction membership is tracked with a 4-bit mask so the +/// 349k-cell case avoids per-cell hashset allocation. +fn collect_wall_adjacent_cells(cells: &SurfaceCells, out: &mut Vec) { + out.clear(); + for (id, edges) in cells.iter() { + let (cx, cy, _) = cells.coord(id); + let mut mask: u8 = 0; + for e in edges { + let (nx, ny, _) = cells.coord(e.dst); + mask |= match (nx - cx, ny - cy) { + (-1, 0) => 1, + (1, 0) => 2, + (0, -1) => 4, + (0, 1) => 8, + _ => 0, + }; + } + if mask != 0b1111 { + out.push(id); + } + } + if out.is_empty() { + if let Some(c) = cells.ids().next() { + out.push(c); } } - wall } -/// Space out nodes based on minimum distance -fn nms_grid(candidates_sorted: &[VoxelKey], voxel_size: f32, node_spacing_m: f32) -> Vec { +/// Space out nodes based on minimum distance. +fn nms_grid( + cells: &SurfaceCells, + candidates_sorted: &[CellId], + voxel_size: f32, + node_spacing_m: f32, +) -> Vec { let bin_size = ((node_spacing_m / voxel_size) as i32).max(1); let r_sq = (node_spacing_m as f64) * (node_spacing_m as f64); let v = voxel_size as f64; @@ -116,19 +110,21 @@ fn nms_grid(candidates_sorted: &[VoxelKey], voxel_size: f32, node_spacing_m: f32 ) }; - let mut bins: AHashMap<(i32, i32, i32), Vec> = AHashMap::new(); - let mut survivors = Vec::new(); - for &cell in candidates_sorted { - let (bx, by, bz) = bin_of(cell); + let mut bins: AHashMap<(i32, i32, i32), Vec> = AHashMap::new(); + let mut survivors: Vec = Vec::new(); + for &id in candidates_sorted { + let coord = cells.coord(id); + let (bx, by, bz) = bin_of(coord); let mut killed = false; 'outer: for dbx in -1..=1 { for dby in -1..=1 { for dbz in -1..=1 { if let Some(nearby) = bins.get(&(bx + dbx, by + dby, bz + dbz)) { - for &n in nearby { - let dx = (cell.0 - n.0) as f64 * v; - let dy = (cell.1 - n.1) as f64 * v; - let dz = (cell.2 - n.2) as f64 * v; + for &n_id in nearby { + let n = cells.coord(n_id); + let dx = (coord.0 - n.0) as f64 * v; + let dy = (coord.1 - n.1) as f64 * v; + let dz = (coord.2 - n.2) as f64 * v; if dx * dx + dy * dy + dz * dz <= r_sq { killed = true; break 'outer; @@ -139,46 +135,36 @@ fn nms_grid(candidates_sorted: &[VoxelKey], voxel_size: f32, node_spacing_m: f32 } } if !killed { - survivors.push(cell); - bins.entry((bx, by, bz)).or_default().push(cell); + survivors.push(id); + bins.entry((bx, by, bz)).or_default().push(id); } } survivors } -/// Scales every edge cost by the average of its endpoint penalties, which -/// pushes shortest paths away from walls. -fn apply_wall_safe_penalty( - adj: &mut SurfaceAdjacency, - dist: &AHashMap, - buffer_m: f32, -) { - let cells: Vec = adj.cells().collect(); - let penalty_pairs: Vec<(VoxelKey, f32)> = cells - .par_iter() - .map(|&c| { - let p = match dist.get(&c) { - Some(&d) => (1.0 + (buffer_m - d) / buffer_m).max(1.0), - None => 1.0, - }; - (c, p) - }) - .collect(); - let penalty: AHashMap = penalty_pairs.into_iter().collect(); - - let mut edge_lists: Vec<(VoxelKey, &mut Vec)> = adj.iter_edges_mut().collect(); +/// Scale every edge cost by the average of its endpoint penalties, which +/// pushes shortest paths away from walls. Unreached cells have +/// dist == +INFINITY which collapses to penalty 1.0. +fn apply_wall_safe_penalty(cells: &mut SurfaceCells, dist: &[f32], buffer_m: f32) { + let mut edge_lists: Vec<(CellId, &mut Vec)> = cells.iter_edges_mut().collect(); edge_lists.par_iter_mut().for_each(|(src, edges)| { - let pu = penalty.get(src).copied().unwrap_or(1.0); + let pu = penalty_of(dist[*src as usize], buffer_m); for edge in edges.iter_mut() { - let pv = penalty.get(&edge.dst).copied().unwrap_or(1.0); + let pv = penalty_of(dist[edge.dst as usize], buffer_m); edge.cost *= (pu + pv) / 2.0; } }); } +#[inline] +fn penalty_of(d: f32, buffer_m: f32) -> f32 { + (1.0 + (buffer_m - d) / buffer_m).max(1.0) +} + #[cfg(test)] mod tests { use super::*; + use crate::adjacency::{build_surface_cells, build_surface_lookup, SurfaceLookup}; const VOXEL: f32 = 0.1; @@ -192,43 +178,55 @@ mod tests { c } + fn build_cells(surface: &[VoxelKey], step_cells: i32) -> SurfaceCells { + let mut lookup = SurfaceLookup::new(); + build_surface_lookup(surface, &mut lookup); + let mut sc = SurfaceCells::default(); + build_surface_cells(&mut sc, &lookup, VOXEL, step_cells); + sc + } + #[test] fn open_patch_places_at_least_one_node() { - // 10x10 at voxel=0.1 is 1m x 1m. Center is ~0.5m from any wall, well above buffer=0.3m. - let sg = place_nodes(&open_patch(0, 0, 10), VOXEL, 2, 1.0, 0.3); - assert!(!sg.nodes.is_empty()); - for n in &sg.nodes { - let (ix, iy, _) = n.cell; + let mut sc = build_cells(&open_patch(0, 0, 10), 2); + let mut state = DijkstraState::default(); + let mut nodes = Vec::new(); + place_nodes(&mut sc, VOXEL, 1.0, 0.3, &mut state, &mut nodes); + assert!(!nodes.is_empty()); + for n in &nodes { + let (ix, iy, _) = sc.coord(n.cell_id); assert!((0..10).contains(&ix) && (0..10).contains(&iy)); } } #[test] fn sloped_patch_places_interior_nodes() { - // 10x10 plane sloped 1 cell of z per cell of x. With step_threshold=2 - // every interior cell still has all 4 xy-direction neighbors in-graph, - // so it must not be flagged as wall-adjacent. - let mut cells = Vec::new(); + let mut cells_in = Vec::new(); for ix in 0..10 { for iy in 0..10 { - cells.push((ix, iy, ix)); + cells_in.push((ix, iy, ix)); } } - let sg = place_nodes(&cells, VOXEL, 2, 1.0, 0.3); - assert!(!sg.nodes.is_empty()); + let mut sc = build_cells(&cells_in, 2); + let mut state = DijkstraState::default(); + let mut nodes = Vec::new(); + place_nodes(&mut sc, VOXEL, 1.0, 0.3, &mut state, &mut nodes); + assert!(!nodes.is_empty()); } #[test] fn nms_enforces_spacing() { - // Two 10x10 patches separated by 1m gap; each places at least one node, no pair within 1m. - let mut cells = open_patch(0, 0, 10); - cells.extend(open_patch(20, 0, 10)); - let sg = place_nodes(&cells, VOXEL, 2, 1.0, 0.3); - assert!(sg.nodes.len() >= 2); - for i in 0..sg.nodes.len() { - for j in (i + 1)..sg.nodes.len() { - let a = sg.nodes[i].pos; - let b = sg.nodes[j].pos; + let mut cells_in = open_patch(0, 0, 10); + cells_in.extend(open_patch(20, 0, 10)); + let mut sc = build_cells(&cells_in, 2); + let mut state = DijkstraState::default(); + let mut nodes = Vec::new(); + place_nodes(&mut sc, VOXEL, 1.0, 0.3, &mut state, &mut nodes); + assert!(nodes.len() >= 2); + for i in 0..nodes.len() { + for j in (i + 1)..nodes.len() { + let a = nodes[i].pos; + let b = nodes[j].pos; let dx = a.0 - b.0; let dy = a.1 - b.1; let dz = a.2 - b.2; @@ -240,12 +238,15 @@ mod tests { #[test] fn wall_cells_scale_outbound_cost() { - let cells: Vec = (0..10).map(|ix| (ix, 0, 0)).collect(); - let sg = place_nodes(&cells, VOXEL, 2, 1.0, 0.3); - let outbound: Vec<_> = sg.adj.neighbors((0, 0, 0)).collect(); + let cells_in: Vec = (0..10).map(|ix| (ix, 0, 0)).collect(); + let mut sc = build_cells(&cells_in, 2); + let mut state = DijkstraState::default(); + let mut nodes = Vec::new(); + place_nodes(&mut sc, VOXEL, 1.0, 0.3, &mut state, &mut nodes); + let id0 = sc.id((0, 0, 0)).unwrap(); + let outbound = sc.neighbors(id0); assert!(!outbound.is_empty()); - // End cell penalty=2, neighbor penalty>=1, so outbound cost >= 1.5 * VOXEL. - for edge in &outbound { + for edge in outbound { assert!(edge.cost >= 1.5 * VOXEL - 1e-5); } } diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/planner.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/planner.rs index 7c4a093f1f..fdc6e8ba2c 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/planner.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/planner.rs @@ -6,8 +6,9 @@ use std::collections::BinaryHeap; use ahash::AHashMap; -use crate::adjacency::SurfaceLookup; -use crate::edges::{walk_preds_to_source, PlannerGraph}; +use crate::adjacency::{CellId, SurfaceLookup}; +use crate::dijkstra::walk_preds; +use crate::edges::PlannerGraph; use crate::voxel::{surface_point_xyz, VoxelKey}; /// Snap a pose to the best surface cell. @@ -79,20 +80,23 @@ pub fn plan( voxel_size: f32, z_tolerance_m: f32, ) -> Option> { - let start_cell = snap_pose_to_cell(&plg.surface_lookup, start_pose, voxel_size, z_tolerance_m)?; - let goal_cell = snap_pose_to_cell(&plg.surface_lookup, goal_pose, voxel_size, z_tolerance_m)?; + let start_coord = + snap_pose_to_cell(&plg.surface_lookup, start_pose, voxel_size, z_tolerance_m)?; + let goal_coord = snap_pose_to_cell(&plg.surface_lookup, goal_pose, voxel_size, z_tolerance_m)?; + let start_cell = plg.cells.id(start_coord)?; + let goal_cell = plg.cells.id(goal_coord)?; - let node_cell_to_idx: AHashMap = plg + let node_idx_by_cell: AHashMap = plg .nodes .iter() .enumerate() - .map(|(i, n)| (n.cell, i as u32)) + .map(|(i, n)| (n.cell_id, i as u32)) .collect(); - let start_segment = walk_preds_to_source(plg, start_cell); - let goal_segment = walk_preds_to_source(plg, goal_cell); - let start_node = *node_cell_to_idx.get(start_segment.last()?)?; - let goal_node = *node_cell_to_idx.get(goal_segment.last()?)?; + let start_segment = walk_preds(&plg.cell_state, start_cell); + let goal_segment = walk_preds(&plg.cell_state, goal_cell); + let start_node = *node_idx_by_cell.get(start_segment.last()?)?; + let goal_node = *node_idx_by_cell.get(goal_segment.last()?)?; let node_seq = shortest_path_nodes(plg, start_node, goal_node)?; Some(assemble_waypoints( @@ -153,13 +157,12 @@ fn assemble_waypoints( plg: &PlannerGraph, node_seq: &[u32], start_pose: (f32, f32, f32), - start_segment: &[VoxelKey], + start_segment: &[CellId], goal_pose: (f32, f32, f32), - goal_segment: &[VoxelKey], + goal_segment: &[CellId], voxel_size: f32, ) -> Vec<(f32, f32, f32)> { - let mut cells: Vec = Vec::new(); - + let mut cells: Vec = Vec::new(); cells.extend_from_slice(start_segment); for pair in node_seq.windows(2) { @@ -173,9 +176,9 @@ fn assemble_waypoints( (edge.boundary_v, edge.boundary_u) }; - let mut from_a = walk_preds_to_source(plg, start_side); + let mut from_a = walk_preds(&plg.cell_state, start_side); from_a.reverse(); - let to_b = walk_preds_to_source(plg, end_side); + let to_b = walk_preds(&plg.cell_state, end_side); for c in from_a.into_iter().chain(to_b) { if cells.last() != Some(&c) { @@ -192,7 +195,8 @@ fn assemble_waypoints( let mut waypoints: Vec<(f32, f32, f32)> = Vec::with_capacity(cells.len() + 2); waypoints.push(start_pose); - for (ix, iy, iz) in cells { + for id in cells { + let (ix, iy, iz) = plg.cells.coord(id); waypoints.push(surface_point_xyz(ix, iy, iz, voxel_size)); } waypoints.push(goal_pose); @@ -232,29 +236,35 @@ impl Ord for Scored { #[cfg(test)] mod tests { use super::*; - use crate::adjacency::{build_surface_adjacency, build_surface_lookup}; - use crate::edges::add_node_edges; - use crate::nodes::{NodeData, SurfaceGraph}; + use crate::adjacency::{build_surface_cells, build_surface_lookup}; + use crate::edges::build_node_edges; + use crate::nodes::NodeData; const VOXEL: f32 = 0.1; const Z_TOL: f32 = 1.5; fn graph_with_nodes(surface_cells: &[VoxelKey], node_cells: &[VoxelKey]) -> PlannerGraph { - let surface_lookup = build_surface_lookup(surface_cells); - let adj = build_surface_adjacency(&surface_lookup, VOXEL, 2); - let nodes: Vec = node_cells + let mut plg = PlannerGraph::new(); + build_surface_lookup(surface_cells, &mut plg.surface_lookup); + build_surface_cells(&mut plg.cells, &plg.surface_lookup, VOXEL, 2); + plg.nodes = node_cells .iter() - .map(|&c| NodeData { - cell: c, - pos: surface_point_xyz(c.0, c.1, c.2, VOXEL), + .map(|&c| { + let id = plg.cells.id(c).expect("node cell must be in surface"); + NodeData { + cell_id: id, + pos: surface_point_xyz(c.0, c.1, c.2, VOXEL), + } }) .collect(); - let sg = SurfaceGraph { - adj, - surface_lookup, - nodes, - }; - add_node_edges(sg) + build_node_edges( + &plg.cells, + &plg.nodes, + &mut plg.cell_state, + &mut plg.node_edges, + &mut plg.node_adj, + ); + plg } fn strip(n: i32) -> Vec { @@ -263,25 +273,26 @@ mod tests { #[test] fn snap_picks_in_column_cell() { - let lookup = build_surface_lookup(&strip(20)); + let mut lookup = SurfaceLookup::new(); + build_surface_lookup(&strip(20), &mut lookup); let cell = snap_pose_to_cell(&lookup, (0.5, 0.0, 0.1), VOXEL, Z_TOL).unwrap(); assert_eq!(cell, (5, 0, 0)); } #[test] fn snap_falls_back_to_nearby_column() { - // Column ix=2 is removed; a pose in that column must snap to an adjacent one. let mut cells = strip(20); cells.retain(|c| c.0 != 2); - let lookup = build_surface_lookup(&cells); + let mut lookup = SurfaceLookup::new(); + build_surface_lookup(&cells, &mut lookup); let cell = snap_pose_to_cell(&lookup, (0.25, 0.0, 0.1), VOXEL, Z_TOL).unwrap(); assert!(cell == (1, 0, 0) || cell == (3, 0, 0)); } #[test] fn snap_rejects_outside_z_tolerance() { - let lookup = build_surface_lookup(&strip(20)); - // Surface at iz=0, pose at z=2.0, tolerance=1.5 is out of tolerance in every column. + let mut lookup = SurfaceLookup::new(); + build_surface_lookup(&strip(20), &mut lookup); assert!(snap_pose_to_cell(&lookup, (0.5, 0.0, 2.0), VOXEL, 1.5).is_none()); } @@ -294,7 +305,6 @@ mod tests { #[test] fn plan_returns_none_if_disconnected() { - // Two strips with a gap. let mut cells: Vec = (0..5).map(|x| (x, 0, 0)).collect(); cells.extend((10..15).map(|x| (x, 0, 0))); let plg = graph_with_nodes(&cells, &[(2, 0, 0), (12, 0, 0)]); @@ -326,7 +336,6 @@ mod tests { fn plan_three_nodes_visits_them_all() { let plg = graph_with_nodes(&strip(20), &[(3, 0, 0), (10, 0, 0), (17, 0, 0)]); let wp = plan(&plg, (0.2, 0.0, 0.05), (1.9, 0.0, 0.05), VOXEL, Z_TOL).unwrap(); - // Each node's XY position should appear in the waypoints. let node_xy: Vec<(f32, f32)> = plg.nodes.iter().map(|n| (n.pos.0, n.pos.1)).collect(); for &(nx, ny) in &node_xy { assert!( diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs index c5499ee6ce..a6d387b051 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs @@ -16,7 +16,7 @@ use crate::voxel::VoxelKey; const ON: Luma = Luma([255]); const OFF: Luma = Luma([0]); -type ColumnIz = AHashMap<(i32, i32), Vec>; +pub type ColumnIz = AHashMap<(i32, i32), Vec>; /// A cell is standable if it has at least the robot's height of clear /// space above it. @@ -32,19 +32,22 @@ fn is_standable(ix: i32, iy: i32, iz: i32, by_col: &ColumnIz, clearance_cells: i } /// Extract standable cells from the voxelized global map, then close small -/// holes. Clearance height given as number of cells. +/// holes. `by_col` is a scratch buffer cleared on entry; its capacity is +/// reused across calls. Results land in `out` (cleared first). pub fn extract_surfaces( voxel_map: &AHashSet, clearance_cells: i32, dilation_passes: u32, erosion_passes: u32, -) -> Vec { + by_col: &mut ColumnIz, + out: &mut Vec, +) { + out.clear(); + by_col.clear(); if voxel_map.is_empty() { - return Vec::new(); + return; } - // bin voxels in to their columns - let mut by_col: AHashMap<(i32, i32), Vec> = AHashMap::new(); for &(ix, iy, iz) in voxel_map { by_col.entry((ix, iy)).or_default().push(iz); } @@ -58,28 +61,28 @@ pub fn extract_surfaces( let standable: Vec = entries .par_iter() .flat_map_iter(|((ix, iy), zs)| { - let mut out: Vec = Vec::new(); - // find gaps of at least clearance height through the column + let mut local: Vec = Vec::new(); for w in zs.windows(2) { if w[1] - w[0] > clearance_cells { - out.push((*ix, *iy, w[0])); + local.push((*ix, *iy, w[0])); } } if let Some(&last_iz) = zs.last() { - out.push((*ix, *iy, last_iz)); + local.push((*ix, *iy, last_iz)); } - out + local }) .collect(); drop(entries); close_surface_holes( standable, - &by_col, + by_col, dilation_passes, erosion_passes, clearance_cells, - ) + out, + ); } /// Dilation and erosion on all xy slices of the extracted surfaces @@ -90,31 +93,29 @@ fn close_surface_holes( dilation_passes: u32, erosion_passes: u32, clearance_cells: i32, -) -> Vec { + out: &mut Vec, +) { if standable.is_empty() || (dilation_passes == 0 && erosion_passes == 0) { - return standable; + out.extend(standable); + return; } - // slice the surfaces in to xy planes so we can do the 2d morphology let mut by_z: AHashMap> = AHashMap::new(); for &(ix, iy, iz) in &standable { by_z.entry(iz).or_default().push((ix, iy)); } let slices: Vec<(i32, Vec<(i32, i32)>)> = by_z.into_iter().collect(); - slices - .par_iter() - .flat_map_iter(|(iz, xys)| { - close_at_z( - xys, - *iz, - by_col, - dilation_passes, - erosion_passes, - clearance_cells, - ) - }) - .collect() + out.par_extend(slices.par_iter().flat_map_iter(|(iz, xys)| { + close_at_z( + xys, + *iz, + by_col, + dilation_passes, + erosion_passes, + clearance_cells, + ) + })); } /// Close holes on an xy slice of the surfaces. @@ -181,35 +182,41 @@ mod tests { cells.iter().copied().collect() } + fn run(cells: &[VoxelKey], clearance: i32, dil: u32, ero: u32) -> Vec { + let map = voxel_map(cells); + let mut by_col = ColumnIz::new(); + let mut out = Vec::new(); + extract_surfaces(&map, clearance, dil, ero, &mut by_col, &mut out); + out + } + #[test] fn empty_input() { - assert!(extract_surfaces(&AHashSet::new(), 5, 0, 0).is_empty()); + assert!(run(&[], 5, 0, 0).is_empty()); } #[test] fn single_cell_is_topmost_surface() { - let s = extract_surfaces(&voxel_map(&[(0, 0, 0)]), 5, 0, 0); + let s = run(&[(0, 0, 0)], 5, 0, 0); assert_eq!(s, vec![(0, 0, 0)]); } #[test] fn stacked_cells_within_headroom_only_topmost_is_surface() { let cells: Vec = (0..5).map(|z| (0, 0, z)).collect(); - let s = extract_surfaces(&voxel_map(&cells), 5, 0, 0); + let s = run(&cells, 5, 0, 0); assert_eq!(s, vec![(0, 0, 4)]); } #[test] fn gap_larger_than_headroom_makes_lower_cell_standable() { - // Voxel map points at iz=0 and iz=10 with clearance_cells=5. Lower cell has gap=10 > 5. - let mut s = extract_surfaces(&voxel_map(&[(0, 0, 0), (0, 0, 10)]), 5, 0, 0); + let mut s = run(&[(0, 0, 0), (0, 0, 10)], 5, 0, 0); s.sort(); assert_eq!(s, vec![(0, 0, 0), (0, 0, 10)]); } #[test] fn morphological_closing_fills_center_hole() { - // Ring of 8 cells around (0,0) at iz=0, nothing above. let cells: Vec = [ (-1, -1), (-1, 0), @@ -223,7 +230,7 @@ mod tests { .into_iter() .map(|(dx, dy)| (dx, dy, 0)) .collect(); - let s = extract_surfaces(&voxel_map(&cells), 5, 3, 3); + let s = run(&cells, 5, 3, 3); assert!( s.contains(&(0, 0, 0)), "closing should fill the center hole" @@ -232,8 +239,6 @@ mod tests { #[test] fn closing_does_not_bridge_voxel_in_headroom() { - // Ring of 8 cells at iz=0 plus a voxel directly above the hole at (0,0,1). - // The hole at (0,0,0) is vetoed because its headroom column is occupied. let mut cells: Vec = [ (-1, -1), (-1, 0), @@ -248,7 +253,7 @@ mod tests { .map(|(dx, dy)| (dx, dy, 0)) .collect(); cells.push((0, 0, 1)); - let s = extract_surfaces(&voxel_map(&cells), 5, 3, 3); + let s = run(&cells, 5, 3, 3); assert!(!s.contains(&(0, 0, 0))); } } From 1385a91f63b4ea384661b5bd78e442a9b511eea2 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Mon, 1 Jun 2026 11:46:03 -0700 Subject: [PATCH 52/55] Clean up --- .../modules/mls_planner/rust/src/adjacency.rs | 43 ++++++++++--------- .../modules/mls_planner/rust/src/dijkstra.rs | 32 +++++++------- .../modules/mls_planner/rust/src/edges.rs | 10 ++--- .../modules/mls_planner/rust/src/nodes.rs | 14 +++--- .../modules/mls_planner/rust/src/surfaces.rs | 3 +- 5 files changed, 54 insertions(+), 48 deletions(-) diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/adjacency.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/adjacency.rs index 395c268cda..075403b267 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/adjacency.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/adjacency.rs @@ -19,12 +19,13 @@ pub type SurfaceLookup = AHashMap<(i32, i32), Vec>; pub type CellId = u32; pub const NO_CELL: CellId = u32::MAX; +/// Represent a deleted cell that can be reincarnated on an insertion. const TOMBSTONE: VoxelKey = (i32::MIN, i32::MIN, i32::MIN); const NEIGHBORS_4: [(i32, i32); 4] = [(-1, 0), (1, 0), (0, -1), (0, 1)]; #[derive(Clone, Copy, Debug)] pub struct Edge { - pub dst: CellId, + pub dest: CellId, pub cost: f32, } @@ -62,8 +63,10 @@ impl SurfaceCells { self.coord[id as usize] != TOMBSTONE } - /// Get-or-insert: O(1) amortized. Returns the CellId for `k`. - pub fn alloc(&mut self, k: VoxelKey) -> CellId { + /// Get or insert a new cell. + /// + /// Only expand the list if there are no available dead cells. + pub fn insert(&mut self, k: VoxelKey) -> CellId { debug_assert_ne!(k, TOMBSTONE, "voxel coord collides with tombstone sentinel"); if let Some(&id) = self.by_coord.get(&k) { return id; @@ -81,26 +84,29 @@ impl SurfaceCells { id } - /// Remove cell `k`, dropping all edges that reference it. CellIds of - /// other live cells are preserved. Required for incremental updates. + /// Remove a cell. + /// + /// Mark the cell as available with a tombstone and remove the output edges. #[allow(dead_code)] pub fn remove(&mut self, k: VoxelKey) -> Option { let id = self.by_coord.remove(&k)?; let outbound = std::mem::take(&mut self.edges[id as usize]); for e in &outbound { - let neigh = &mut self.edges[e.dst as usize]; - neigh.retain(|x| x.dst != id); + let neigh = &mut self.edges[e.dest as usize]; + neigh.retain(|x| x.dest != id); } self.coord[id as usize] = TOMBSTONE; self.free.push(id); Some(id) } + /// XYZ coord to cell ID. #[inline] pub fn id(&self, k: VoxelKey) -> Option { self.by_coord.get(&k).copied() } + /// Cell ID to XYZ coord. #[inline] pub fn coord(&self, id: CellId) -> VoxelKey { self.coord[id as usize] @@ -111,9 +117,9 @@ impl SurfaceCells { &self.edges[id as usize] } - #[allow(dead_code)] - pub fn add_edge(&mut self, src: CellId, dst: CellId, cost: f32) { - self.edges[src as usize].push(Edge { dst, cost }); + #[cfg(test)] + pub fn add_edge(&mut self, src: CellId, dest: CellId, cost: f32) { + self.edges[src as usize].push(Edge { dest, cost }); } /// Iterate live cells: (id, outgoing edges). @@ -165,11 +171,8 @@ pub fn build_surface_lookup(cells: &[VoxelKey], out: &mut SurfaceLookup) { } } -/// Populate `cells` with surface adjacency from the lookup. Existing -/// contents are dropped. CellIds are assigned in a deterministic column -/// order so debug logs and tests reproduce across runs. The edge pass is -/// parallel: each cell's outbound edges are computed independently by -/// reading the immutable coord array and by_coord map. +/// Populate cells with surface adjacency from the lookup. Deletes any +/// existing contents pub fn build_surface_cells( cells: &mut SurfaceCells, surface_lookup: &SurfaceLookup, @@ -182,7 +185,7 @@ pub fn build_surface_cells( keys.sort_unstable(); for &(ix, iy) in &keys { for &iz in &surface_lookup[&(ix, iy)] { - cells.alloc((ix, iy, iz)); + cells.insert((ix, iy, iz)); } } @@ -205,11 +208,11 @@ pub fn build_surface_cells( if dz.abs() > step_threshold_cells { continue; } - let dst = *by_coord + let dest = *by_coord .get(&(ix + dx, iy + dy, nz)) .expect("neighbor cell exists in lookup"); let cost = ((dx * dx + dy * dy + dz * dz) as f32).sqrt() * voxel_size; - local.push(Edge { dst, cost }); + local.push(Edge { dest, cost }); } } }); @@ -238,7 +241,7 @@ mod tests { let id = sc.id(k).expect("cell should exist"); sc.neighbors(id) .iter() - .map(|e| (sc.coord(e.dst), e.cost)) + .map(|e| (sc.coord(e.dest), e.cost)) .collect() } @@ -320,7 +323,7 @@ mod tests { fn alloc_after_remove_reuses_freed_slot() { let (_, mut sc) = build(&[(0, 0, 0), (1, 0, 0)]); let removed_id = sc.remove((1, 0, 0)).unwrap(); - let new_id = sc.alloc((5, 5, 0)); + let new_id = sc.insert((5, 5, 0)); assert_eq!(new_id, removed_id); assert_eq!(sc.coord(new_id), (5, 5, 0)); assert!(sc.is_live(new_id)); diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/dijkstra.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/dijkstra.rs index 28a11bccdd..73c45e7310 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/dijkstra.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/dijkstra.rs @@ -18,8 +18,7 @@ pub struct DijkstraState { } impl DijkstraState { - /// Resize parallel vecs to `n` slots and reset their contents to the - /// "unreached" sentinels. Capacity is preserved across calls. + /// Reset all vecs to the specified capacity. pub fn reset(&mut self, n: usize) { self.dist.clear(); self.dist.resize(n, f32::INFINITY); @@ -31,6 +30,9 @@ impl DijkstraState { } } +/// Multi-source dijkstra. +/// +/// Labels each node with distance to nearest source, the source id, and the path. pub fn dijkstra(cells: &SurfaceCells, sources: &[CellId], state: &mut DijkstraState) { state.reset(cells.slot_capacity()); @@ -51,20 +53,20 @@ pub fn dijkstra(cells: &SurfaceCells, sources: &[CellId], state: &mut DijkstraSt let su = state.source[u as usize]; for edge in cells.neighbors(u) { let nd = d + edge.cost; - let v = edge.dst as usize; + let v = edge.dest as usize; if nd < state.dist[v] { state.dist[v] = nd; state.pred[v] = u; state.source[v] = su; - state.heap.push(Scored(nd, edge.dst)); + state.heap.push(Scored(nd, edge.dest)); } } } } -/// Walk predecessors from `start` back to its source, returning the chain -/// inclusive of both ends. Returns just [start] for source cells and for -/// unreached cells. +/// Reconstruct the path back to the nearest source. +/// +/// Returns the start if the cell has not been reached by any dijkstra calls. pub fn walk_preds(state: &DijkstraState, start: CellId) -> Vec { let mut cells = vec![start]; let mut cur = start; @@ -106,7 +108,7 @@ mod tests { fn chain(n: i32) -> (SurfaceCells, Vec) { let mut sc = SurfaceCells::default(); - let ids: Vec = (0..n).map(|i| sc.alloc((i, 0, 0))).collect(); + let ids: Vec = (0..n).map(|i| sc.insert((i, 0, 0))).collect(); for i in 0..n - 1 { sc.add_edge(ids[i as usize], ids[(i + 1) as usize], 1.0); sc.add_edge(ids[(i + 1) as usize], ids[i as usize], 1.0); @@ -155,10 +157,10 @@ mod tests { #[test] fn disconnected_cells_stay_unreachable() { let mut sc = SurfaceCells::default(); - let a = sc.alloc((0, 0, 0)); - let b = sc.alloc((1, 0, 0)); - let c = sc.alloc((2, 0, 0)); - let d = sc.alloc((3, 0, 0)); + let a = sc.insert((0, 0, 0)); + let b = sc.insert((1, 0, 0)); + let c = sc.insert((2, 0, 0)); + let d = sc.insert((3, 0, 0)); sc.add_edge(a, b, 1.0); sc.add_edge(b, a, 1.0); sc.add_edge(c, d, 1.0); @@ -174,9 +176,9 @@ mod tests { #[test] fn shorter_path_overrides_longer() { let mut sc = SurfaceCells::default(); - let a = sc.alloc((0, 0, 0)); - let b = sc.alloc((1, 0, 0)); - let c = sc.alloc((2, 0, 0)); + let a = sc.insert((0, 0, 0)); + let b = sc.insert((1, 0, 0)); + let c = sc.insert((2, 0, 0)); sc.add_edge(a, b, 10.0); sc.add_edge(b, a, 10.0); sc.add_edge(a, c, 1.0); diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/edges.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/edges.rs index 91331f9b85..f7f0f2e21a 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/edges.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/edges.rs @@ -44,10 +44,10 @@ impl PlannerGraph { } } -/// Run multi-source Dijkstra from all node cells, then derive the cheapest -/// inter-node edge per Voronoi-region boundary pair. Reuses `state` as the -/// scratch buffer. The state is left holding the node-source Dijkstra -/// result so callers can walk preds for waypoint reconstruction. +/// Assemble the cheapest paths between neighboring source nodes. +/// +/// Runs multi-source dijkstra from the sources, then adds the cheapest edges +/// between Voronoi region boundaries. pub fn build_node_edges( cells: &SurfaceCells, nodes: &[NodeData], @@ -92,7 +92,7 @@ fn best_boundary_edges(cells: &SurfaceCells, state: &DijkstraState, out: &mut Ve } let sa = state.source[*u as usize]; for edge in *edges { - let v = edge.dst; + let v = edge.dest; let dv = state.dist[v as usize]; if !dv.is_finite() { continue; diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs index cbb1ba8d03..50c0463763 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs @@ -18,10 +18,10 @@ pub struct NodeData { pub pos: (f32, f32, f32), } -/// Run wall-distance Dijkstra, NMS-pick node cells, write them into -/// `out_nodes`, and apply wall-safe edge-cost scaling to `cells`. The -/// DijkstraState is left holding the wall-distance result so callers can -/// reuse it before the next Dijkstra overwrites it. +/// Distribute nodes on the surfaces. +/// +/// Runs multi source dijkstra using edges as sources, then distribute nodes +/// using a grid based NMS. pub fn place_nodes( cells: &mut SurfaceCells, voxel_size: f32, @@ -70,9 +70,11 @@ fn collect_wall_adjacent_cells(cells: &SurfaceCells, out: &mut Vec) { out.clear(); for (id, edges) in cells.iter() { let (cx, cy, _) = cells.coord(id); + + // Check if all 4 neighbors are present let mut mask: u8 = 0; for e in edges { - let (nx, ny, _) = cells.coord(e.dst); + let (nx, ny, _) = cells.coord(e.dest); mask |= match (nx - cx, ny - cy) { (-1, 0) => 1, (1, 0) => 2, @@ -150,7 +152,7 @@ fn apply_wall_safe_penalty(cells: &mut SurfaceCells, dist: &[f32], buffer_m: f32 edge_lists.par_iter_mut().for_each(|(src, edges)| { let pu = penalty_of(dist[*src as usize], buffer_m); for edge in edges.iter_mut() { - let pv = penalty_of(dist[edge.dst as usize], buffer_m); + let pv = penalty_of(dist[edge.dest as usize], buffer_m); edge.cost *= (pu + pv) / 2.0; } }); diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs index a6d387b051..4c92d14ac4 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs +++ b/dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs @@ -32,8 +32,7 @@ fn is_standable(ix: i32, iy: i32, iz: i32, by_col: &ColumnIz, clearance_cells: i } /// Extract standable cells from the voxelized global map, then close small -/// holes. `by_col` is a scratch buffer cleared on entry; its capacity is -/// reused across calls. Results land in `out` (cleared first). +/// holes. pub fn extract_surfaces( voxel_map: &AHashSet, clearance_cells: i32, From c9f28dda2fa5d6a7de303f79cda6d7b6f0c832b4 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Mon, 1 Jun 2026 12:17:28 -0700 Subject: [PATCH 53/55] Move to 3d nav --- .../navigation/nav_3d/evaluator/blueprints.py | 125 ++++++++++++++++++ .../evaluator/evaluator.py | 2 +- .../evaluator/mesh_loader.py | 0 .../evaluator/scenarios.py | 2 +- .../mls_planner/mls_planner_native.py | 6 +- .../mls_planner/rust/Cargo.lock | 0 .../mls_planner/rust/Cargo.toml | 2 +- .../mls_planner/rust/src/adjacency.rs | 0 .../mls_planner/rust/src/dijkstra.rs | 0 .../mls_planner/rust/src/edges.rs | 0 .../mls_planner/rust/src/main.rs | 18 +-- .../mls_planner/rust/src/nodes.rs | 0 .../mls_planner/rust/src/planner.rs | 0 .../mls_planner/rust/src/surfaces.rs | 0 .../mls_planner/rust/src/voxel.rs | 0 .../nav_stack/evaluator/blueprints.py | 68 ---------- .../evaluator/straight_line_planner.py | 93 ------------- .../click_start_goal_router.py | 19 +-- dimos/robot/all_blueprints.py | 7 +- 19 files changed, 154 insertions(+), 188 deletions(-) create mode 100644 dimos/navigation/nav_3d/evaluator/blueprints.py rename dimos/navigation/{nav_stack => nav_3d}/evaluator/evaluator.py (98%) rename dimos/navigation/{nav_stack => nav_3d}/evaluator/mesh_loader.py (100%) rename dimos/navigation/{nav_stack => nav_3d}/evaluator/scenarios.py (98%) rename dimos/navigation/{nav_stack/modules => nav_3d}/mls_planner/mls_planner_native.py (93%) rename dimos/navigation/{nav_stack/modules => nav_3d}/mls_planner/rust/Cargo.lock (100%) rename dimos/navigation/{nav_stack/modules => nav_3d}/mls_planner/rust/Cargo.toml (90%) rename dimos/navigation/{nav_stack/modules => nav_3d}/mls_planner/rust/src/adjacency.rs (100%) rename dimos/navigation/{nav_stack/modules => nav_3d}/mls_planner/rust/src/dijkstra.rs (100%) rename dimos/navigation/{nav_stack/modules => nav_3d}/mls_planner/rust/src/edges.rs (100%) rename dimos/navigation/{nav_stack/modules => nav_3d}/mls_planner/rust/src/main.rs (97%) rename dimos/navigation/{nav_stack/modules => nav_3d}/mls_planner/rust/src/nodes.rs (100%) rename dimos/navigation/{nav_stack/modules => nav_3d}/mls_planner/rust/src/planner.rs (100%) rename dimos/navigation/{nav_stack/modules => nav_3d}/mls_planner/rust/src/surfaces.rs (100%) rename dimos/navigation/{nav_stack/modules => nav_3d}/mls_planner/rust/src/voxel.rs (100%) delete mode 100644 dimos/navigation/nav_stack/evaluator/blueprints.py delete mode 100644 dimos/navigation/nav_stack/evaluator/straight_line_planner.py diff --git a/dimos/navigation/nav_3d/evaluator/blueprints.py b/dimos/navigation/nav_3d/evaluator/blueprints.py new file mode 100644 index 0000000000..150080dfd9 --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/blueprints.py @@ -0,0 +1,125 @@ +# Copyright 2026 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. + +"""Blueprint for the path-planner evaluator. + +Wires the Evaluator and MLSPlannerNative together and bridges all streams to rerun. +Run with:: + + dimos run path-planner-eval +""" + +from __future__ import annotations + +from typing import Any + +import numpy as np + +from dimos.core.coordination.blueprints import autoconnect +from dimos.navigation.nav_3d.evaluator.evaluator import Evaluator +from dimos.navigation.nav_3d.mls_planner.mls_planner_native import MLSPlannerNative +from dimos.navigation.nav_stack.modules.click_start_goal_router.click_start_goal_router import ( + ClickStartGoalRouter, +) +from dimos.visualization.rerun.bridge import RerunBridgeModule +from dimos.visualization.rerun.websocket_server import RerunWebSocketServer + +_POSE_MARKER_RADIUS = 0.4 +# Small lift so graph artifacts render visibly above the surface points instead of z-fighting. +_GRAPH_Z_LIFT = 0.05 + + +def _render_start_pose(msg: Any) -> Any: + import rerun as rr + + return rr.Points3D( + positions=[[msg.x, msg.y, msg.z]], + colors=[[0, 255, 0]], + radii=[_POSE_MARKER_RADIUS], + ) + + +def _render_goal_pose(msg: Any) -> Any: + import rerun as rr + + return rr.Points3D( + positions=[[msg.x, msg.y, msg.z]], + colors=[[255, 0, 0]], + radii=[_POSE_MARKER_RADIUS], + ) + + +def _render_global_map(msg: Any) -> Any: + return msg.to_rerun(voxel_size=0.03, colors=[128, 128, 128]) + + +def _render_surface_map(msg: Any) -> Any: + return msg.to_rerun(voxel_size=0.1, colors=[40, 75, 130]) + + +def _render_nodes(msg: Any) -> Any: + import rerun as rr + + pts, _ = msg.as_numpy() + if pts is None or len(pts) == 0: + return rr.Points3D([]) + pts = pts.copy() + pts[:, 2] += _GRAPH_Z_LIFT + return rr.Points3D(positions=pts, colors=[[75, 156, 211]], radii=[0.15]) + + +def _render_node_edges(msg: Any) -> Any: + """Color each segment by its safe-adj weight on a log-scale green->red gradient.""" + import rerun as rr + + if not msg._segments: + return rr.LineStrips3D([]) + weights = np.asarray(msg._traversability, dtype=np.float64) + log_w = np.log10(np.maximum(weights, 1e-6)) + lo, hi = float(log_w.min()), float(log_w.max()) + norm = (log_w - lo) / (hi - lo) if hi > lo else np.zeros_like(log_w) + r = (255 * norm).astype(np.uint8) + g = (255 * (1.0 - norm)).astype(np.uint8) + b = np.full_like(r, 60) + a = np.full_like(r, 220) + colors = np.column_stack([r, g, b, a]) + strips = [ + [ + [p1[0], p1[1], p1[2] + _GRAPH_Z_LIFT], + [p2[0], p2[1], p2[2] + _GRAPH_Z_LIFT], + ] + for p1, p2 in msg._segments + ] + return rr.LineStrips3D(strips, colors=colors, radii=[0.04] * len(strips)) + + +path_planner_eval = autoconnect( + Evaluator.blueprint(), + MLSPlannerNative.blueprint(), + ClickStartGoalRouter.blueprint(), + RerunWebSocketServer.blueprint(), + RerunBridgeModule.blueprint( + visual_override={ + "world/start_pose": _render_start_pose, + "world/goal_pose": _render_goal_pose, + "world/global_map": _render_global_map, + "world/surface_map": _render_surface_map, + "world/nodes": _render_nodes, + "world/node_edges": _render_node_edges, + } + ), +) + + +__all__ = ["path_planner_eval"] diff --git a/dimos/navigation/nav_stack/evaluator/evaluator.py b/dimos/navigation/nav_3d/evaluator/evaluator.py similarity index 98% rename from dimos/navigation/nav_stack/evaluator/evaluator.py rename to dimos/navigation/nav_3d/evaluator/evaluator.py index 8b33e55ec3..502bf1a7a6 100644 --- a/dimos/navigation/nav_stack/evaluator/evaluator.py +++ b/dimos/navigation/nav_3d/evaluator/evaluator.py @@ -31,7 +31,7 @@ from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.nav_msgs.Path import Path from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.navigation.nav_stack.evaluator.scenarios import ( +from dimos.navigation.nav_3d.evaluator.scenarios import ( PlannerScenario, default_scenarios, ) diff --git a/dimos/navigation/nav_stack/evaluator/mesh_loader.py b/dimos/navigation/nav_3d/evaluator/mesh_loader.py similarity index 100% rename from dimos/navigation/nav_stack/evaluator/mesh_loader.py rename to dimos/navigation/nav_3d/evaluator/mesh_loader.py diff --git a/dimos/navigation/nav_stack/evaluator/scenarios.py b/dimos/navigation/nav_3d/evaluator/scenarios.py similarity index 98% rename from dimos/navigation/nav_stack/evaluator/scenarios.py rename to dimos/navigation/nav_3d/evaluator/scenarios.py index 0173a273a8..992c7c5104 100644 --- a/dimos/navigation/nav_stack/evaluator/scenarios.py +++ b/dimos/navigation/nav_3d/evaluator/scenarios.py @@ -24,7 +24,7 @@ from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.navigation.nav_stack.evaluator.mesh_loader import load_voxelized_mesh +from dimos.navigation.nav_3d.evaluator.mesh_loader import load_voxelized_mesh from dimos.utils.logging_config import setup_logger logger = setup_logger() diff --git a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner_native.py b/dimos/navigation/nav_3d/mls_planner/mls_planner_native.py similarity index 93% rename from dimos/navigation/nav_stack/modules/mls_planner/mls_planner_native.py rename to dimos/navigation/nav_3d/mls_planner/mls_planner_native.py index da61e3706d..1a435fa0cb 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/mls_planner_native.py +++ b/dimos/navigation/nav_3d/mls_planner/mls_planner_native.py @@ -18,8 +18,8 @@ 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.LineSegments3D import LineSegments3D -from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.nav_msgs.Path import Path from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 @@ -47,8 +47,8 @@ class MLSPlannerNative(NativeModule): config: MLSPlannerNativeConfig global_map: In[PointCloud2] - start_pose: In[Odometry] - goal_pose: In[Odometry] + start_pose: In[PoseStamped] + goal_pose: In[PoseStamped] path: Out[Path] surface_map: Out[PointCloud2] diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/Cargo.lock b/dimos/navigation/nav_3d/mls_planner/rust/Cargo.lock similarity index 100% rename from dimos/navigation/nav_stack/modules/mls_planner/rust/Cargo.lock rename to dimos/navigation/nav_3d/mls_planner/rust/Cargo.lock diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/Cargo.toml b/dimos/navigation/nav_3d/mls_planner/rust/Cargo.toml similarity index 90% rename from dimos/navigation/nav_stack/modules/mls_planner/rust/Cargo.toml rename to dimos/navigation/nav_3d/mls_planner/rust/Cargo.toml index 71744de8fd..aae1496381 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/Cargo.toml +++ b/dimos/navigation/nav_3d/mls_planner/rust/Cargo.toml @@ -10,7 +10,7 @@ name = "mls_planner" path = "src/main.rs" [dependencies] -dimos-module = { path = "../../../../../../native/rust/dimos-module" } +dimos-module = { path = "../../../../../native/rust/dimos-module" } lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen" } tokio = { version = "1", features = ["rt-multi-thread", "macros", "signal"] } serde = { version = "1", features = ["derive"] } diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/adjacency.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/adjacency.rs similarity index 100% rename from dimos/navigation/nav_stack/modules/mls_planner/rust/src/adjacency.rs rename to dimos/navigation/nav_3d/mls_planner/rust/src/adjacency.rs diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/dijkstra.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/dijkstra.rs similarity index 100% rename from dimos/navigation/nav_stack/modules/mls_planner/rust/src/dijkstra.rs rename to dimos/navigation/nav_3d/mls_planner/rust/src/dijkstra.rs diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/edges.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/edges.rs similarity index 100% rename from dimos/navigation/nav_stack/modules/mls_planner/rust/src/edges.rs rename to dimos/navigation/nav_3d/mls_planner/rust/src/edges.rs diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/main.rs similarity index 97% rename from dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs rename to dimos/navigation/nav_3d/mls_planner/rust/src/main.rs index eead046324..e2ba1281e9 100644 --- a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/main.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/main.rs @@ -13,7 +13,7 @@ use std::time::{Duration, Instant, SystemTime, UNIX_EPOCH}; use dimos_module::{error_throttled, run, warn_throttled, Input, LcmTransport, Module, Output}; use lcm_msgs::geometry_msgs::{Point, Pose, PoseStamped, Quaternion}; -use lcm_msgs::nav_msgs::{Odometry, Path}; +use lcm_msgs::nav_msgs::Path; use lcm_msgs::sensor_msgs::{PointCloud2, PointField}; use lcm_msgs::std_msgs::{Header, Time}; use serde::Deserialize; @@ -46,11 +46,11 @@ struct MlsPlanner { #[input(decode = PointCloud2::decode, handler = on_global_map)] global_map: Input, - #[input(decode = Odometry::decode, handler = on_start_pose)] - start_pose: Input, + #[input(decode = PoseStamped::decode, handler = on_start_pose)] + start_pose: Input, - #[input(decode = Odometry::decode, handler = on_goal_pose)] - goal_pose: Input, + #[input(decode = PoseStamped::decode, handler = on_goal_pose)] + goal_pose: Input, #[output(encode = PointCloud2::encode)] surface_map: Output, @@ -197,15 +197,15 @@ impl MlsPlanner { ); } - async fn on_start_pose(&mut self, msg: Odometry) { - let p = &msg.pose.pose.position; + async fn on_start_pose(&mut self, msg: PoseStamped) { + let p = &msg.pose.position; self.latest_start = Some((p.x as f32, p.y as f32, p.z as f32)); // Drop any previous plan so the visualizer doesn't show a stale path // rooted at the old start. publish_path(&self.path, &empty_path(&self.config.world_frame, now())).await; } - async fn on_goal_pose(&mut self, msg: Odometry) { + async fn on_goal_pose(&mut self, msg: PoseStamped) { let Some(start) = self.latest_start else { tracing::warn!("MLSPlanner received goal before start; skipping"); return; @@ -219,7 +219,7 @@ impl MlsPlanner { return; } - let p = &msg.pose.pose.position; + let p = &msg.pose.position; let goal = (p.x as f32, p.y as f32, p.z as f32); let t_plan = Instant::now(); diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs similarity index 100% rename from dimos/navigation/nav_stack/modules/mls_planner/rust/src/nodes.rs rename to dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/planner.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs similarity index 100% rename from dimos/navigation/nav_stack/modules/mls_planner/rust/src/planner.rs rename to dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/surfaces.rs similarity index 100% rename from dimos/navigation/nav_stack/modules/mls_planner/rust/src/surfaces.rs rename to dimos/navigation/nav_3d/mls_planner/rust/src/surfaces.rs diff --git a/dimos/navigation/nav_stack/modules/mls_planner/rust/src/voxel.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/voxel.rs similarity index 100% rename from dimos/navigation/nav_stack/modules/mls_planner/rust/src/voxel.rs rename to dimos/navigation/nav_3d/mls_planner/rust/src/voxel.rs diff --git a/dimos/navigation/nav_stack/evaluator/blueprints.py b/dimos/navigation/nav_stack/evaluator/blueprints.py deleted file mode 100644 index 7eda8c33df..0000000000 --- a/dimos/navigation/nav_stack/evaluator/blueprints.py +++ /dev/null @@ -1,68 +0,0 @@ -# Copyright 2026 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. - -"""Simulate various inputs and outputs to test path planners. - -dimos run path-planner-eval -""" - -from __future__ import annotations - -from typing import TYPE_CHECKING - -from dimos.core.coordination.blueprints import autoconnect -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.navigation.nav_stack.evaluator.evaluator import Evaluator -from dimos.navigation.nav_stack.evaluator.straight_line_planner import StraightLinePlanner -from dimos.visualization.rerun.bridge import RerunBridgeModule - -if TYPE_CHECKING: - from rerun._baseclasses import Archetype - -_POSE_MARKER_RADIUS = 0.4 - - -def _render_start_pose(msg: PoseStamped) -> Archetype: - import rerun as rr - - return rr.Points3D( - positions=[[msg.x, msg.y, msg.z]], - colors=[[0, 255, 0]], - radii=[_POSE_MARKER_RADIUS], - ) - - -def _render_goal_pose(msg: PoseStamped) -> Archetype: - import rerun as rr - - return rr.Points3D( - positions=[[msg.x, msg.y, msg.z]], - colors=[[255, 0, 0]], - radii=[_POSE_MARKER_RADIUS], - ) - - -path_planner_eval = autoconnect( - Evaluator.blueprint(), - StraightLinePlanner.blueprint(), - RerunBridgeModule.blueprint( - visual_override={ - "world/start_pose": _render_start_pose, - "world/goal_pose": _render_goal_pose, - } - ), -) - - -__all__ = ["path_planner_eval"] diff --git a/dimos/navigation/nav_stack/evaluator/straight_line_planner.py b/dimos/navigation/nav_stack/evaluator/straight_line_planner.py deleted file mode 100644 index 29e5815fbc..0000000000 --- a/dimos/navigation/nav_stack/evaluator/straight_line_planner.py +++ /dev/null @@ -1,93 +0,0 @@ -# Copyright 2026 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. - -"""Sanity check modules for the eval framework. - -Just outputs a path from start to goal ignoring map. -""" - -from __future__ import annotations - -import time -from typing import Any - -import numpy as np - -from dimos.core.module import Module, ModuleConfig -from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.nav_msgs.Path import Path -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.utils.logging_config import setup_logger - -logger = setup_logger() - - -class StraightLinePlannerConfig(ModuleConfig): - world_frame: str = "map" - num_waypoints: int = 20 - - -class StraightLinePlanner(Module): - """Emits a straight-line Path from start to goal. Ignores the map.""" - - config: StraightLinePlannerConfig - - global_map: In[PointCloud2] - start_pose: In[PoseStamped] - goal_pose: In[PoseStamped] - path: Out[Path] - - def __init__(self, **kwargs: Any) -> None: - super().__init__(**kwargs) - self._latest_start: PoseStamped | None = None - - async def handle_global_map(self, _msg: PointCloud2) -> None: - return - - async def handle_start_pose(self, msg: PoseStamped) -> None: - self._latest_start = msg - - async def handle_goal_pose(self, msg: PoseStamped) -> None: - start = self._latest_start - if start is None: - logger.warning("StraightLinePlanner received goal before start; skipping") - return - path = self._straight_line(start, msg) - self.path.publish(path) - - def _straight_line(self, start: PoseStamped, goal: PoseStamped) -> Path: - n = max(2, self.config.num_waypoints) - sx, sy, sz = start.x, start.y, start.z - gx, gy, gz = goal.x, goal.y, goal.z - xs = np.linspace(sx, gx, n) - ys = np.linspace(sy, gy, n) - zs = np.linspace(sz, gz, n) - orient = [ - start.orientation.x, - start.orientation.y, - start.orientation.z, - start.orientation.w, - ] - now = time.time() - poses = [ - PoseStamped( - ts=now, - frame_id=self.config.world_frame, - position=[float(x), float(y), float(z)], - orientation=orient, - ) - for x, y, z in zip(xs, ys, zs, strict=True) - ] - return Path(ts=now, frame_id=self.config.world_frame, poses=poses) diff --git a/dimos/navigation/nav_stack/modules/click_start_goal_router/click_start_goal_router.py b/dimos/navigation/nav_stack/modules/click_start_goal_router/click_start_goal_router.py index 146769492e..253d3b909a 100644 --- a/dimos/navigation/nav_stack/modules/click_start_goal_router/click_start_goal_router.py +++ b/dimos/navigation/nav_stack/modules/click_start_goal_router/click_start_goal_router.py @@ -21,8 +21,7 @@ from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out from dimos.msgs.geometry_msgs.PointStamped import PointStamped -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -38,21 +37,25 @@ class ClickStartGoalRouter(Module): config: ClickStartGoalRouterConfig clicked_point: In[PointStamped] - start_pose: Out[Odometry] - goal_pose: Out[Odometry] + start_pose: Out[PoseStamped] + goal_pose: Out[PoseStamped] def __init__(self, **kwargs: Any) -> None: super().__init__(**kwargs) self._next_is_start: bool = True async def handle_clicked_point(self, msg: PointStamped) -> None: - pose = Pose(position=[msg.x, msg.y, msg.z], orientation=[0.0, 0.0, 0.0, 1.0]) - odom = Odometry(ts=msg.ts, frame_id=self.config.world_frame, pose=pose) + pose = PoseStamped( + ts=msg.ts, + frame_id=self.config.world_frame, + position=[msg.x, msg.y, msg.z], + orientation=[0.0, 0.0, 0.0, 1.0], + ) if self._next_is_start: self._next_is_start = False logger.info("Click set start; next click will set goal", x=msg.x, y=msg.y, z=msg.z) - self.start_pose.publish(odom) + self.start_pose.publish(pose) return self._next_is_start = True logger.info("Click set goal", x=msg.x, y=msg.y, z=msg.z) - self.goal_pose.publish(odom) + self.goal_pose.publish(pose) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 05c52f9f10..f6cf378bfd 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -71,7 +71,7 @@ "mid360-fastlio-voxels-native": "dimos.hardware.sensors.lidar.fastlio2.fastlio_blueprints:mid360_fastlio_voxels_native", "openarm-mock-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints:openarm_mock_planner_coordinator", "openarm-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints:openarm_planner_coordinator", - "path-planner-eval": "dimos.navigation.nav_stack.evaluator.blueprints:path_planner_eval", + "path-planner-eval": "dimos.navigation.nav_3d.evaluator.blueprints:path_planner_eval", "teleop-phone": "dimos.teleop.phone.blueprints:teleop_phone", "teleop-phone-go2": "dimos.teleop.phone.blueprints:teleop_phone_go2", "teleop-phone-go2-fleet": "dimos.teleop.phone.blueprints:teleop_phone_go2_fleet", @@ -144,7 +144,7 @@ "drone-tracking-module": "dimos.robot.drone.drone_tracking_module.DroneTrackingModule", "embedding-memory": "dimos.memory.embedding.EmbeddingMemory", "emitter-module": "dimos.utils.demo_image_encoding.EmitterModule", - "evaluator": "dimos.navigation.nav_stack.evaluator.evaluator.Evaluator", + "evaluator": "dimos.navigation.nav_3d.evaluator.evaluator.Evaluator", "far-planner": "dimos.navigation.nav_stack.modules.far_planner.far_planner.FarPlanner", "fast-lio2": "dimos.hardware.sensors.lidar.fastlio2.module.FastLio2", "g1-connection": "dimos.robot.unitree.g1.connection.G1Connection", @@ -173,7 +173,7 @@ "mcp-client": "dimos.agents.mcp.mcp_client.McpClient", "mcp-server": "dimos.agents.mcp.mcp_server.McpServer", "memory-module": "dimos.memory2.module.MemoryModule", - "mls-planner-native": "dimos.navigation.nav_stack.modules.mls_planner.mls_planner_native.MLSPlannerNative", + "mls-planner-native": "dimos.navigation.nav_3d.mls_planner.mls_planner_native.MLSPlannerNative", "mock-b1-connection-module": "dimos.robot.unitree.b1.connection.MockB1ConnectionModule", "module-a": "dimos.robot.unitree.demo_error_on_name_conflicts.ModuleA", "module-b": "dimos.robot.unitree.demo_error_on_name_conflicts.ModuleB", @@ -211,7 +211,6 @@ "simple-planner": "dimos.navigation.nav_stack.modules.simple_planner.simple_planner.SimplePlanner", "spatial-memory": "dimos.perception.spatial_perception.SpatialMemory", "speak-skill": "dimos.agents.skills.speak_skill.SpeakSkill", - "straight-line-planner": "dimos.navigation.nav_stack.evaluator.straight_line_planner.StraightLinePlanner", "tare-planner": "dimos.navigation.nav_stack.modules.tare_planner.tare_planner.TarePlanner", "temporal-memory": "dimos.perception.experimental.temporal_memory.temporal_memory.TemporalMemory", "terrain-analysis": "dimos.navigation.nav_stack.modules.terrain_analysis.terrain_analysis.TerrainAnalysis", From 13536d77fbad112d2353d209643de90ce53d2713 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Mon, 1 Jun 2026 12:47:40 -0700 Subject: [PATCH 54/55] Type maxing --- .../navigation/nav_3d/evaluator/blueprints.py | 27 ++++++++---------- .../nav_3d/mls_planner/rust/src/adjacency.rs | 15 ++++------ .../nav_3d/mls_planner/rust/src/dijkstra.rs | 2 +- .../nav_3d/mls_planner/rust/src/edges.rs | 28 +++++++++++-------- .../nav_3d/mls_planner/rust/src/planner.rs | 24 ++++++++-------- 5 files changed, 47 insertions(+), 49 deletions(-) diff --git a/dimos/navigation/nav_3d/evaluator/blueprints.py b/dimos/navigation/nav_3d/evaluator/blueprints.py index 150080dfd9..97200c98fc 100644 --- a/dimos/navigation/nav_3d/evaluator/blueprints.py +++ b/dimos/navigation/nav_3d/evaluator/blueprints.py @@ -22,11 +22,14 @@ from __future__ import annotations -from typing import Any - import numpy as np +import rerun as rr +from rerun._baseclasses import Archetype from dimos.core.coordination.blueprints import autoconnect +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.nav_msgs.LineSegments3D import LineSegments3D +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.navigation.nav_3d.evaluator.evaluator import Evaluator from dimos.navigation.nav_3d.mls_planner.mls_planner_native import MLSPlannerNative from dimos.navigation.nav_stack.modules.click_start_goal_router.click_start_goal_router import ( @@ -40,9 +43,7 @@ _GRAPH_Z_LIFT = 0.05 -def _render_start_pose(msg: Any) -> Any: - import rerun as rr - +def _render_start_pose(msg: PoseStamped) -> Archetype: return rr.Points3D( positions=[[msg.x, msg.y, msg.z]], colors=[[0, 255, 0]], @@ -50,9 +51,7 @@ def _render_start_pose(msg: Any) -> Any: ) -def _render_goal_pose(msg: Any) -> Any: - import rerun as rr - +def _render_goal_pose(msg: PoseStamped) -> Archetype: return rr.Points3D( positions=[[msg.x, msg.y, msg.z]], colors=[[255, 0, 0]], @@ -60,17 +59,15 @@ def _render_goal_pose(msg: Any) -> Any: ) -def _render_global_map(msg: Any) -> Any: +def _render_global_map(msg: PointCloud2) -> Archetype: return msg.to_rerun(voxel_size=0.03, colors=[128, 128, 128]) -def _render_surface_map(msg: Any) -> Any: +def _render_surface_map(msg: PointCloud2) -> Archetype: return msg.to_rerun(voxel_size=0.1, colors=[40, 75, 130]) -def _render_nodes(msg: Any) -> Any: - import rerun as rr - +def _render_nodes(msg: PointCloud2) -> Archetype: pts, _ = msg.as_numpy() if pts is None or len(pts) == 0: return rr.Points3D([]) @@ -79,10 +76,8 @@ def _render_nodes(msg: Any) -> Any: return rr.Points3D(positions=pts, colors=[[75, 156, 211]], radii=[0.15]) -def _render_node_edges(msg: Any) -> Any: +def _render_node_edges(msg: LineSegments3D) -> Archetype: """Color each segment by its safe-adj weight on a log-scale green->red gradient.""" - import rerun as rr - if not msg._segments: return rr.LineStrips3D([]) weights = np.asarray(msg._traversability, dtype=np.float64) diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/adjacency.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/adjacency.rs index 075403b267..a4408acae7 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/adjacency.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/adjacency.rs @@ -1,13 +1,10 @@ // Copyright 2026 Dimensional Inc. // SPDX-License-Identifier: Apache-2.0 -//! Surface cells indexed by dense CellId. Hot-path Dijkstra / NMS / edge work -//! operates on CellId; VoxelKey is only translated at the boundary with the -//! voxel map and at output (publishing, waypoint emission). +//! Surface cells indexed by dense CellId. //! -//! The structure is a slot map. Inserting allocates a fresh id (or recycles -//! a freed one). Deleting tombstones the slot and walks the cell's neighbors -//! to drop back-edges, leaving every surviving cell's CellId unchanged. +//! Uses a "slot map" to store cells. When inserting, either expand the map +//! or reuse a freed location marked with a tombstone. use ahash::AHashMap; use rayon::prelude::*; @@ -16,6 +13,7 @@ use crate::voxel::VoxelKey; pub type SurfaceLookup = AHashMap<(i32, i32), Vec>; +/// Index of surface voxel pub type CellId = u32; pub const NO_CELL: CellId = u32::MAX; @@ -42,13 +40,12 @@ impl SurfaceCells { self.by_coord.is_empty() } - /// Total slot count, including tombstoned ones. Use as the size for - /// CellId-indexed scratch buffers. + /// Total slot count, including tombstoned cells. pub fn slot_capacity(&self) -> usize { self.coord.len() } - /// Drop all identity and edges while keeping allocation capacity. + /// Clear all vecs but keeps space allocated. pub fn clear(&mut self) { self.coord.clear(); self.by_coord.clear(); diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/dijkstra.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/dijkstra.rs index 73c45e7310..59a2890c89 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/dijkstra.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/dijkstra.rs @@ -96,7 +96,7 @@ impl PartialOrd for Scored { } impl Ord for Scored { fn cmp(&self, other: &Self) -> Ordering { - // Min-heap on f32 score with CellId tie-break for determinism. + // Order on score, and use cell id for tie-breaker for repeatability other.0.total_cmp(&self.0).then(self.1.cmp(&other.1)) } } diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/edges.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/edges.rs index f7f0f2e21a..8884b96af0 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/edges.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/edges.rs @@ -16,10 +16,17 @@ use crate::dijkstra::{dijkstra, walk_preds, DijkstraState}; use crate::nodes::NodeData; use crate::voxel::VoxelKey; +/// Index into planner graph nodes +pub type NodeId = u32; +pub const NO_NODE: NodeId = u32::MAX; + +/// Index into planner graph node edges +pub type NodeEdgeIdx = u32; + #[derive(Clone, Copy, Debug)] pub struct NodeEdge { - pub a: u32, - pub b: u32, + pub a: NodeId, + pub b: NodeId, pub cost: f32, /// Cell on a's side of the cheapest Voronoi boundary crossing. pub boundary_u: CellId, @@ -27,14 +34,13 @@ pub struct NodeEdge { pub boundary_v: CellId, } -/// Long-lived planner graph. Buffers are reused across rebuilds. #[derive(Default)] pub struct PlannerGraph { pub cells: SurfaceCells, pub surface_lookup: SurfaceLookup, pub nodes: Vec, pub node_edges: Vec, - pub node_adj: Vec>, + pub node_adj: Vec>, pub cell_state: DijkstraState, } @@ -53,7 +59,7 @@ pub fn build_node_edges( nodes: &[NodeData], state: &mut DijkstraState, out_edges: &mut Vec, - out_adj: &mut Vec>, + out_adj: &mut Vec>, ) { out_edges.clear(); out_adj.clear(); @@ -73,18 +79,18 @@ pub fn build_node_edges( v.clear(); } for (edge_idx, edge) in out_edges.iter().enumerate() { - out_adj[edge.a as usize].push(edge_idx as u32); - out_adj[edge.b as usize].push(edge_idx as u32); + out_adj[edge.a as usize].push(edge_idx as NodeEdgeIdx); + out_adj[edge.b as usize].push(edge_idx as NodeEdgeIdx); } } fn best_boundary_edges(cells: &SurfaceCells, state: &DijkstraState, out: &mut Vec) { let cell_entries: Vec<(CellId, &[Edge])> = cells.iter().collect(); - let merged: AHashMap<(u32, u32), NodeEdge> = cell_entries + let merged: AHashMap<(NodeId, NodeId), NodeEdge> = cell_entries .par_iter() .fold( - AHashMap::<(u32, u32), NodeEdge>::new, + AHashMap::<(NodeId, NodeId), NodeEdge>::new, |mut local, (u, edges)| { let du = state.dist[*u as usize]; if !du.is_finite() { @@ -125,7 +131,7 @@ fn best_boundary_edges(cells: &SurfaceCells, state: &DijkstraState, out: &mut Ve local }, ) - .reduce(AHashMap::<(u32, u32), NodeEdge>::new, |mut a, b| { + .reduce(AHashMap::<(NodeId, NodeId), NodeEdge>::new, |mut a, b| { for (k, v_edge) in b { let entry = a.entry(k).or_insert(v_edge); if v_edge.cost < entry.cost { @@ -213,7 +219,7 @@ mod tests { #[test] fn three_nodes_in_line_form_a_chain() { let pg = setup(&strip_cells(), &[(3, 0, 0), (10, 0, 0), (17, 0, 0)]); - let pairs: Vec<(u32, u32)> = pg.node_edges.iter().map(|e| (e.a, e.b)).collect(); + let pairs: Vec<(NodeId, NodeId)> = pg.node_edges.iter().map(|e| (e.a, e.b)).collect(); assert_eq!(pairs, vec![(0, 1), (1, 2)]); } diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs index fdc6e8ba2c..e3bb8e818a 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs @@ -8,7 +8,7 @@ use ahash::AHashMap; use crate::adjacency::{CellId, SurfaceLookup}; use crate::dijkstra::walk_preds; -use crate::edges::PlannerGraph; +use crate::edges::{NodeEdgeIdx, NodeId, PlannerGraph, NO_NODE}; use crate::voxel::{surface_point_xyz, VoxelKey}; /// Snap a pose to the best surface cell. @@ -86,11 +86,11 @@ pub fn plan( let start_cell = plg.cells.id(start_coord)?; let goal_cell = plg.cells.id(goal_coord)?; - let node_idx_by_cell: AHashMap = plg + let node_idx_by_cell: AHashMap = plg .nodes .iter() .enumerate() - .map(|(i, n)| (n.cell_id, i as u32)) + .map(|(i, n)| (n.cell_id, i as NodeId)) .collect(); let start_segment = walk_preds(&plg.cell_state, start_cell); @@ -110,13 +110,13 @@ pub fn plan( )) } -pub fn shortest_path_nodes(plg: &PlannerGraph, start: u32, goal: u32) -> Option> { +pub fn shortest_path_nodes(plg: &PlannerGraph, start: NodeId, goal: NodeId) -> Option> { if start == goal { return Some(vec![start]); } let n = plg.nodes.len(); let mut dist = vec![f32::INFINITY; n]; - let mut pred = vec![-1i32; n]; + let mut pred = vec![NO_NODE; n]; dist[start as usize] = 0.0; let mut heap: BinaryHeap = BinaryHeap::new(); heap.push(Scored(0.0, start)); @@ -134,7 +134,7 @@ pub fn shortest_path_nodes(plg: &PlannerGraph, start: u32, goal: u32) -> Option< let nd = d + edge.cost; if nd < dist[neighbor as usize] { dist[neighbor as usize] = nd; - pred[neighbor as usize] = u as i32; + pred[neighbor as usize] = u; heap.push(Scored(nd, neighbor)); } } @@ -144,10 +144,10 @@ pub fn shortest_path_nodes(plg: &PlannerGraph, start: u32, goal: u32) -> Option< return None; } let mut path = vec![goal]; - let mut cur = goal as i32; - while pred[cur as usize] >= 0 { + let mut cur = goal; + while pred[cur as usize] != NO_NODE { cur = pred[cur as usize]; - path.push(cur as u32); + path.push(cur); } path.reverse(); Some(path) @@ -155,7 +155,7 @@ pub fn shortest_path_nodes(plg: &PlannerGraph, start: u32, goal: u32) -> Option< fn assemble_waypoints( plg: &PlannerGraph, - node_seq: &[u32], + node_seq: &[NodeId], start_pose: (f32, f32, f32), start_segment: &[CellId], goal_pose: (f32, f32, f32), @@ -203,7 +203,7 @@ fn assemble_waypoints( waypoints } -fn edge_between(plg: &PlannerGraph, a: u32, b: u32) -> Option { +fn edge_between(plg: &PlannerGraph, a: NodeId, b: NodeId) -> Option { for &edge_idx in &plg.node_adj[a as usize] { let edge = &plg.node_edges[edge_idx as usize]; let other = if edge.a == a { edge.b } else { edge.a }; @@ -214,7 +214,7 @@ fn edge_between(plg: &PlannerGraph, a: u32, b: u32) -> Option { None } -struct Scored(f32, u32); +struct Scored(f32, NodeId); impl PartialEq for Scored { fn eq(&self, other: &Self) -> bool { From 4a42f956965bd90e97f7f14b25f3aa63e43a6bb1 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Mon, 1 Jun 2026 13:11:03 -0700 Subject: [PATCH 55/55] Fix --- dimos/navigation/nav_3d/mls_planner/rust/src/main.rs | 2 +- dimos/navigation/nav_3d/mls_planner/rust/src/surfaces.rs | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/main.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/main.rs index e2ba1281e9..e824dd64fc 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/main.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/main.rs @@ -295,7 +295,7 @@ fn now() -> Time { .duration_since(UNIX_EPOCH) .unwrap_or_default(); Time { - sec: dur.as_secs() as i32, + sec: dur.as_secs().min(i32::MAX as u64) as i32, nsec: dur.subsec_nanos() as i32, } } diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/surfaces.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/surfaces.rs index 4c92d14ac4..7451b959c4 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/surfaces.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/surfaces.rs @@ -149,10 +149,10 @@ fn close_at_z( } if dilation_passes > 0 { - img = dilate(&img, Norm::L1, dilation_passes as u8); + img = dilate(&img, Norm::L1, dilation_passes.min(u8::MAX as u32) as u8); } if erosion_passes > 0 { - img = erode(&img, Norm::L1, erosion_passes as u8); + img = erode(&img, Norm::L1, erosion_passes.min(u8::MAX as u32) as u8); } let mut out = Vec::new();