feat: planner incremental builds and mapper frame registration#2565
Open
aclauer wants to merge 10 commits into
Open
feat: planner incremental builds and mapper frame registration#2565aclauer wants to merge 10 commits into
aclauer wants to merge 10 commits into
Conversation
❌ 2 Tests Failed:
View the full list of 2 ❄️ flaky test(s)
To view more test analytics, go to the Test Analytics Dashboard |
aclauer
changed the title
aclauer
requested review from
arkluc,
leshy,
mustafab0,
paul-nechifor and
spomichter
as code owners
Contributor
Greptile SummaryThis PR introduces incremental graph updates to the MLS planner (replacing full rebuilds with windowed
Confidence Score: 4/5Safe to merge with two low-severity hardening items addressed first. The core logic — incremental surface extraction, Dijkstra region relabeling, frame registration, and stamp-paired async ingestion — is well-structured and covered by tests. Two P2 gaps prevent a 5:
Important Files Changed
Sequence Diagram%%{init: {'theme': 'neutral'}}%%
sequenceDiagram
participant Lidar as Lidar
participant RTMapper as RayTraceMap
participant RustMapper as VoxelRayMapper
participant MLSHandle as MlsPlanner Handle
participant Worker as MlsPlanner Worker
Lidar->>RTMapper: Observation[PointCloud2] + pose(qx,qy,qz,qw)
RTMapper->>RTMapper: rotate+translate pts to world frame
RTMapper->>RustMapper: add_frame(world_pts, origin)
RTMapper->>RustMapper: local_map(cx, cy, radius, z_min, z_max)
RTMapper-->>MLSHandle: local_cloud + region_bounds tag
MLSHandle->>MLSHandle: on_local_map sets pending_local, calls try_pair
MLSHandle->>MLSHandle: on_region_bounds sets pending_bounds, calls try_pair
MLSHandle->>MLSHandle: stamps_paired check
MLSHandle->>Worker: Arc-Mutex gets MapUpdate::Region, Notify fires
Worker->>Worker: replace_region_voxels (parallel remove + insert)
Worker->>Worker: extract_surfaces_region (windowed morphological close)
Worker->>Worker: rebuild_region_graph (edges, nodes, boundaries)
Worker->>Worker: dijkstra_region (wall + node Dijkstra states)
Worker->>Worker: snap_candidates, node_dijkstra, string_pull
Worker-->>MLSHandle: publish Path + viz
%%{init: {'theme': 'base', 'themeVariables': {"darkMode": true, "background": "#0d1117", "primaryColor": "#21262d", "primaryTextColor": "#e6edf3", "primaryBorderColor": "#8b949e", "lineColor": "#8b949e", "textColor": "#e6edf3", "edgeLabelBackground": "#161b22", "actorBkg": "#21262d", "actorBorder": "#8b949e", "actorTextColor": "#e6edf3", "actorLineColor": "#8b949e", "signalColor": "#8b949e", "signalTextColor": "#e6edf3", "noteBkgColor": "#373320", "noteBorderColor": "#d4a72c", "noteTextColor": "#f0e6c0", "labelBoxBkgColor": "#21262d", "labelBoxBorderColor": "#8b949e", "labelTextColor": "#e6edf3", "loopTextColor": "#e6edf3", "activationBkgColor": "#30363d", "activationBorderColor": "#8b949e"}}}%%
sequenceDiagram
participant Lidar as Lidar
participant RTMapper as RayTraceMap
participant RustMapper as VoxelRayMapper
participant MLSHandle as MlsPlanner Handle
participant Worker as MlsPlanner Worker
Lidar->>RTMapper: Observation[PointCloud2] + pose(qx,qy,qz,qw)
RTMapper->>RTMapper: rotate+translate pts to world frame
RTMapper->>RustMapper: add_frame(world_pts, origin)
RTMapper->>RustMapper: local_map(cx, cy, radius, z_min, z_max)
RTMapper-->>MLSHandle: local_cloud + region_bounds tag
MLSHandle->>MLSHandle: on_local_map sets pending_local, calls try_pair
MLSHandle->>MLSHandle: on_region_bounds sets pending_bounds, calls try_pair
MLSHandle->>MLSHandle: stamps_paired check
MLSHandle->>Worker: Arc-Mutex gets MapUpdate::Region, Notify fires
Worker->>Worker: replace_region_voxels (parallel remove + insert)
Worker->>Worker: extract_surfaces_region (windowed morphological close)
Worker->>Worker: rebuild_region_graph (edges, nodes, boundaries)
Worker->>Worker: dijkstra_region (wall + node Dijkstra states)
Worker->>Worker: snap_candidates, node_dijkstra, string_pull
Worker-->>MLSHandle: publish Path + viz
Reviews (3): Last reviewed commit: "Minor fixes" | Re-trigger Greptile |
![greptile-apps[bot]](https://avatars.githubusercontent.com/in/867647?s=60&v=4){kind=small}
aclauer
changed the title
github-actions
Bot
added
the
ready-to-merge
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Problem
Improvements to MLS planner and ray tracing
Closes DIM-XXX
Solution
ray tracer:
planner:
How to Test
Contributor License Agreement