Adds mapf benchmark tools to this repo

.gitignore

@@ -1,2 +1,4 @@
 /target
 Cargo.lock
+cache/
+benchmark_report.json

Cargo.toml

@@ -2,6 +2,7 @@
 members = [
     "mapf",
     "mapf-viz",
+    "mapf-bench",
 ]
 
 resolver = "2"

mapf-bench/Cargo.toml

@@ -0,0 +1,13 @@
+[package]
+name = "mapf-bench"
+version = "0.1.0"
+edition = "2021"
+
+[dependencies]
+mapf = { path = "../mapf" }
+movingai = "0.2"
+anyhow = "1.0"
+clap = { version = "4.0", features = ["derive"] }
+serde = { version = "1.0", features = ["derive"] }
+serde_json = "1.0"
+indicatif = "0.17"

mapf-bench/src/main.rs

@@ -0,0 +1,65 @@
+mod movingai;
+
+use anyhow::Result;
+use clap::Parser;
+use std::path::PathBuf;
+use std::time::Instant;
+
+#[derive(Parser, Debug)]
+#[command(author, version, about, long_about = None)]
+struct Args {
+    #[arg(short, long)]
+    map: PathBuf,
+
+    #[arg(short, long)]
+    scen: PathBuf,
+
+    #[arg(short, long, default_value_t = 0.45)]
+    radius: f64,
+
+    #[arg(short, long, default_value_t = 1.0)]
+    speed: f64,
+
+    #[arg(short, long, default_value_t = 10)]
+    num_agents: usize,
+
+    #[arg(short, long, default_value_t = 30)]
+    timeout: u64,
+}
+
+fn main() -> Result<()> {
+    let args = Args::parse();
+
+    println!("Loading map: {:?}", args.map);
+    let map = movingai::Map::from_file(&args.map)?;
+
+    println!("Loading scenario: {:?}", args.scen);
+    let scenario = movingai::MovingAIScenario::from_file(&args.scen)?;
+
+    let negotiation_scenario = scenario.to_negotiation_scenario(
+        &map,
+        args.num_agents,
+        args.radius,
+        args.speed,
+        60_f64.to_radians(),
+    );
+
+    println!("Running negotiation with {} agents", args.num_agents);
+    let start_time = Instant::now();
+    // We don't have a clean way to pass wall-clock timeout into negotiate yet,
+    // so we'll rely on the parent script to enforce strict timeouts for now,
+    // or we could add a QueueLengthLimit as a proxy.
+    let result = mapf::negotiation::negotiate(&negotiation_scenario, None);
+    let duration = start_time.elapsed();
+
+    match result {
+        Ok((_solution, _arena, _name_map)) => {
+            println!("Negotiation successful in {:?}", duration);
+        }
+        Err(e) => {
+            println!("Negotiation failed: {:?}", e);
+        }
+    }
+
+    Ok(())
+}

mapf-bench/src/movingai.rs

@@ -0,0 +1,149 @@
+use std::fs::File;
+use std::io::{BufRead, BufReader};
+use std::path::Path;
+use anyhow::Result;
+use std::collections::HashMap;
+
+use mapf::negotiation::{Agent, Scenario};
+use std::collections::BTreeMap;
+
+pub struct Map {
+    pub width: usize,
+    pub height: usize,
+    pub grid: Vec<Vec<char>>,
+}
+
+impl Map {
+    pub fn from_file<P: AsRef<Path>>(path: P) -> Result<Self> {
+        let file = File::open(path)?;
+        let reader = BufReader::new(file);
+        let mut lines = reader.lines();
+
+        let mut width = 0;
+        let mut height = 0;
+
+        // Parse header
+        while let Some(line) = lines.next() {
+            let line = line?;
+            if line.starts_with("type") {
+                continue;
+            } else if line.starts_with("height") {
+                height = line.split_whitespace().nth(1).unwrap().parse()?;
+            } else if line.starts_with("width") {
+                width = line.split_whitespace().nth(1).unwrap().parse()?;
+            } else if line.starts_with("map") {
+                break;
+            }
+        }
+
+        let mut grid = Vec::with_capacity(height);
+        for _ in 0..height {
+            if let Some(line) = lines.next() {
+                let line = line?;
+                grid.push(line.chars().collect());
+            }
+        }
+
+        Ok(Map { width, height, grid })
+    }
+
+    pub fn to_occupancy_map(&self) -> HashMap<i64, Vec<i64>> {
+        let mut occupancy = HashMap::new();
+        for y in 0..self.height {
+            let mut row = Vec::new();
+            for x in 0..self.width {
+                let c = self.grid[y][x];
+                if c != '.' && c != 'G' && c != 'S' {
+                    row.push(x as i64);
+                }
+            }
+            if !row.is_empty() {
+                occupancy.insert(y as i64, row);
+            }
+        }
+        occupancy
+    }
+}
+
+pub struct ScenarioEntry {
+    pub bucket: usize,
+    pub map_file: String,
+    pub map_width: usize,
+    pub map_height: usize,
+    pub start_x: usize,
+    pub start_y: usize,
+    pub goal_x: usize,
+    pub goal_y: usize,
+    pub optimal_length: f64,
+}
+
+pub struct MovingAIScenario {
+    pub entries: Vec<ScenarioEntry>,
+}
+
+impl MovingAIScenario {
+    pub fn from_file<P: AsRef<Path>>(path: P) -> Result<Self> {
+        let file = File::open(path)?;
+        let reader = BufReader::new(file);
+        let mut lines = reader.lines();
+
+        // Skip version line
+        lines.next();
+
+        let mut entries = Vec::new();
+        for line in lines {
+            let line = line?;
+            let parts: Vec<&str> = line.split_whitespace().collect();
+            if parts.len() < 9 {
+                continue;
+            }
+
+            entries.push(ScenarioEntry {
+                bucket: parts[0].parse()?,
+                map_file: parts[1].to_string(),
+                map_width: parts[2].parse()?,
+                map_height: parts[3].parse()?,
+                start_x: parts[4].parse()?,
+                start_y: parts[5].parse()?,
+                goal_x: parts[6].parse()?,
+                goal_y: parts[7].parse()?,
+                optimal_length: parts[8].parse()?,
+            });
+        }
+
+        Ok(MovingAIScenario { entries })
+    }
+
+    pub fn to_negotiation_scenario(
+        &self,
+        map: &Map,
+        num_agents: usize,
+        radius: f64,
+        speed: f64,
+        spin: f64,
+    ) -> Scenario {
+        let mut agents = BTreeMap::new();
+        for i in 0..num_agents.min(self.entries.len()) {
+            let entry = &self.entries[i];
+            agents.insert(
+                format!("agent_{}", i),
+                Agent {
+                    start: [entry.start_x as i64, entry.start_y as i64],
+                    yaw: 0.0,
+                    goal: [entry.goal_x as i64, entry.goal_y as i64],
+                    radius,
+                    speed,
+                    spin,
+                },
+            );
+        }
+
+        Scenario {
+            agents,
+            obstacles: Vec::new(),
+            occupancy: map.to_occupancy_map(),
+            cell_size: 1.0,
+            camera_bounds: None,
+        }
+    }
+}

mapf-viz/examples/grid.rs

@@ -1316,7 +1316,14 @@ impl App {
         let elapsed = start_time.elapsed();
         println!("Successful planning took {} seconds", elapsed.as_secs_f64());
         dbg!(node_history.len());
-
+        let mut total_length = 0.0;
+        for solution in solution_node.proposals.values() {
+            total_length += solution.meta.trajectory.windows(2).fold(0.0, |acc, w| {
+                (w[0].position.translation.vector - w[1].position.translation.vector).magnitude() + acc
+            });
+
+        }
+        println!("Total length: {total_length}");
         assert!(self.canvas.program.layers.3.solutions.is_empty());
         for (i, proposal) in &solution_node.proposals {
             let name = name_map.get(i).unwrap();