Skip to content

Merge mapf_post into mapf and align types - #37

Draft
arjo129 wants to merge 11 commits into
mainfrom
arjo/feat/merge-mapf-post
Draft

Merge mapf_post into mapf and align types#37
arjo129 wants to merge 11 commits into
mainfrom
arjo/feat/merge-mapf-post

Conversation

@arjo129

@arjo129 arjo129 commented Mar 6, 2026 •
edited
Loading

Copy link
Copy Markdown
Member

Implemented feature

#29

Implementation description

This PR merges the mapf_post functionality into the mapf crate, as tracked in issue #29. This consolidation simplifies the codebase and aligns the types between the core MAPF utilities and the post-processing algorithms used for robust plan execution.

Motivation

Previously, mapf_post existed as a separate utility or was part of an external experimental repository. Merging it into mapf allows for tighter integration with the base traits and types of the multi-agent planning framework, facilitating a more seamless transition from planning to execution-ready semantic plans.

Key Changes

  1. Core post Module:

    • Introduced mapf/src/post/mod.rs containing the implementation of the mapf_post algorithm based on the approach described in Hoenig et al., 2019 (https://whoenig.github.io/publications/2019_RA-L_Hoenig.pdf).
    • This algorithm converts time-discretized MAPF results into dependency-based SemanticPlans, which are more resilient to timing delays during robot execution.

  2. Semantic Planning & Following:

    • Added SemanticPlan and SemanticWaypoint types to represent agent-level dependencies.
    • Implemented WaypointFollower to help estimate an agent's progress along its assigned trajectory and identify the next safe waypoint.

  3. Collision Detection:

    • Integrated parry2d-f64 for high-fidelity collision checking between agent footprints during trajectory execution. This replaces simpler time-based checks with geometric sweeps.

  4. Spatial Allocation:

    • Added mapf/src/post/spatial_allocation/mod.rs, which provides a Grid2D and AllocationField system.
    • This system enables the reservation of spatial zones (dynamic safety areas) based on the semantic plan's dependencies, ensuring robots maintain safe separation even when they deviate from the original time-schedule.

  5. Crate Updates:

    • Updated mapf to the Rust 2024 edition.
    • Added new dependencies: parry2d, petgraph, bresenham, rand, and csv to support the new post-processing features.
    • Updated nalgebra to version 0.32.

Implementation Details

  • mapf_post identifies "Type 1" (sequential) and "Type 2" (collision-avoidance) dependencies between waypoints.
  • AllocationField implements tie-breaking rules for spatial reservations, including leader-follower hierarchies and intersection-aware prioritization.
  • WaypointFollower uses projection onto trajectory segments to remain robust against localization jitter or minor navigation errors.

Verification

  • Added unit tests in mapf/src/post/mod.rs covering:
    • WaypointFollower logic.
    • SemanticPlan waypoint and dependency management.
    • Collision detection between moving shapes.
    • Core mapf_post algorithm on a simple cross-junction scenario.
    • Violation checking and claim dictionary generation.

GenAI Use

We follow OSRA's policy on GenAI tools

Note the original library doesnt use gen-ai I used gen-ai to see if it could merge the two libraries and reconcile APIs.

  • I used a GenAI tool in this PR.
  • I did not use GenAI

Generated-by: Gemini-cli

@mxgrey mxgrey added this to PMC Board Mar 6, 2026
@github-project-automation github-project-automation Bot moved this to Inbox in PMC Board Mar 6, 2026
@xiyuoh xiyuoh moved this from Inbox to In Progress in PMC Board Mar 10, 2026
@arjo129
arjo129 force-pushed the arjo/feat/merge-mapf-post branch from 0d9939c to 26e2496 Compare March 19, 2026 05:00
arjo129
arjo129 commented Mar 19, 2026
View reviewed changes
Comment thread mapf/src/negotiation/scenario.rs Outdated
Comment thread mapf/src/post/spatial_allocation/mod.rs
Comment thread mapf/src/negotiation/mod.rs
Comment thread mapf/src/negotiation/mod.rs
arjo129 added 10 commits March 20, 2026 09:11
@arjo129
Merge mapf_post into mapf and align types
081cac7 
Generated-by: Gemini-CLI

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
@arjo129
Integrate Scenario with solver and post-processor
2d88e90 
This adds solve(), derive_mapf_result(), and derive_semantic_plan()
to the Scenario struct for tighter integration between MAPF solving
and trajectory derivation.

Generated-by: Gemini-CLI
Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
@arjo129
Include moving obstacles as agents in MapfResult for traffic dependen…
d3a5f32 
…cies

This allows obstacles in a Scenario to be represented as agents in the
MAPFPost output, enabling traffic dependency analysis for moving
objects.

Generated-by: Gemini-CLI
Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
@arjo129
Downgrade mapf to Rust 2021 for broader compatibility
093e807 
- Changed edition to 2021 in mapf/Cargo.toml
- Refactored 'let chains' in post module to nested 'if let' blocks
- Removed unused petgraph import
- Updated formatting across multiple files to match 2021 edition

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
@arjo129
Fix formatting
6a192f6 
Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
@arjo129
undo unessecary changes
ecb1afc 
Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
@arjo129
Remove dead slop code
e4af8fb 
Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
@arjo129
Refactor agent ID handling and remove unnecessary mutability
c473b14 
- Move agent ID assignment from negotiate to derive_mapf_result
- Remove id_to_name field from Scenario struct
- Change negotiate and Scenario::solve to take shared references
- Add agent_name_to_id to MapfResult and SemanticPlan
- Add get_agent_id helper to SemanticPlan

Generated-by: Gemini CLI
Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
@arjo129
Make MapfResult private to prvent confusion
71cfa00 
Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
@arjo129
Implement Grid2D constructor from Scenario
a001f0e 
- Add Grid2D::from_scenario and From<&Scenario> for Grid2D
- Correctly calculate grid dimensions and occupancy from Scenario data
- Update allocate_trajectory to use dynamic grid dimensions
- Add unit test for from_scenario constructor

Generated-by: Gemini CLI
Signed-off-by: Gemini CLI <gemini-cli@google.com>
Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
@arjo129
arjo129 force-pushed the arjo/feat/merge-mapf-post branch from 1282d90 to a001f0e Compare March 20, 2026 09:11
@arjo129 arjo129 linked an issue Apr 8, 2026 that may be closed by this pull request
[Feature request]: Add support for extracting semantic plan #29
Open
1 task
@mxgrey mxgrey moved this from In Progress to Blocked in PMC Board Jun 16, 2026
@uday-kalyan-s

uday-kalyan-s commented Aug 3, 2026

Copy link
Copy Markdown
Contributor

Proposal to switch to spatial partitioning for broad-phase collision detection

Hi! I'm Uday. I had previously submitted a GSoC proposal to work on this PR, and I'm still very interested in contributing to it.

As part of that proposal, I planned to implement the MAPF-POST improvements from P3GASUS, which includes replacing the current broad-phase collision detection with a more efficient spatial partitioning approach.

Some references I found useful while working on this:

One of the earlier blockers was supporting heterogeneous robots (robots with different sizes). That issue has since been addressed by replacing KD-trees with AABB trees in the graph creation pipeline. I implemented and benchmarked this approach here:

I believe this work provides a solid foundation for implementing the same idea here.

Would it be okay if I started by using this branch in my fork to develop the changes, and later opened a PR targeting the merge-mapf-post branch?

@uday-kalyan-s

uday-kalyan-s commented Aug 3, 2026

Copy link
Copy Markdown
Contributor

@arjo129 @mxgrey

@arjo129

arjo129 commented Aug 4, 2026

Copy link
Copy Markdown
Member Author

Hi @uday-kalyan-s upstream (https://github.com/arjo129/mapf_post) already has a broadphase based acceleration. I would encourage you to look at it and implement the same on this branch.

@uday-kalyan-s

uday-kalyan-s commented Aug 4, 2026

Copy link
Copy Markdown
Contributor

Right yes I found the implementation of the AABB tree there. extremely sorry for my earlier comment. I will get to working on this branch then.

@uday-kalyan-s
Transfer collision detection optimizations from arjo129/mapf_post (#44)
ced6539 
### Implemented feature

Adds an AABB sweep-and-prune broad phase to `mapf::post::mapf_post`'s Type‑2
(cross-agent collision) dependency detection, replacing the previous
brute-force O(N²T²) nested-loop check. Also adds Criterion benchmarks so the
improvement (and any future regression) is measurable rather than just
asserted.

<!-- TODO: link the feature-request/tracking issue here, e.g. Closes #XXXX -->

### Implementation description

The broad phase mirrors the approach already used in the sibling `mapf_post`
crate: for every trajectory segment, compute a merged AABB spanning its
start and end pose, pick whichever axis (X or Y) has the larger spread
across all segments, sort segments by AABB min on that axis, then sweep with
an early-exit break once a later segment's AABB min passes the current
segment's AABB max on that axis. Only AABB-overlapping pairs go through the
exact continuous collision check (`collides`, via
`parry2d::query::cast_shapes_nonlinear`) that used to run on every pair
unconditionally.

Changes:
- `mapf/mapf/src/post/mod.rs`: replaced the brute-force Type‑2 edge loop in
  `mapf_post` with the sweep-and-prune broad phase.
- `mapf/mapf/src/post/mod.rs` (`sweep_line_tests` module): added tests
  comparing the sweep-line output against a brute-force reference
  implementation across several scenarios (crossing, following, head-on
  swap, diagonal, far-apart, unequal-length trajectories), plus a
  sort-axis-invariance test (X-sort vs. Y-sort scenes must agree).

Complexity is unchanged in the worst case (still O((N·T)²) when a scene is
AABB-dense on both axes), but substantially better in the common case of
spatially separated trajectories, since the sweep prunes the AABB
comparisons that no longer need the expensive narrow-phase check.

Signed-off-by: uday-kalyan-s <udaykalyansreenivasa@gmail.com>

* fixed styling

Signed-off-by: uday-kalyan-s <udaykalyansreenivasa@gmail.com>

* code cleanup and better benchmarking

Signed-off-by: uday-kalyan-s <udaykalyansreenivasa@gmail.com>

* moved analysis to cargo bench

Signed-off-by: uday-kalyan-s <udaykalyansreenivasa@gmail.com>

---------

Signed-off-by: uday-kalyan-s <udaykalyansreenivasa@gmail.com>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Reviewers

No reviews

Assignees

No one assigned

Labels

None yet

Projects

PMC Board
Status: Blocked

Milestone

No milestone

Development

Successfully merging this pull request may close these issues.

[Feature request]: Add support for extracting semantic plan

4 participants

@arjo129 @uday-kalyan-s @mxgrey @xiyuoh