Skip to content

Feat/ivan/local planner trajectory - #4203

Open
leshy wants to merge 19 commits into
mainfrom
feat/ivan/local_planner_trajectory
Open

leshy wants to merge 19 commits into
mainfrom
feat/ivan/local_planner_trajectory

Conversation

@leshy

@leshy leshy commented Sep 17, 2026

Copy link
Copy Markdown
Member

there are a few things here that can be split into separate PRs but nav team is the only consumer so keeping here unless we decide otherwise

restructure of navigation/ much cleaner folder, new specs

class GlobalPlanner(Protocol):
    tf: In[TFMessage]
    goal: In[PointStamped]

    path: Out[Path]


class BlindLocalPlanner(Protocol):
    """Shapes the global route without looking: no map."""

    planner_path: In[Path]
    tf: In[TFMessage]

    path: Out[Path]


class MapLocalPlanner(Protocol):
    """Routes around what the local map says is there."""

    planner_path: In[Path]
    local_map: In[PointCloud2]
    tf: IO[TFMessage]

    path: Out[Path]


class TrajectoryFollower(Protocol):
    tf: IO[TFMessage]
    path: In[Path]

    nav_cmd_vel: Out[Twist]

generic mcap mem2 decoder of all dimos msgs + tests

bridge hacky selective sub feature

we need to be able to explicitly subscribe to just SOME topics not all, so that they are not sent over network or decoded by py

module introspection fixes

module.io() hides default internal RPCs

lidar module config standardized

removed global env reading, switched to local module config

distributed zenoh architecture for nav

we run go2-zenoh-motion-pointlio on go2 jetson and dimos --robot-ip go22 run go2-viewer on the laptop

this means zenoh (network robust) subs go over network, and rerun rendering and RPC happens locally on the laptop

TODO

  • requires go2web on the actual go2 or jetson for zenoh control, this needs to be reimplemented and moved to dimos in follow up PR
  • experiment with global planner actually running the local planner internally so we have a single planner

nav_3d/ is global_planner/. The Go2's own A* planner (and its RPC spec, the
agent skills' contract with it) go under go2/; dannav, frontier_exploration
and patrolling under experimental/; basic_path_follower becomes
trajectory_follower/basic/; the visual servoing, its bbox goal module and
navigation/visual/query join the skills that use them under
agents/skills/visual_servoing/. Renames and import rewrites only.
topics= subscribes one zenoh key per name so an unlisted topic never
crosses the link; serve_coordinator_rpc=False lets a viewer sit on a bus
whose Coordinator name another stack owns. Both for a viewer on the far
side of the robot's wifi.
local_planner/: an SE(2) search over the raycaster's local map, sliced
through the body's own z-band, replanning to a carrot along the MLS route.
trajectory_follower/fancy/: a pursuit law that reads the required precision
off the path's own stamps. Each is a python module with a rust native twin
(feature module) that is what runs; pyo3 (feature python) is test-only,
for the parity tests. spec.py says what a GlobalPlanner, a Blind/Map
LocalPlanner and a TrajectoryFollower are, as ports. An empty path is stop,
a single pose is hold. embodiment/ is the measured Go2; tf_pose.py reads
the body pose off tf with a deadman; cmd_vel_mux lets teleop preempt nav.

go2-zenoh-motion, go2-zenoh-motion-pointlio (the Jetson rig, with its own
Point-LIO and lidar addresses) and go2-viewer.
The config system feeds module fields from the environment (MID360__LIDAR_IP,
.env, --mid360.lidar-ip); the Mid-360, virtual Mid-360, Point-LIO and
FAST-LIO configs stop reading DIMOS_*_LIDAR_IP themselves.
The wire topic carries the message type in its last segment, so a dimos
channel needs no codec registry; injected codecs still win.
… build

A flake ref copies its whole source tree into the store; pointed at the repo
root that is the build dirs, .venv and the LFS blobs. This directory is two
files. The clippy hook skips the root flake for the same reason.
.pre-commit-config.yaml: main's, where #4202 turned the clippy hook off;
our variant of that hook goes with it.
@codecov

codecov Bot commented Sep 17, 2026

Copy link
Copy Markdown

Codecov Report

❌ Patch coverage is 85.80758% with 326 lines in your changes missing coverage. Please review.
✅ All tests successful. No failed tests found.

Files with missing lines Patch % Lines
...tion/trajectory_follower/fancy/test_rust_parity.py 16.66% 121 Missing and 4 partials ⚠️
dimos/navigation/local_planner/search/se2.py 76.98% 58 Missing ⚠️
dimos/navigation/local_planner/module.py 85.10% 25 Missing and 3 partials ⚠️
...avigation/trajectory_follower/fancy/laws/hinted.py 75.00% 27 Missing ⚠️
dimos/navigation/local_planner/search/target.py 69.81% 16 Missing ⚠️
...mos/navigation/trajectory_follower/fancy/module.py 87.69% 14 Missing and 2 partials ⚠️
dimos/memory/store/test_mcap_wire.py 73.91% 12 Missing ⚠️
.../navigation/trajectory_follower/fancy/laws/seed.py 88.00% 9 Missing ⚠️
dimos/memory/store/mcap.py 75.00% 5 Missing and 2 partials ⚠️
...navigation/trajectory_follower/fancy/controller.py 79.31% 6 Missing ⚠️
... and 10 more
@@            Coverage Diff             @@
##             main    #4203      +/-   ##
==========================================
- Coverage   79.45%   77.84%   -1.62%     
==========================================
  Files        1485     1531      +46     
  Lines      140563   145195    +4632     
  Branches    12062    12545     +483     
==========================================
+ Hits       111690   113025    +1335     
- Misses      25460    28811    +3351     
+ Partials     3413     3359      -54     
Components Coverage Δ
Tests 92.22% <83.99%> (-3.13%) ⬇️
Flag Coverage Δ
OS-ubuntu-24.04-arm 75.27% <82.65%> (+0.25%) ⬆️
OS-ubuntu-latest ?
Py-3.10 ?
Py-3.11 ?
Py-3.12 75.27% <82.65%> (-0.58%) ⬇️

Flags with carried forward coverage won't be shown. Click here to find out more.

Files with missing lines Coverage Δ
dimos/agents/skills/navigation.py 41.50% <100.00%> (ø)
dimos/agents/skills/person_follow.py 29.34% <100.00%> (ø)
...nts/skills/visual_servoing/detection_navigation.py 28.26% <ø> (ø)
dimos/agents/skills/visual_servoing/query.py 29.41% <ø> (ø)
...gents/skills/visual_servoing/visual_servoing_2d.py 42.85% <ø> (ø)
dimos/cli/bake/test_discovery.py 100.00% <100.00%> (ø)
dimos/cli/dimos.py 97.89% <100.00%> (ø)
dimos/codebase_checks/test_no_sections.py 71.66% <ø> (ø)
...rol/tasks/path_follower_task/path_follower_task.py 72.36% <100.00%> (ø)
dimos/core/coordination/module_coordinator.py 86.72% <100.00%> (+0.06%) ⬆️
... and 126 more

... and 120 files with indirect coverage changes

🚀 New features to boost your workflow:
  • 📦 JS Bundle Analysis: Save yourself from yourself by tracking and limiting bundle sizes in JS merges.

@greptile-apps

greptile-apps Bot commented Sep 17, 2026

Copy link
Copy Markdown
Contributor

RetriggerConfidence Score: 5/5

Safe to merge.

Summary

This update completes the follow-up fixes for native navigation and MCAP handling. Native planner builds use Cargo directly, point-cloud extraction uses checked shared metadata handling, and MCAP stream naming now disambiguates compatible DimOS channels while rejecting ambiguous collisions. No new actionable issues were identified.

Reviews (2) · Last reviewed commit: "mcap: a stream name never covers two top..."

Comment thread dimos/navigation/local_planner/rust/src/module/msg.rs Outdated
Comment thread dimos/mapping/ray_tracing/module.py Outdated
Comment thread dimos/memory/store/mcap.py
The raytracer and the local planner each carried a copy of the same
cloud reader, and both trusted the wire counts: a negative width or
offset wrapped through `as usize`, width*height*point_step overflowed,
and a malformed cloud could panic the module instead of being dropped.
The one copy lives in dimos-module now, every count is range-checked
and multiplied with checked_mul, and both modules call it.

The raytracer and MLS build_command go back to plain cargo (the nix
toolchain flake served the clippy hook #4202 removed), so a no-nix
install can still build them on first start.
Naming a dimos channel by its port dropped the type, so two types on one
port collapsed to one stream and the later channel silently replaced the
earlier one. Now the type tells them apart (shared, shared_Vector3), and
two different ports folding to one slug is refused with both topics
named; streams= is how they get names of their own.
A native module's tf port both subscribes and publishes, so its python
wrapper declares it IO. The manifest check only knew In and Out and
called tf "not a port". The motion twins are the first natives with one.
Comment thread dimos/navigation/test_tf_pose.py
It did what MovementManager does (teleop preempts nav, cooldown, scaling,
stop_movement) plus a watchdog, and the motion stacks ran both with the
manager's outputs remapped into the void. One mux is enough.
@github-actions github-actions Bot added the ready-to-merge Required CI checks have passed on this PR label Sep 17, 2026
}

/// The finite xyz points of a little-endian float32 cloud, in order.
pub fn extract_xyz(msg: &PointCloud2) -> Result<Vec<[f32; 3]>, ExtractError> {

Copy link
Copy Markdown
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think this should actually go in the lcm repository in the pointcloud struct, but we can do that later. I need to publish that crate on crates.io anyway.

Copy link
Copy Markdown
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

yeah I agree. TBH I'm not sure if lcm repository should go to dimos

/// configured with, deserialised straight from its config.
#[derive(Clone, Debug, serde::Deserialize, serde::Serialize)]
#[serde(deny_unknown_fields)]
pub struct Emb {

Copy link
Copy Markdown
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

should this struct be declared somewhere outside of the local planner? it doesn't seem like it really belongs with a specific module

/// Adding 1.5 * 2^52 forces the significand to shed its fractional bits under
/// the ambient rounding mode -- which is round-to-nearest-ties-to-even, and
/// which Rust never changes -- leaving the rounded integer in the low mantissa
/// bits, biased by the constant. Subtracting the constant's own bit pattern

Copy link
Copy Markdown
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

almost 1000 lines of this file are comments. If they were shortened I think it would help both the AI and me.

Copy link
Copy Markdown
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

jesus christ

977 comment lines to 222. Contracts and python cross-references stay;
perf narrative, measurement history and asides go.
146 sites in 42 files. Asides become colons, commas or parentheses; the
essay blocks around them are cut to the contract.
Every comment and docstring the branch added, cut to the contract:
one line, two where an invariant needs it, none where the code or the
test name already says it. 40 files.
Neither is a layer anyone runs; the motion stacks are the path forward.
Registry regenerated.
@github-actions github-actions Bot removed the ready-to-merge Required CI checks have passed on this PR label Sep 18, 2026
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

2 participants