Merge mapf_post into mapf and align types

[arjo129]

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