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,11 @@ 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"

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,

mapf/src/negotiation/scenario.rs

@@ -18,10 +18,11 @@
 use crate::{
     graph::occupancy::Cell,
     motion::{
-        se2::{GoalSE2, Orientation, StartSE2, WaypointSE2},
+        se2::{GoalSE2, Orientation, Position, StartSE2, WaypointSE2},
         TimePoint, Trajectory,
     },
 };
+use nalgebra::{Isometry2, Vector2};
 use serde::{Deserialize, Serialize};
 use std::collections::{BTreeMap, HashMap};
 
@@ -69,7 +70,7 @@ impl Agent {
     }
 }
 
-#[derive(Serialize, Deserialize)]
+#[derive(Serialize, Deserialize, Clone)]
 pub struct Obstacle {
     /// Trajectory of the obstacle in terms of (time (s), x cell, y cell)
     pub trajectory: Vec<(f64, i64, i64)>,
@@ -97,9 +98,78 @@ impl Obstacle {
             indefinite_finish: trajectory.has_indefinite_finish_time(),
         }
     }
+
+    pub fn to_agent(&self) -> Option<Agent> {
+        let first = self.trajectory.first()?;
+        let last = self.trajectory.last()?;
+        let yaw = if self.trajectory.len() > 1 {
+            let second = &self.trajectory[1];
+            let dx = (second.1 - first.1) as f64;
+            let dy = (second.2 - first.2) as f64;
+            dy.atan2(dx)
+        } else {
+            0.0
+        };
+
+        Some(Agent {
+            start: [first.1, first.2],
+            yaw,
+            goal: [last.1, last.2],
+            radius: self.radius,
+            speed: default_speed(),
+            spin: default_spin(),
+        })
+    }
+
+    pub fn interpolate(&self, time: f64, cell_size: f64) -> Position {
+        if self.trajectory.is_empty() {
+            return Position::identity();
+        }
+
+        if time <= self.trajectory[0].0 {
+            let (_, x, y) = self.trajectory[0];
+            let p = Cell::new(x, y).center_point(cell_size);
+            return Position::translation(p.x, p.y);
+        }
+
+        if time >= self.trajectory.last().unwrap().0 {
+            let (_, x, y) = *self.trajectory.last().unwrap();
+            let p = Cell::new(x, y).center_point(cell_size);
+            return Position::translation(p.x, p.y);
+        }
+
+        // Binary search for the interval
+        let result = self.trajectory.binary_search_by(|(t, _, _)| {
+            t.partial_cmp(&time).unwrap_or(std::cmp::Ordering::Equal)
+        });
+
+        let idx = match result {
+            Ok(i) => i,
+            Err(i) => i,
+        };
+
+        if idx == 0 {
+            let (_, x, y) = self.trajectory[0];
+            let p = Cell::new(x, y).center_point(cell_size);
+            return Position::translation(p.x, p.y);
+        }
+
+        let (t0, x0, y0) = self.trajectory[idx - 1];
+        let (t1, x1, y1) = self.trajectory[idx];
+
+        let f = (time - t0) / (t1 - t0);
+        let p0 = Cell::new(x0, y0).center_point(cell_size);
+        let p1 = Cell::new(x1, y1).center_point(cell_size);
+
+        let x = p0.x + f * (p1.x - p0.x);
+        let y = p0.y + f * (p1.y - p0.y);
+        let yaw = (p1.y - p0.y).atan2(p1.x - p0.x);
+
+        Isometry2::new(Vector2::new(x, y), yaw)
+    }
 }
 
-#[derive(Serialize, Deserialize)]
+#[derive(Serialize, Deserialize, Clone)]
 pub struct Scenario {
     pub agents: BTreeMap<String, Agent>,
     pub obstacles: Vec<Obstacle>,
@@ -134,3 +204,68 @@ pub fn bool_false() -> bool {
 pub fn is_false(b: &bool) -> bool {
     !b
 }
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use crate::domain::Cost;
+    use crate::negotiation::NegotiationNode;
+    use std::collections::HashMap;
+
+    #[test]
+    fn test_derive_mapf_result_with_obstacles() {
+        let scenario = Scenario {
+            agents: BTreeMap::new(),
+            obstacles: vec![Obstacle {
+                trajectory: vec![(0.0, 0, 0), (1.0, 1, 0)],
+                radius: 0.5,
+                indefinite_start: false,
+                indefinite_finish: false,
+            }],
+            occupancy: HashMap::new(),
+            cell_size: 1.0,
+            camera_bounds: None,
+        };
+
+        // We need a mock solution node.
+        // Proposals can be empty if there are no agents.
+        let solution = NegotiationNode {
+            negotiation: crate::negotiation::Negotiation::default(),
+            proposals: HashMap::new(),
+            environment: crate::motion::CcbsEnvironment::new(std::sync::Arc::new(
+                crate::motion::DynamicEnvironment::new(
+                    crate::motion::CircularProfile::new(0.0, 0.0, 0.0).unwrap(),
+                ),
+            )),
+            keys: std::collections::HashSet::new(),
+            conceded: None,
+            cost: Cost(0.0),
+            depth: 0,
+            outcome: crate::negotiation::NodeOutcome::Success,
+            id: 0,
+            parent: None,
+        };
+
+        let timestep = 0.5;
+        let mapf_result = scenario.derive_mapf_result(&solution, timestep);
+
+        // One obstacle should result in one trajectory
+        assert_eq!(mapf_result.trajectories.len(), 1);
+        assert_eq!(mapf_result.footprints.len(), 1);
+
+        // Obstacle trajectory from t=0 to t=1 with timestep 0.5 should have 3 poses (0.0, 0.5, 1.0)
+        assert_eq!(mapf_result.trajectories[0].poses.len(), 3);
+
+        // Check first and last poses
+        let p0 = mapf_result.trajectories[0].poses[0].translation.vector;
+        let p2 = mapf_result.trajectories[0].poses[2].translation.vector;
+
+        // Cell (0,0) center is (0.5, 0.5)
+        assert!((p0.x - 0.5).abs() < 1e-6);
+        assert!((p0.y - 0.5).abs() < 1e-6);
+
+        // Cell (1,0) center is (1.5, 0.5)
+        assert!((p2.x - 1.5).abs() < 1e-6);
+        assert!((p2.y - 0.5).abs() < 1e-6);
+    }
+}