Merge mapf_post into mapf and align types

mapf/Cargo.toml

@@ -11,7 +11,7 @@ keywords = ["mapf", "multi", "agent", "planning", "search"]
 maintenance = {status = "experimental"}
 
 [dependencies]
-nalgebra = "0.31.1"
+nalgebra = "0.33"
 time-point = "0.1"
 sorted-vec = "0.8.2"
 cached = "0.40"
@@ -27,6 +27,15 @@ smallvec = "1.10"
 serde = { version="1.0", features = ["derive"] }
 serde_yaml = "0.9"
 slotmap = "1.0"
+parry2d = { version = "0.21", package = "parry2d-f64" }
+petgraph = "0.6"
+bresenham = "0.1.1"
+rand = "0.8"
+csv = "1.1"
 
 [dev-dependencies]
 approx = "0.5"
+
+[[bench]]
+name = "grid_scene_timings"
+harness = false

mapf/benches/grid_scene_timings.rs

@@ -0,0 +1,89 @@
+//! Plain `Instant` timing, no benchmarking crate. See `post::timing::grid_scene`
+//! in `src/post/mod.rs` for why this scene is adversarial to the sweep.
+
+use mapf::post::{
+    mapf_post, mapf_post_sweep,
+    na::{Isometry2, Vector2},
+    shape::{Ball, Shape},
+    MapfResult, SemanticPlan, Trajectory,
+};
+use std::collections::HashMap;
+use std::sync::Arc;
+use std::time::{Duration, Instant};
+
+fn grid_scene(n: usize, num_waypoints: usize) -> MapfResult {
+    let side = (n as f64).sqrt().ceil() as usize;
+    let cell_spacing = 3.0;
+    let wiggle = 0.4;
+    let denom = (num_waypoints.max(2) - 1) as f64;
+
+    let mut trajectories = Vec::with_capacity(n);
+    let mut footprints = Vec::with_capacity(n);
+    for idx in 0..n {
+        let col = (idx % side) as f64;
+        let row = (idx / side) as f64;
+        let cx = col * cell_spacing;
+        let cy = row * cell_spacing;
+        let poses = (0..num_waypoints)
+            .map(|wp_idx| {
+                let t = wp_idx as f64 / denom * std::f64::consts::TAU;
+                Isometry2::new(
+                    Vector2::new(cx + t.sin() * wiggle, cy + t.cos() * wiggle),
+                    0.0,
+                )
+            })
+            .collect();
+        trajectories.push(Trajectory { poses });
+        footprints.push(Arc::new(Ball::new(0.49)) as Arc<dyn Shape>);
+    }
+    MapfResult {
+        trajectories,
+        footprints,
+        discretization_timestep: 1.0,
+        agent_name_to_id: HashMap::default(),
+    }
+}
+
+fn time<F: FnMut()>(mut f: F, iters: u32) -> Duration {
+    let start = Instant::now();
+    for _ in 0..iters {
+        f();
+    }
+    start.elapsed() / iters
+}
+
+fn main() {
+    let agent_counts = [
+        1usize, 10, 50, 100, 500, 1000, 2000, 4000, 8000, 16000,
+    ];
+    let traj_lengths = [1usize, 10, 50];
+
+    for (strategy_name, strategy) in [
+        ("sweep", mapf_post_sweep as fn(&MapfResult) -> SemanticPlan),
+        ("aabb_tree", mapf_post as fn(&MapfResult) -> SemanticPlan),
+    ] {
+        println!("\n== {strategy_name}: agents (rows) x trajectory length (columns) ==");
+        print!("{:>10}", "agents");
+        for &tl in &traj_lengths {
+            print!(" {:>14}", format!("len={tl}"));
+        }
+        println!();
+
+        for &n in &agent_counts {
+            print!("{n:>10}");
+            for &tl in &traj_lengths {
+                let scene = grid_scene(n, tl);
+                let segments = n * tl.saturating_sub(1);
+                let iters = if segments > 5_000 { 1 } else { 3 };
+                let elapsed = time(
+                    || {
+                        std::hint::black_box(strategy(&scene));
+                    },
+                    iters,
+                );
+                print!(" {elapsed:>14?}");
+            }
+            println!();
+        }
+    }
+}

mapf/src/lib.rs

@@ -37,6 +37,8 @@ pub mod error;
 
 pub mod premade;
 
+pub mod post;
+
 mod util;
 
 pub mod prelude {

mapf/src/negotiation/mod.rs

@@ -32,7 +32,7 @@ use crate::{
         se2::{DifferentialDriveLineFollow, WaypointSE2},
         trajectory::TrajectoryIter,
         BoundingBox, CcbsConstraint, CcbsEnvironment, CircularProfile, Duration,
-        DynamicCircularObstacle, DynamicEnvironment, TimePoint, Timed, TravelEffortCost,
+        DynamicCircularObstacle, DynamicEnvironment, Motion, TimePoint, Timed, TravelEffortCost,
     },
     planner::{halt::QueueLengthLimit, Planner},
     premade::{SippSE2, StateSippSE2},
@@ -150,7 +150,7 @@ pub fn negotiate(
             Err(err) => {
                 return Err(NegotiationError::PlanningImpossible(
                     format!("{err:?}").to_owned(),
-                ))
+                ));
             }
         }
         .solve()
@@ -653,6 +653,97 @@ impl NegotiationNode {
     }
 }
 
+impl Scenario {
+    pub fn solve(
+        &self,
+        queue_length_limit: Option<usize>,
+    ) -> Result<NegotiationNode, NegotiationError> {
+        let (solution, _, _) = negotiate(self, queue_length_limit)?;
+        Ok(solution)
+    }
+
+    fn derive_mapf_result(
+        &self,
+        solution: &NegotiationNode,
+        timestep: f64,
+    ) -> crate::post::MapfResult {
+        let mut max_finish_time = TimePoint::zero();
+        for proposal in solution.proposals.values() {
+            max_finish_time = max_finish_time.max(proposal.meta.trajectory.finish_motion().time());
+        }
+
+        for obstacle in &self.obstacles {
+            if let Some(last) = obstacle.trajectory.last() {
+                max_finish_time = max_finish_time.max(TimePoint::from_secs_f64(last.0));
+            }
+        }
+
+        let mut trajectories = Vec::new();
+        let mut footprints = Vec::new();
+        let mut agent_name_to_id = std::collections::HashMap::new();
+
+        for (i, (name, agent)) in self.agents.iter().enumerate() {
+            agent_name_to_id.insert(name.clone(), i);
+            footprints.push(
+                std::sync::Arc::new(crate::post::shape::Ball::new(agent.radius))
+                    as std::sync::Arc<dyn crate::post::shape::Shape>,
+            );
+
+            let mut poses = Vec::new();
+            if let Some(proposal) = solution.proposals.get(&i) {
+                let traj = &proposal.meta.trajectory;
+                let motion = traj.motion();
+                let start_time = traj.initial_motion().time();
+
+                let duration = max_finish_time - start_time;
+                let steps = (duration.as_secs_f64() / timestep).ceil() as usize;
+
+                for step in 0..=steps {
+                    let mut t = start_time + Duration::from_secs_f64(step as f64 * timestep);
+                    if t > max_finish_time {
+                        t = max_finish_time;
+                    }
+                    if let Ok(pos) = motion.compute_position(&t) {
+                        poses.push(pos);
+                    }
+                }
+            }
+            trajectories.push(crate::post::Trajectory { poses });
+        }
+
+        for obstacle in &self.obstacles {
+            footprints.push(
+                std::sync::Arc::new(crate::post::shape::Ball::new(obstacle.radius))
+                    as std::sync::Arc<dyn crate::post::shape::Shape>,
+            );
+
+            let mut poses = Vec::new();
+            let steps = (max_finish_time.as_secs_f64() / timestep).ceil() as usize;
+            for step in 0..=steps {
+                let t = step as f64 * timestep;
+                poses.push(obstacle.interpolate(t, self.cell_size));
+            }
+            trajectories.push(crate::post::Trajectory { poses });
+        }
+
+        crate::post::MapfResult {
+            trajectories,
+            footprints,
+            discretization_timestep: timestep,
+            agent_name_to_id,
+        }
+    }
+
+    pub fn derive_semantic_plan(
+        &self,
+        solution: &NegotiationNode,
+        timestep: f64,
+    ) -> crate::post::SemanticPlan {
+        let result = self.derive_mapf_result(solution, timestep);
+        crate::post::mapf_post(&result)
+    }
+}
+
 #[derive(Clone)]
 struct QueueEntry {
     node: NegotiationNode,