diff --git a/.gitignore b/.gitignore index ffc1c4f31f..d7f4b78a77 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/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/dimos/navigation/nav_3d/evaluator/blueprints.py b/dimos/navigation/nav_3d/evaluator/blueprints.py new file mode 100644 index 0000000000..97200c98fc --- /dev/null +++ b/dimos/navigation/nav_3d/evaluator/blueprints.py @@ -0,0 +1,120 @@ +# 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 + +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 ( + 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: PoseStamped) -> Archetype: + 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: + return rr.Points3D( + positions=[[msg.x, msg.y, msg.z]], + colors=[[255, 0, 0]], + radii=[_POSE_MARKER_RADIUS], + ) + + +def _render_global_map(msg: PointCloud2) -> Archetype: + return msg.to_rerun(voxel_size=0.03, colors=[128, 128, 128]) + + +def _render_surface_map(msg: PointCloud2) -> Archetype: + return msg.to_rerun(voxel_size=0.1, colors=[40, 75, 130]) + + +def _render_nodes(msg: PointCloud2) -> Archetype: + 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: LineSegments3D) -> Archetype: + """Color each segment by its safe-adj weight on a log-scale green->red gradient.""" + 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_3d/mls_planner/mls_planner_native.py b/dimos/navigation/nav_3d/mls_planner/mls_planner_native.py new file mode 100644 index 0000000000..1a435fa0cb --- /dev/null +++ b/dimos/navigation/nav_3d/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.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.nav_msgs.LineSegments3D import LineSegments3D +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[PoseStamped] + goal_pose: In[PoseStamped] + + path: Out[Path] + surface_map: Out[PointCloud2] + nodes: Out[PointCloud2] + node_edges: Out[LineSegments3D] diff --git a/dimos/navigation/nav_3d/mls_planner/rust/Cargo.lock b/dimos/navigation/nav_3d/mls_planner/rust/Cargo.lock new file mode 100644 index 0000000000..bb89daed36 --- /dev/null +++ b/dimos/navigation/nav_3d/mls_planner/rust/Cargo.lock @@ -0,0 +1,1073 @@ +# This file is automatically @generated by Cargo. +# 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" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5a15f179cd60c4584b8a8c596927aadc462e27f2ca70c04e0071964a73ba7a75" +dependencies = [ + "cfg-if", + "getrandom 0.3.4", + "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 = "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" +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 = "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" +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", + "image", + "imageproc", + "lcm-msgs", + "rayon", + "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 = "either" +version = "1.16.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "91622ff5e7162018101f2fea40d6ebf4a78bbe5a49736a2020649edf9693679e" + +[[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 = "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" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "899def5c37c4fd7b2664648c28120ecec138e4d395b459e5ca34f9cce2dd77fd" +dependencies = [ + "cfg-if", + "libc", + "r-efi", + "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" +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 = "libm" +version = "0.2.16" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b6d2cec3eae94f9f509c767b45932f1ada8350c4bdb85af2fcab4a3c14807981" + +[[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 = "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" +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 = "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" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7957b9740744892f114936ab4a57b3f487491bbeafaf8083688b16841a4240e5" +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" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8fd00f0bb2e90d81d1044c2b32617f68fcb9fa3bb7640c23e9c748e53fb30934" +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" +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 = "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 = "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" +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 = "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" +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 = "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" +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 = "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" +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 = "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" +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_3d/mls_planner/rust/Cargo.toml b/dimos/navigation/nav_3d/mls_planner/rust/Cargo.toml new file mode 100644 index 0000000000..aae1496381 --- /dev/null +++ b/dimos/navigation/nav_3d/mls_planner/rust/Cargo.toml @@ -0,0 +1,25 @@ +[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" +image = { version = "0.25", default-features = false } +imageproc = { version = "0.25", default-features = false } +rayon = "1" + +[profile.release] +lto = "thin" +codegen-units = 1 diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/adjacency.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/adjacency.rs new file mode 100644 index 0000000000..a4408acae7 --- /dev/null +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/adjacency.rs @@ -0,0 +1,336 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 + +//! Surface cells indexed by dense CellId. +//! +//! 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::*; + +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; + +/// 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 dest: CellId, + pub cost: f32, +} + +#[derive(Default)] +pub struct SurfaceCells { + coord: Vec, + edges: Vec>, + by_coord: AHashMap, + free: Vec, +} + +impl SurfaceCells { + pub fn is_empty(&self) -> bool { + self.by_coord.is_empty() + } + + /// Total slot count, including tombstoned cells. + pub fn slot_capacity(&self) -> usize { + self.coord.len() + } + + /// Clear all vecs but keeps space allocated. + pub fn clear(&mut self) { + self.coord.clear(); + self.by_coord.clear(); + self.free.clear(); + for e in self.edges.iter_mut() { + e.clear(); + } + } + + #[inline] + pub fn is_live(&self, id: CellId) -> bool { + self.coord[id as usize] != TOMBSTONE + } + + /// 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; + } + 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 + } + + /// 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.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] + } + + #[inline] + pub fn neighbors(&self, id: CellId) -> &[Edge] { + &self.edges[id as usize] + } + + #[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). + 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], out: &mut SurfaceLookup) { + out.clear(); + for &(ix, iy, iz) in cells { + out.entry((ix, iy)).or_default().push(iz); + } + for zs in out.values_mut() { + zs.sort_unstable(); + zs.dedup(); + } +} + +/// Populate cells with surface adjacency from the lookup. Deletes any +/// existing contents +pub fn build_surface_cells( + cells: &mut SurfaceCells, + surface_lookup: &SurfaceLookup, + voxel_size: f32, + step_threshold_cells: i32, +) { + 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.insert((ix, iy, iz)); + } + } + + 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; + }; + for &nz in nzs { + let dz = nz - iz; + if dz.abs() > step_threshold_cells { + continue; + } + 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 { dest, cost }); + } + } + }); +} + +#[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 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.dest), e.cost)) + .collect() + } + + #[test] + fn same_z_neighbors_are_bidirectional() { + 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].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 (_, 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 (_, 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 (_, sc) = build(&[(0, 0, 0), (1, 0, 1)]); + let expected = (2.0_f32).sqrt() * VOXEL; + 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].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 (_, 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.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)); + } + + #[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_3d/mls_planner/rust/src/dijkstra.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/dijkstra.rs new file mode 100644 index 0000000000..59a2890c89 --- /dev/null +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/dijkstra.rs @@ -0,0 +1,205 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 + +//! 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 crate::adjacency::{CellId, SurfaceCells, NO_CELL}; + +#[derive(Default)] +pub struct DijkstraState { + pub dist: Vec, + pub pred: Vec, + pub source: Vec, + heap: BinaryHeap, +} + +impl DijkstraState { + /// Reset all vecs to the specified capacity. + 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(); + } +} + +/// 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()); + + for (label, &s) in sources.iter().enumerate() { + if !cells.is_live(s) { + continue; + } + 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)) = state.heap.pop() { + let cur = state.dist[u as usize]; + if d > cur { + continue; + } + let su = state.source[u as usize]; + for edge in cells.neighbors(u) { + let nd = d + edge.cost; + 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.dest)); + } + } + } +} + +/// 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; + loop { + let p = state.pred[cur as usize]; + if p == NO_CELL { + break; + } + cur = p; + cells.push(cur); + } + cells +} + +struct Scored(f32, CellId); + +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 { + // Order on score, and use cell id for tie-breaker for repeatability + other.0.total_cmp(&self.0).then(self.1.cmp(&other.1)) + } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::adjacency::SurfaceCells; + + fn chain(n: i32) -> (SurfaceCells, Vec) { + let mut sc = SurfaceCells::default(); + 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); + } + (sc, ids) + } + + #[test] + fn single_source_dist_and_pred() { + 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_eq!(st.pred[ids[0] as usize], NO_CELL); + let mut cur = ids[4]; + let mut hops = 0; + while st.pred[cur as usize] != NO_CELL { + cur = st.pred[cur as usize]; + hops += 1; + } + assert_eq!(cur, ids[0]); + assert_eq!(hops, 4); + } + + #[test] + fn multi_source_labels_by_nearest() { + 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!(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() { + let mut sc = SurfaceCells::default(); + 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); + 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() { + let mut sc = SurfaceCells::default(); + 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); + 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); + } + } +} diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/edges.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/edges.rs new file mode 100644 index 0000000000..8884b96af0 --- /dev/null +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/edges.rs @@ -0,0 +1,248 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 + +//! Node-graph edge construction. +//! +//! 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. + +use ahash::AHashMap; +use rayon::prelude::*; + +use crate::adjacency::{CellId, Edge, SurfaceCells, SurfaceLookup, NO_CELL}; +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: NodeId, + pub b: NodeId, + pub cost: f32, + /// Cell on a's side of the cheapest Voronoi boundary crossing. + pub boundary_u: CellId, + /// Cell on b's side. + pub boundary_v: CellId, +} + +#[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, +} + +impl PlannerGraph { + pub fn new() -> Self { + Self::default() + } +} + +/// 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], + state: &mut DijkstraState, + out_edges: &mut Vec, + out_adj: &mut Vec>, +) { + out_edges.clear(); + out_adj.clear(); + + if nodes.is_empty() { + state.reset(cells.slot_capacity()); + return; + } + + let source_cells: Vec = nodes.iter().map(|n| n.cell_id).collect(); + dijkstra(cells, &source_cells, state); + + best_boundary_edges(cells, state, out_edges); + + 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 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<(NodeId, NodeId), NodeEdge> = cell_entries + .par_iter() + .fold( + AHashMap::<(NodeId, NodeId), 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.dest; + 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::<(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 { + *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. 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(state, edge.boundary_u); + from_a.reverse(); + 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| (cells.coord(pair[0]), cells.coord(pair[1]), cost)) + .collect::>() + }) + .collect() +} + +#[cfg(test)] +mod tests { + use super::*; + 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 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| { + 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(); + build_node_edges( + &plg.cells, + &plg.nodes, + &mut plg.cell_state, + &mut plg.node_edges, + &mut plg.node_adj, + ); + plg + } + + fn strip_cells() -> Vec { + (0..20).map(|x| (x, 0, 0)).collect() + } + + #[test] + fn two_nodes_on_strip_have_one_edge() { + 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)); + 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() { + let pg = setup(&strip_cells(), &[(3, 0, 0), (10, 0, 0), (17, 0, 0)]); + let pairs: Vec<(NodeId, NodeId)> = 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() { + let mut cells: Vec = (0..5).map(|x| (x, 0, 0)).collect(); + cells.extend((10..15).map(|x| (x, 0, 0))); + let pg = setup(&cells, &[(2, 0, 0), (12, 0, 0)]); + assert!(pg.node_edges.is_empty()); + } + + #[test] + fn predecessor_walk_recovers_cell_path() { + 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_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_3d/mls_planner/rust/src/main.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/main.rs new file mode 100644 index 0000000000..e824dd64fc --- /dev/null +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/main.rs @@ -0,0 +1,470 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 + +mod adjacency; +mod dijkstra; +mod edges; +mod nodes; +mod planner; +mod surfaces; +mod voxel; + +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::Path; +use lcm_msgs::sensor_msgs::{PointCloud2, PointField}; +use lcm_msgs::std_msgs::{Header, Time}; +use serde::Deserialize; +use tracing::info; + +use ahash::AHashSet; + +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, ColumnIz}; +use crate::voxel::{surface_point_xyz, voxelize, VoxelKey}; + +#[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, +} + +#[derive(Module)] +#[module(setup = setup)] +struct MlsPlanner { + #[input(decode = PointCloud2::decode, handler = on_global_map)] + global_map: Input, + + #[input(decode = PoseStamped::decode, handler = on_start_pose)] + start_pose: Input, + + #[input(decode = PoseStamped::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, + + clearance_cells: i32, + 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 { + async fn setup(&mut self) { + let cfg = &self.config; + 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; + + info!( + world_frame = %cfg.world_frame, + voxel_size = cfg.voxel_size, + robot_height = cfg.robot_height, + clearance_cells = self.clearance_cells, + step_cells = self.step_cells, + "mls_planner ready", + ); + } + + async fn on_global_map(&mut self, msg: PointCloud2) { + 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 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(); + 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)> = self + .surface_buf + .iter() + .map(|&(ix, iy, iz)| surface_point_xyz(ix, iy, iz, voxel_size)) + .collect(); + publish_cloud(&self.surface_map, &surface_points, &frame, now()).await; + + let t_nodes = Instant::now(); + 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 nodes_count = plg.nodes.len(); + + 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 = 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 edges_count = plg.node_edges.len(); + + 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 = voxels_count, + surface_cells = surface_count, + nodes = nodes_count, + edges = edges_count, + surface_ms, + nodes_ms, + edges_ms, + "global_map processed", + ); + } + + 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: PoseStamped) { + 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; + }; + if plg.nodes.is_empty() { + tracing::warn!("MLSPlanner received goal before graph had nodes; skipping"); + return; + } + + let p = &msg.pose.position; + let goal = (p.x as f32, p.y as f32, p.z as f32); + + let t_plan = Instant::now(); + let waypoints = match planner::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 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(), plan_ms, "path planned"); + publish_path(&self.path, &path_msg).await; + } +} + +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)], + 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().min(i32::MAX as u64) 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 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.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); + 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() + .await + .expect("failed to create LCM transport"); + run::(transport) + .await + .expect("mls_planner run failed"); +} diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs new file mode 100644 index 0000000000..50c0463763 --- /dev/null +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs @@ -0,0 +1,255 @@ +// 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. + +use ahash::AHashMap; +use rayon::prelude::*; + +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_id: CellId, + pub pos: (f32, f32, f32), +} + +/// 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, + node_spacing_m: f32, + node_wall_buffer_m: f32, + state: &mut DijkstraState, + out_nodes: &mut Vec, +) { + out_nodes.clear(); + if cells.is_empty() { + return; + } + + let mut wall_seeds: Vec = Vec::new(); + collect_wall_adjacent_cells(cells, &mut wall_seeds); + dijkstra(cells, &wall_seeds, state); + + 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)) + }); + + let survivors = nms_grid(cells, &candidates, voxel_size, node_spacing_m); + + 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. 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); + + // Check if all 4 neighbors are present + let mut mask: u8 = 0; + for e in edges { + let (nx, ny, _) = cells.coord(e.dest); + 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); + } + } +} + +/// 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; + 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 = 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_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; + } + } + } + } + } + } + if !killed { + survivors.push(id); + bins.entry((bx, by, bz)).or_default().push(id); + } + } + survivors +} + +/// 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_of(dist[*src as usize], buffer_m); + for edge in edges.iter_mut() { + let pv = penalty_of(dist[edge.dest 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; + + 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 + } + + 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() { + 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() { + let mut cells_in = Vec::new(); + for ix in 0..10 { + for iy in 0..10 { + cells_in.push((ix, iy, ix)); + } + } + 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() { + 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; + 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() { + 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()); + for edge in outbound { + assert!(edge.cost >= 1.5 * VOXEL - 1e-5); + } + } +} diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs new file mode 100644 index 0000000000..e3bb8e818a --- /dev/null +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs @@ -0,0 +1,348 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 + +use std::cmp::Ordering; +use std::collections::BinaryHeap; + +use ahash::AHashMap; + +use crate::adjacency::{CellId, SurfaceLookup}; +use crate::dijkstra::walk_preds; +use crate::edges::{NodeEdgeIdx, NodeId, PlannerGraph, NO_NODE}; +use crate::voxel::{surface_point_xyz, VoxelKey}; + +/// Snap a pose to the best surface cell. +pub fn snap_pose_to_cell( + surface_lookup: &SurfaceLookup, + pose: (f32, f32, f32), + 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)); + } + } + } + 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)); + } + } + let (bd, iz) = best?; + if bd > tol_cells { + return None; + } + Some((ix, iy, iz)) +} + +/// 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), + goal_pose: (f32, f32, f32), + voxel_size: f32, + z_tolerance_m: f32, +) -> Option> { + 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_idx_by_cell: AHashMap = plg + .nodes + .iter() + .enumerate() + .map(|(i, n)| (n.cell_id, i as NodeId)) + .collect(); + + 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( + plg, + &node_seq, + start_pose, + &start_segment, + goal_pose, + &goal_segment, + voxel_size, + )) +} + +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![NO_NODE; 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; + heap.push(Scored(nd, neighbor)); + } + } + } + + if !dist[goal as usize].is_finite() { + return None; + } + let mut path = vec![goal]; + let mut cur = goal; + while pred[cur as usize] != NO_NODE { + cur = pred[cur as usize]; + path.push(cur); + } + path.reverse(); + Some(path) +} + +fn assemble_waypoints( + plg: &PlannerGraph, + node_seq: &[NodeId], + start_pose: (f32, f32, f32), + start_segment: &[CellId], + goal_pose: (f32, f32, f32), + goal_segment: &[CellId], + 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 = + 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(&plg.cell_state, start_side); + from_a.reverse(); + 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) { + cells.push(c); + } + } + } + + 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 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); + waypoints +} + +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 }; + if other == b { + return Some(edge_idx); + } + } + None +} + +struct Scored(f32, NodeId); + +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_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 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| { + 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(); + 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 { + (0..n).map(|x| (x, 0, 0)).collect() + } + + #[test] + fn snap_picks_in_column_cell() { + 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() { + let mut cells = strip(20); + cells.retain(|c| c.0 != 2); + 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 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()); + } + + #[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() { + 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_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.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] + 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(); + 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" + ); + } + } +} diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/surfaces.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/surfaces.rs new file mode 100644 index 0000000000..7451b959c4 --- /dev/null +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/surfaces.rs @@ -0,0 +1,258 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 + +//! 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}; +use imageproc::distance_transform::Norm; +use imageproc::morphology::{dilate, erode}; +use rayon::prelude::*; + +use crate::voxel::VoxelKey; + +const ON: Luma = Luma([255]); +const OFF: Luma = Luma([0]); + +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. +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; + }; + let idx = zs.partition_point(|&z| z <= iz); + match zs.get(idx) { + Some(&next) => next - iz > clearance_cells, + None => true, + } +} + +/// Extract standable cells from the voxelized global map, then close small +/// holes. +pub fn extract_surfaces( + voxel_map: &AHashSet, + clearance_cells: i32, + dilation_passes: u32, + erosion_passes: u32, + by_col: &mut ColumnIz, + out: &mut Vec, +) { + out.clear(); + by_col.clear(); + if voxel_map.is_empty() { + return; + } + + for &(ix, iy, iz) in voxel_map { + by_col.entry((ix, iy)).or_default().push(iz); + } + + 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 local: Vec = Vec::new(); + for w in zs.windows(2) { + if w[1] - w[0] > clearance_cells { + local.push((*ix, *iy, w[0])); + } + } + if let Some(&last_iz) = zs.last() { + local.push((*ix, *iy, last_iz)); + } + local + }) + .collect(); + drop(entries); + + close_surface_holes( + standable, + by_col, + dilation_passes, + erosion_passes, + clearance_cells, + out, + ); +} + +/// Dilation and erosion on all xy slices of the extracted surfaces +/// to fill in small holes. +fn close_surface_holes( + standable: Vec, + by_col: &ColumnIz, + dilation_passes: u32, + erosion_passes: u32, + clearance_cells: i32, + out: &mut Vec, +) { + if standable.is_empty() || (dilation_passes == 0 && erosion_passes == 0) { + out.extend(standable); + return; + } + + 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(); + 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. +fn close_at_z( + xys: &[(i32, i32)], + iz: i32, + by_col: &ColumnIz, + dilation_passes: u32, + erosion_passes: u32, + clearance_cells: i32, +) -> Vec { + 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; + 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); + } + + if dilation_passes > 0 { + 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.min(u8::MAX as u32) 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, by_col, clearance_cells) { + continue; + } + out.push((ix, iy, iz)); + } + } + out +} + +#[cfg(test)] +mod tests { + use super::*; + + fn voxel_map(cells: &[VoxelKey]) -> AHashSet { + 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!(run(&[], 5, 0, 0).is_empty()); + } + + #[test] + fn single_cell_is_topmost_surface() { + 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 = run(&cells, 5, 0, 0); + assert_eq!(s, vec![(0, 0, 4)]); + } + + #[test] + fn gap_larger_than_headroom_makes_lower_cell_standable() { + 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() { + let cells: Vec = [ + (-1, -1), + (-1, 0), + (-1, 1), + (0, -1), + (0, 1), + (1, -1), + (1, 0), + (1, 1), + ] + .into_iter() + .map(|(dx, dy)| (dx, dy, 0)) + .collect(); + let s = run(&cells, 5, 3, 3); + assert!( + s.contains(&(0, 0, 0)), + "closing should fill the center hole" + ); + } + + #[test] + fn closing_does_not_bridge_voxel_in_headroom() { + let mut cells: Vec = [ + (-1, -1), + (-1, 0), + (-1, 1), + (0, -1), + (0, 1), + (1, -1), + (1, 0), + (1, 1), + ] + .into_iter() + .map(|(dx, dy)| (dx, dy, 0)) + .collect(); + cells.push((0, 0, 1)); + let s = run(&cells, 5, 3, 3); + assert!(!s.contains(&(0, 0, 0))); + } +} diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/voxel.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/voxel.rs new file mode 100644 index 0000000000..2fca61c048 --- /dev/null +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/voxel.rs @@ -0,0 +1,26 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 + +//! Voxel-grid coordinate math. + +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) { + ( + (ix as f32 + 0.5) * voxel_size, + (iy as f32 + 0.5) * voxel_size, + (iz as f32 + 1.0) * voxel_size, + ) +} diff --git a/dimos/navigation/nav_stack/evaluator/blueprints.py b/dimos/navigation/nav_stack/evaluator/blueprints.py deleted file mode 100644 index ad9a2c7325..0000000000 --- a/dimos/navigation/nav_stack/evaluator/blueprints.py +++ /dev/null @@ -1,64 +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 Any - -from dimos.core.coordination.blueprints import autoconnect -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]], - 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], - ) - - -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 new file mode 100644 index 0000000000..253d3b909a --- /dev/null +++ b/dimos/navigation/nav_stack/modules/click_start_goal_router/click_start_goal_router.py @@ -0,0 +1,61 @@ +# 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.PoseStamped import PoseStamped +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[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 = 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(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(pose) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 0c312a5491..0b0785dcb8 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", @@ -133,6 +133,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", @@ -145,7 +146,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", @@ -175,6 +176,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_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", @@ -212,7 +214,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", 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