From 6872283cfd2cac38f23584aea9a6e2207ae09de8 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 7 Apr 2025 12:41:19 +0800 Subject: [PATCH 01/12] Fix #27: Panic was being caused by invalid access to bitfield I think the root cause of #27 is this. I'm not sure why it was not being triggered earlier, but seems like we are accessing an 8 bit bitfield but limiting the index to `u8::MAX`instead of 7. Signed-off-by: Arjo Chakravarty Signed-off-by: Arjo Chakravarty --- mapf/src/graph/occupancy/accessibility_graph.rs | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/mapf/src/graph/occupancy/accessibility_graph.rs b/mapf/src/graph/occupancy/accessibility_graph.rs index d91ac89..5a7a718 100644 --- a/mapf/src/graph/occupancy/accessibility_graph.rs +++ b/mapf/src/graph/occupancy/accessibility_graph.rs @@ -192,13 +192,14 @@ pub struct CellDirectionsIter { impl Iterator for CellDirectionsIter { type Item = [i64; 2]; fn next(&mut self) -> Option { - if self.next_dir > u8::MAX as u16 { + if self.next_dir > 7 { + // We have already iterated over all the directions return None; } while self.directions.bit(self.next_dir as usize) != self.accessibility { self.next_dir += 1; - if self.next_dir > u8::MAX as u16 { + if self.next_dir > 7 { return None; } } From f31c9d979e9a5c2bf6329d3347a7349573992cc5 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 10 Apr 2025 14:31:45 +0800 Subject: [PATCH 02/12] Add arclength calculation Signed-off-by: Arjo Chakravarty --- mapf-viz/examples/grid.rs | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/mapf-viz/examples/grid.rs b/mapf-viz/examples/grid.rs index c8229fc..844d370 100644 --- a/mapf-viz/examples/grid.rs +++ b/mapf-viz/examples/grid.rs @@ -1305,7 +1305,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(); From 2e9231804b4b79ee9af47e5fd7bd2cc7eab7f092 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Mon, 16 Jun 2025 17:08:14 +0800 Subject: [PATCH 03/12] Add triage github action (#31) Signed-off-by: Luca Della Vedova Signed-off-by: Arjo Chakravarty --- .github/workflows/triage.yml | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) create mode 100644 .github/workflows/triage.yml diff --git a/.github/workflows/triage.yml b/.github/workflows/triage.yml new file mode 100644 index 0000000..2ff18d8 --- /dev/null +++ b/.github/workflows/triage.yml @@ -0,0 +1,16 @@ +on: + issues: + types: [opened] + pull_request_target: + types: [opened] +name: Ticket opened +jobs: + assign: + name: Add ticket to inbox + runs-on: ubuntu-latest + steps: + - name: Add ticket to inbox + uses: actions/add-to-project@v1.0.2 + with: + project-url: https://github.com/orgs/open-rmf/projects/10 + github-token: ${{ secrets.ADD_TO_PROJECT_PAT }} From 3bd056d811b3d2ff9d30bb0b8dbf0335d0434c07 Mon Sep 17 00:00:00 2001 From: Grey Date: Mon, 30 Jun 2025 20:25:54 +0800 Subject: [PATCH 04/12] Migrate to the stable Rust toolchain (#30) Signed-off-by: Michael X. Grey Signed-off-by: Arjo Chakravarty --- .github/workflows/ci.yaml | 10 +- mapf-viz/Cargo.toml | 4 +- mapf-viz/examples/grid.rs | 48 +- mapf/Cargo.toml | 2 +- mapf/src/algorithm/a_star.rs | 51 +-- mapf/src/algorithm/dijkstra/backward.rs | 42 +- mapf/src/algorithm/dijkstra/forward.rs | 89 +--- mapf/src/algorithm/path.rs | 2 +- mapf/src/algorithm/tree.rs | 29 ++ mapf/src/{domain/mod.rs => domain.rs} | 72 +-- mapf/src/domain/activity.rs | 413 ++++++++++++------ mapf/src/domain/closable/keyed_closed_set.rs | 16 - mapf/src/domain/closable/mod.rs | 12 - .../closable/partial_keyed_closed_set.rs | 16 - .../closable/time_variant_keyed_closed_set.rs | 16 - .../time_variant_partial_keyed_closed_set.rs | 16 - mapf/src/domain/connectable.rs | 48 +- mapf/src/domain/define_trait.rs | 2 +- mapf/src/domain/initializable.rs | 122 +++++- mapf/src/graph/mod.rs | 2 +- .../graph/occupancy/accessibility_graph.rs | 42 +- mapf/src/graph/occupancy/mod.rs | 271 ++++++++---- mapf/src/graph/occupancy/visibility_graph.rs | 276 ++++++------ mapf/src/graph/simple.rs | 30 +- mapf/src/lib.rs | 13 +- mapf/src/motion/environment.rs | 62 ++- mapf/src/motion/r2/direct_travel.rs | 2 +- mapf/src/motion/r2/line_follow.rs | 22 +- mapf/src/motion/safe_interval.rs | 61 ++- .../se2/differential_drive_line_follow.rs | 22 +- mapf/src/motion/se2/space.rs | 238 +++++----- mapf/src/templates/conflict_avoidance.rs | 19 +- mapf/src/templates/graph_motion.rs | 145 ++++-- .../src/templates/incremental_graph_motion.rs | 301 ++++++------- mapf/src/templates/informed_search.rs | 43 +- mapf/src/templates/lazy_graph_motion.rs | 225 +++++++--- mapf/src/util.rs | 67 +-- 37 files changed, 1668 insertions(+), 1183 deletions(-) rename mapf/src/{domain/mod.rs => domain.rs} (70%) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 5237453..5225aa5 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -34,9 +34,6 @@ jobs: .\rustup-init.exe -y --default-host=x86_64-pc-windows-msvc --default-toolchain=none del rustup-init.exe - - name: Set Rust version to nightly - run: rustup default nightly - - name: Add Windows target if: matrix.runs-on == 'windows' run: | @@ -44,6 +41,13 @@ jobs: cargo -V rustup target add x86_64-pc-windows-msvc + - name: Add ubuntu dependencies + if: matrix.runs-on == 'ubuntu' + run: | + sudo apt-get update + sudo apt-get install libglib2.0-dev libatk-bridge2.0-dev libgtk-3-dev + + - name: Cache Cargo uses: actions/cache@v4 with: diff --git a/mapf-viz/Cargo.toml b/mapf-viz/Cargo.toml index 85ac038..5b37e0f 100644 --- a/mapf-viz/Cargo.toml +++ b/mapf-viz/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "mapf-viz" -version = "0.1.1" +version = "0.2.0" edition = "2021" # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html @@ -14,7 +14,7 @@ serde = { version="1.0", features = ["derive"] } serde_yaml = "*" async-std = "1.0" directories-next = "2.0" -native-dialog = "*" +rfd = "0.12" derivative = "*" lyon = "*" nalgebra = "*" diff --git a/mapf-viz/examples/grid.rs b/mapf-viz/examples/grid.rs index 844d370..2f904f0 100644 --- a/mapf-viz/examples/grid.rs +++ b/mapf-viz/examples/grid.rs @@ -14,7 +14,6 @@ * limitations under the License. * */ -#![feature(array_windows, binary_heap_into_iter_sorted)] use clap::Parser; use iced::{ @@ -29,7 +28,10 @@ use iced::{ }; use iced_native; use mapf::{ - algorithm::{tree::NodeContainer, AStarConnect, QueueLength, SearchStatus}, + algorithm::{ + tree::{IntoIterSorted, NodeContainer}, + AStarConnect, QueueLength, SearchStatus, + }, graph::{ occupancy::{Accessibility, AccessibilityGraph, Cell, Grid, SparseGrid}, SharedGraph, @@ -53,7 +55,7 @@ use mapf_viz::{ toggle::{KeyToggler, Toggle, Toggler}, InfiniteGrid, SparseGridAccessibilityVisual, }; -use native_dialog::FileDialog; +use rfd::FileDialog; use std::{ collections::{BTreeMap, HashMap}, sync::Arc, @@ -189,7 +191,7 @@ fn draw_trajectory( color: iced::Color, width: Option, ) { - for [wp0, wp1] in trajectory.array_windows() { + for [wp0, wp1] in trajectory.iter().pairs() { let p0 = wp0.position.translation; let p1 = wp1.position.translation; frame.stroke( @@ -976,7 +978,7 @@ impl App { .0 .queue .clone() - .into_iter_sorted() + .binary_heap_into_iter_sorted() .map(|n| n.0.clone()) .collect(); @@ -1257,7 +1259,7 @@ impl App { .0 .queue .clone() - .into_iter_sorted() + .binary_heap_into_iter_sorted() .map(|n| n.0.clone()) .collect(); self.search = Some((r, search)); @@ -1519,36 +1521,22 @@ impl Application for App { } Message::SaveFile | Message::SaveFileAs => { if matches!(message, Message::SaveFileAs) { - match FileDialog::new().show_save_single_file() { - Ok(f) => match f { - Some(f) => { - self.file_text_input_value = - f.as_path().as_os_str().to_str().unwrap().to_owned() - } - None => return Command::none(), - }, - Err(err) => { - println!("Unable to select file: {err:?}"); - return Command::none(); - } + if let Some(f) = FileDialog::new().save_file() { + self.file_text_input_value = + f.as_path().as_os_str().to_str().unwrap().to_owned(); + } else { + return Command::none(); } } self.save_file(&self.file_text_input_value); } Message::LoadFile => { - match FileDialog::new().show_open_single_file() { - Ok(f) => match f { - Some(value) => { - self.file_text_input_value = - value.as_path().as_os_str().to_str().unwrap().to_owned() - } - None => return Command::none(), - }, - Err(err) => { - println!("Unable to load selected file: {err:?}"); - return Command::none(); - } + if let Some(f) = FileDialog::new().pick_file() { + self.file_text_input_value = + f.as_path().as_os_str().to_str().unwrap().to_owned(); + } else { + return Command::none(); } let (agents, obstacles, grid, zone, success) = diff --git a/mapf/Cargo.toml b/mapf/Cargo.toml index b14771c..30c034c 100644 --- a/mapf/Cargo.toml +++ b/mapf/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "mapf" -version = "0.1.1" +version = "0.2.0" edition = "2021" description = "Base traits and utilities for multi-agent planning" license = "Apache-2.0" diff --git a/mapf/src/algorithm/a_star.rs b/mapf/src/algorithm/a_star.rs index 1143e17..db9c98b 100644 --- a/mapf/src/algorithm/a_star.rs +++ b/mapf/src/algorithm/a_star.rs @@ -95,9 +95,9 @@ impl AStar { impl AStar where - D: Domain + Closable + Activity + Weighted, + D: Domain + Closable + Activity + Weighted, D::State: Clone, - D::ActivityAction: Clone, + D::Action: Clone, D::WeightedError: Into, D::Cost: Ord + Add + Clone, { @@ -147,12 +147,9 @@ where domain: &D, closed_set: &mut D::ClosedSet, queue: &mut TreeFrontierQueue, - arena: &Vec>, + arena: &Vec>, goal: &Goal, - ) -> Result< - Flow<(usize, Node), D>, - AStarSearchError, - > + ) -> Result), D>, AStarSearchError> where D: Satisfiable + Activity, D::SatisfactionError: Into, @@ -193,12 +190,12 @@ where domain: &D, memory: &mut ::Memory, parent_id: usize, - parent: &Node, + parent: &Node, goal: &Goal, ) -> Result<(), AStarSearchError> where D: Activity, - D::ActivityAction: Into, + D::Action: Into, D::ActivityError: Into, D: Informed, D::InformedError: Into, @@ -227,7 +224,7 @@ where parent_id: usize, parent_state: &D::State, parent_cost: &D::Cost, - action: D::ActivityAction, + action: D::Action, child_state: D::State, goal: &Goal, ) -> Result<(), AStarSearchError> @@ -267,9 +264,9 @@ where impl Algorithm for AStar where - D: Domain + Closable + Activity + Weighted, + D: Domain + Closable + Activity + Weighted, { - type Memory = Memory, D::State, D::ActivityAction, D::Cost>; + type Memory = Memory, D::State, D::Action, D::Cost>; } impl Coherent for AStar @@ -278,10 +275,10 @@ where + Initializable + Closable + Activity - + Weighted + + Weighted + Informed, D::State: Clone, - D::ActivityAction: Clone, + D::Action: Clone, D::Cost: Ord + Add + Clone, D::InitialError: Into, D::WeightedError: Into, @@ -299,11 +296,11 @@ where D: Domain + Closable + Activity - + Weighted + + Weighted + Informed + Satisfiable, D::State: Clone, - D::ActivityAction: Clone, + D::Action: Clone, D::Error: Into, D::Cost: Ord + Add + Clone, D::SatisfactionError: Into, @@ -311,7 +308,7 @@ where D::WeightedError: Into, D::InformedError: Into, { - type Solution = Path; + type Solution = Path; type StepError = AStarSearchError; fn step( @@ -348,9 +345,9 @@ impl Configurable for AStar { impl Algorithm for AStarConnect where - D: Domain + Closable + Activity + Weighted, + D: Domain + Closable + Activity + Weighted, { - type Memory = Memory, D::State, D::ActivityAction, D::Cost>; + type Memory = Memory, D::State, D::Action, D::Cost>; } impl Coherent for AStarConnect @@ -359,10 +356,10 @@ where + Initializable + Closable + Activity - + Weighted + + Weighted + Informed, D::State: Clone, - D::ActivityAction: Clone, + D::Action: Clone, D::Cost: Ord + Add + Clone, D::InitialError: Into, D::WeightedError: Into, @@ -380,12 +377,12 @@ where D: Domain + Closable + Activity - + Weighted + + Weighted + Informed + Satisfiable - + Connectable, + + Connectable, D::State: Clone, - D::ActivityAction: Clone, + D::Action: Clone, D::Cost: Ord + Add + Clone, D::SatisfactionError: Into, D::ActivityError: Into, @@ -393,7 +390,7 @@ where D::InformedError: Into, D::ConnectionError: Into, { - type Solution = Path; + type Solution = Path; type StepError = AStarSearchError; fn step( @@ -497,9 +494,9 @@ where /// Control flow return value for functions that constitute step() enum Flow where - D: Domain + Activity + Weighted, + D: Domain + Activity + Weighted, // D::Error: StdError, { Proceed(T), - Return(SearchStatus>), + Return(SearchStatus>), } diff --git a/mapf/src/algorithm/dijkstra/backward.rs b/mapf/src/algorithm/dijkstra/backward.rs index 1087238..cc8b04d 100644 --- a/mapf/src/algorithm/dijkstra/backward.rs +++ b/mapf/src/algorithm/dijkstra/backward.rs @@ -33,22 +33,14 @@ use std::ops::Add; pub struct BackwardDijkstra where - D: Domain - + Keyed - + Activity - + Weighted - + Closable, + D: Domain + Keyed + Activity + Weighted + Closable, { backward: Dijkstra, } impl BackwardDijkstra where - D: Domain - + Keyed - + Activity - + Weighted - + Closable, + D: Domain + Keyed + Activity + Weighted + Closable, { pub fn new(domain: &D) -> Result { Ok(Self { @@ -63,22 +55,14 @@ where impl Algorithm for BackwardDijkstra where - D: Domain - + Keyed - + Activity - + Weighted - + Closable, + D: Domain + Keyed + Activity + Weighted + Closable, { type Memory = BackwardMemory; } pub struct BackwardMemory where - D: Domain - + Keyed - + Activity - + Weighted - + Closable, + D: Domain + Keyed + Activity + Weighted + Closable, { backward: Memory, } @@ -89,12 +73,12 @@ where + Keyring + Initializable + Activity - + Weighted + + Weighted + Closable - + Connectable + + Connectable + ArrivalKeyring, D::ClosedSet: ClosedStatusForKey, - D::ActivityAction: Clone, + D::Action: Clone, D::InitialError: Into, D::ArrivalKeyError: Into, D::WeightedError: Into, @@ -118,13 +102,13 @@ where D: Domain + Reversible + Activity - + Weighted + + Weighted + Keyring + Closable - + Connectable - + Backtrack, + + Connectable + + Backtrack, D::State: Clone, - D::ActivityAction: Clone, + D::Action: Clone, D::Cost: Clone + Ord + Add, D::ClosedSet: ClosedStatusForKey, D::ActivityError: Into, @@ -132,7 +116,7 @@ where D::ConnectionError: Into, D::BacktrackError: Into, { - type Solution = Path; + type Solution = Path; type StepError = DijkstraSearchError; fn step( @@ -159,7 +143,7 @@ where + Reversible + Keyed + Activity - + Weighted + + Weighted + Closable, D::ReversalError: Into, { diff --git a/mapf/src/algorithm/dijkstra/forward.rs b/mapf/src/algorithm/dijkstra/forward.rs index 78a412e..ce6aeff 100644 --- a/mapf/src/algorithm/dijkstra/forward.rs +++ b/mapf/src/algorithm/dijkstra/forward.rs @@ -33,11 +33,7 @@ use std::{ pub struct Dijkstra where - D: Domain - + Keyed - + Activity - + Weighted - + Closable, + D: Domain + Keyed + Activity + Weighted + Closable, { domain: D, cache: Arc>>, @@ -45,22 +41,14 @@ where impl Algorithm for Dijkstra where - D: Domain - + Keyed - + Activity - + Weighted - + Closable, + D: Domain + Keyed + Activity + Weighted + Closable, { type Memory = Memory; } impl Dijkstra where - D: Domain - + Keyed - + Activity - + Weighted - + Closable, + D: Domain + Keyed + Activity + Weighted + Closable, { pub fn new(domain: D) -> Self { Self { @@ -99,12 +87,12 @@ where + Keyring + Initializable + Activity - + Weighted + + Weighted + Closable - + Connectable + + Connectable + ArrivalKeyring, D::ClosedSet: ClosedStatusForKey, - D::ActivityAction: Clone, + D::Action: Clone, D::InitialError: Into, D::ArrivalKeyError: Into, D::WeightedError: Into, @@ -236,18 +224,18 @@ where D: Domain + Keyring + Activity - + Weighted + + Weighted + Closable - + Connectable, + + Connectable, D::State: Clone, - D::ActivityAction: Clone, + D::Action: Clone, D::Cost: Clone + Ord + Add, D::ClosedSet: ClosedStatusForKey, D::ActivityError: Into, D::WeightedError: Into, D::ConnectionError: Into, { - type Solution = Path; + type Solution = Path; type StepError = DijkstraSearchError; fn step( @@ -283,8 +271,7 @@ where if Some(closed_set_len) != mt.last_known_closed_len { // The tree was grown by another search, so let's check // if we already found a solution. - let mut best_solution: Option> = - None; + let mut best_solution: Option> = None; for goal_key in &memory.goal_keys { match tree.closed_set.status_for_key(goal_key) { ClosedStatus::Open => { @@ -476,11 +463,7 @@ where /// previous search remains valid. impl Configurable for Dijkstra where - D: Domain - + Keyed - + Activity - + Weighted - + Closable, + D: Domain + Keyed + Activity + Weighted + Closable, { type Configuration = D::Configuration; fn configure(self, f: F) -> Result @@ -520,22 +503,14 @@ impl From for DijkstraImplError { #[derive(Clone)] struct Cache where - D: Domain - + Keyed - + Activity - + Weighted - + Closable, + D: Domain + Keyed + Activity + Weighted + Closable, { trees: HashMap>, } impl Default for Cache where - D: Domain - + Keyed - + Activity - + Weighted - + Closable, + D: Domain + Keyed + Activity + Weighted + Closable, { fn default() -> Self { Self { @@ -552,16 +527,16 @@ type SharedCachedTree = Arc>>; pub struct CachedTree where - D: Domain + Activity + Weighted + Closable, + D: Domain + Activity + Weighted + Closable, { /// The tree data that has been cached - tree: Tree, Node, D::Cost>, + tree: Tree, Node, D::Cost>, decisive_closed_nodes: Vec, } impl CachedTree where - D: Domain + Activity + Weighted + Closable, + D: Domain + Activity + Weighted + Closable, { fn new(closed_set: D::ClosedSet) -> Self where @@ -573,20 +548,14 @@ where } } - pub fn tree( - &self, - ) -> &Tree, Node, D::Cost> { + pub fn tree(&self) -> &Tree, Node, D::Cost> { &self.tree } } pub struct Memory where - D: Domain - + Keyed - + Activity - + Weighted - + Closable, + D: Domain + Keyed + Activity + Weighted + Closable, { /// Trees that are being grown for this search. // TODO(@mxgrey): Consider using a SmallVec here to avoid heap allocation @@ -606,11 +575,7 @@ where impl Memory where - D: Domain - + Keyed - + Activity - + Weighted - + Closable, + D: Domain + Keyed + Activity + Weighted + Closable, { fn new(trees: Vec>, goal_keys: Vec) -> Self { Self { @@ -625,11 +590,7 @@ where pub struct TreeMemory where - D: Domain - + Keyed - + Activity - + Weighted - + Closable, + D: Domain + Keyed + Activity + Weighted + Closable, { /// A reference to the cache entry that is being searched. tree: SharedCachedTree, @@ -638,16 +599,12 @@ where /// tree in the interim. last_known_closed_len: Option, /// Tracks whether a solution was found for this tree. - solution: Option>, + solution: Option>, } impl TreeMemory where - D: Domain - + Keyed - + Activity - + Weighted - + Closable, + D: Domain + Keyed + Activity + Weighted + Closable, { fn new(tree: SharedCachedTree) -> Self { Self { diff --git a/mapf/src/algorithm/path.rs b/mapf/src/algorithm/path.rs index 0e6f6d1..9cffd16 100644 --- a/mapf/src/algorithm/path.rs +++ b/mapf/src/algorithm/path.rs @@ -144,7 +144,7 @@ impl Path { } /// Make a trajectory out of this path if possible. If producing a path is - /// not possible because no motion is necessary, then creating a trajectory + /// not possible because no motion is necessary, then create a trajectory /// that holds the agent in place at the start location for the given /// `hold_duration`. /// diff --git a/mapf/src/algorithm/tree.rs b/mapf/src/algorithm/tree.rs index ea7e5bf..40ecaad 100644 --- a/mapf/src/algorithm/tree.rs +++ b/mapf/src/algorithm/tree.rs @@ -236,3 +236,32 @@ where self.queue.peek().map(|n| n.0.evaluation.clone()) } } + +/// This is implemented based off of +/// `std::collections::binary_head::IntoIterSorted` which is an unstable feature +/// of the standard library. +pub struct BinaryHeapIntoIterSorted { + inner: BinaryHeap, +} + +impl Iterator for BinaryHeapIntoIterSorted { + type Item = T; + fn next(&mut self) -> Option { + self.inner.pop() + } + + fn size_hint(&self) -> (usize, Option) { + let exact = self.inner.len(); + (exact, Some(exact)) + } +} + +pub trait IntoIterSorted { + fn binary_heap_into_iter_sorted(self) -> BinaryHeapIntoIterSorted; +} + +impl IntoIterSorted for BinaryHeap { + fn binary_heap_into_iter_sorted(self) -> BinaryHeapIntoIterSorted { + BinaryHeapIntoIterSorted { inner: self } + } +} diff --git a/mapf/src/domain/mod.rs b/mapf/src/domain.rs similarity index 70% rename from mapf/src/domain/mod.rs rename to mapf/src/domain.rs index 6ca38fd..1810a26 100644 --- a/mapf/src/domain/mod.rs +++ b/mapf/src/domain.rs @@ -1,5 +1,5 @@ /* - * Copyright (C) 2023 Open Source Robotics Foundation + * Copyright (C) 2025 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -16,53 +16,75 @@ */ /// A domain that is being planned over must, at a minimum, specify a type for -/// its state representation and what kind of actions can be performed. +/// its state representation and a type for errors that may occur while using +/// the domain. +/// +/// Domains may also implement various traits like [`Activity`], [`Weighted`], +/// and [`Informed`] which can be used by planners to search the domain. You can +/// easily gather implementations for these traits into a domain using the +/// `#[derive(Domain)]` macro. /// -/// Use DefineTrait to construct the traits of a domain using the trait -/// impls provided by mapf or by using your own custom defined trait impls. /// -/// Domain trait impls that are provided by mapf out-of-the-box can be found in -/// the sub-modules of this domain module, such as Activity and Dynamics. pub trait Domain { + /// Data structure that represents a state within this domain. type State; + + /// The error type that this domain may produce from its various operations. type Error; } -pub mod action_map; -pub mod activity; -pub mod closable; -pub mod configurable; -pub mod conflict; -pub mod connectable; -pub mod cost; -pub mod define_trait; -pub mod domain_map; -pub mod extrapolator; -pub mod informed; -pub mod initializable; -pub mod keyed; -pub mod reversible; -pub mod satisfiable; -pub mod space; -pub mod state_map; -pub mod weighted; +// pub use mapf_derive::Domain; +pub mod action_map; pub use action_map::*; + +pub mod activity; pub use activity::*; + +pub mod closable; pub use closable::*; -pub use closable::*; + +pub mod configurable; pub use configurable::*; + +pub mod conflict; pub use conflict::*; + +pub mod connectable; pub use connectable::*; + +pub mod cost; pub use cost::Cost; + +pub mod define_trait; pub use define_trait::*; + +pub mod domain_map; pub use domain_map::*; + +pub mod extrapolator; pub use extrapolator::*; + +pub mod informed; pub use informed::*; + +pub mod initializable; pub use initializable::*; + +pub mod keyed; pub use keyed::*; + +pub mod reversible; pub use reversible::*; + +pub mod satisfiable; pub use satisfiable::*; + +pub mod space; pub use space::*; + +pub mod state_map; pub use state_map::*; + +pub mod weighted; pub use weighted::*; diff --git a/mapf/src/domain/activity.rs b/mapf/src/domain/activity.rs index f5fcefe..f2f4c23 100644 --- a/mapf/src/domain/activity.rs +++ b/mapf/src/domain/activity.rs @@ -16,7 +16,7 @@ */ use super::*; -use crate::{error::NoError, util::FlatResultMapTrait}; +use crate::error::NoError; /// The Activity trait describes an activity that can be performed within a /// domain. An activity yields action choices for an agent where those choices @@ -29,17 +29,16 @@ use crate::{error::NoError, util::FlatResultMapTrait}; /// that vertex. pub trait Activity { /// What kind of action is produced by this activity - type ActivityAction; + type Action; /// What kind of error can happen if a bad state is provided type ActivityError; /// Concrete type for the returned container of choices - type Choices<'a>: IntoIterator> - + 'a + type Choices<'a>: IntoIterator> + 'a where Self: 'a, - Self::ActivityAction: 'a, + Self::Action: 'a, Self::ActivityError: 'a, State: 'a; @@ -48,7 +47,7 @@ pub trait Activity { fn choices<'a>(&'a self, from_state: State) -> Self::Choices<'a> where Self: 'a, - Self::ActivityAction: 'a, + Self::Action: 'a, Self::ActivityError: 'a, State: 'a; } @@ -57,20 +56,20 @@ pub trait Activity { /// but it doesn't need to do anything. pub struct NoActivity(std::marker::PhantomData); impl Activity for NoActivity { - type ActivityAction = A; + type Action = A; type ActivityError = NoError; type Choices<'a> = [Result<(A, State), NoError>; 0] where Self: 'a, - Self::ActivityAction: 'a, + Self::Action: 'a, Self::ActivityError: 'a, State: 'a; fn choices<'a>(&'a self, _: State) -> Self::Choices<'a> where Self: 'a, - Self::ActivityAction: 'a, + Self::Action: 'a, Self::ActivityError: 'a, State: 'a, { @@ -139,30 +138,56 @@ where Prop: Activity, Prop::ActivityError: Into, { - type ActivityAction = Prop::ActivityAction; + type Action = Prop::Action; type ActivityError = Base::Error; type Choices<'a> - = - impl Iterator> + 'a + = IncorporatedChoices<'a, Base, Prop> where Self: 'a, Base: 'a, Prop: 'a, - Self::ActivityAction: 'a, + Self::Action: 'a, Self::ActivityError: 'a, Base::State: 'a; fn choices<'a>(&'a self, from_state: Base::State) -> Self::Choices<'a> where Self: 'a, - Self::ActivityAction: 'a, + Self::Action: 'a, Self::ActivityError: 'a, Base::State: 'a, { - self.prop - .choices(from_state) - .into_iter() - .map(|c| c.map(|(a, s)| (a.into(), s.into())).map_err(Into::into)) + let choices = self.prop.choices(from_state).into_iter(); + IncorporatedChoices { choices } + } +} + +pub struct IncorporatedChoices<'a, Base, Prop> +where + Base: Domain, + Base::State: 'a, + Prop: Activity + 'a, + Prop::Action: 'a, + Prop::ActivityError: Into + 'a, +{ + choices: as IntoIterator>::IntoIter, +} + +impl<'a, Base, Prop> Iterator for IncorporatedChoices<'a, Base, Prop> +where + Base: Domain, + Prop: Activity, + Prop::ActivityError: Into, +{ + type Item = Result<(Prop::Action, Base::State), Base::Error>; + + fn next(&mut self) -> Option { + Some( + self.choices + .next()? + .map(|(a, s)| (a.into(), s.into())) + .map_err(Into::into), + ) } } @@ -170,41 +195,72 @@ impl Activity for Chained where Base: Domain + Activity, Base::State: Clone, - Base::ActivityAction: Into, + Base::Action: Into, Base::ActivityError: Into, Prop: Activity, Prop::ActivityError: Into, { - type ActivityAction = Prop::ActivityAction; + type Action = Prop::Action; type ActivityError = Base::Error; type Choices<'a> - = - impl Iterator> + 'a + = ChainedChoices<'a, Base, Prop> where Self: 'a, Base: 'a, Prop: 'a, - Self::ActivityAction: 'a, + Self::Action: 'a, Self::ActivityError: 'a, Base::State: 'a; fn choices<'a>(&'a self, from_state: Base::State) -> Self::Choices<'a> where Self: 'a, - Self::ActivityAction: 'a, + Self::Action: 'a, Self::ActivityError: 'a, Base::State: 'a, { - self.base - .choices(from_state.clone()) - .into_iter() - .map(|c| c.map(|(a, s)| (a.into(), s.into())).map_err(Into::into)) - .chain( - self.prop - .choices(from_state) - .into_iter() - .map(|c| c.map(|(a, s)| (a.into(), s.into())).map_err(Into::into)), - ) + ChainedChoices { + base_choices: self.base.choices(from_state.clone()).into_iter(), + prop_choices: self.prop.choices(from_state).into_iter(), + } + } +} + +pub struct ChainedChoices<'a, Base, Prop> +where + Base: Domain + Activity + 'a, + Base::State: Clone, + Base::Action: Into, + Base::ActivityError: Into + 'a, + Prop: Activity + 'a, + Prop::ActivityError: Into, +{ + base_choices: as IntoIterator>::IntoIter, + prop_choices: as IntoIterator>::IntoIter, +} + +impl<'a, Base, Prop> Iterator for ChainedChoices<'a, Base, Prop> +where + Base: Domain + Activity, + Base::State: Clone, + Base::Action: Into, + Base::ActivityError: Into, + Prop: Activity, + Prop::ActivityError: Into, +{ + type Item = Result<(Prop::Action, Base::State), Base::Error>; + + fn next(&mut self) -> Option { + loop { + if let Some(next) = self.base_choices.next() { + return Some(next.map(|(a, s)| (a.into(), s)).map_err(Into::into)); + } + + return self + .prop_choices + .next() + .map(|r| r.map(|(a, s)| (a, s.into())).map_err(Into::into)); + } } } @@ -213,140 +269,231 @@ where Base: Domain + Activity, Base::State: Clone, Base::ActivityError: Into, - Prop: ActivityModifier, + Prop: ActivityModifier, Prop::ModifiedActionError: Into, { - type ActivityAction = Prop::ModifiedAction; + type Action = Prop::ModifiedAction; type ActivityError = Base::Error; type Choices<'a> - = - impl Iterator> + 'a + = MappedChoices<'a, Base, Prop> where Self: 'a, Base: 'a, Prop: 'a, - Self::ActivityAction: 'a, + Self::Action: 'a, Self::ActivityError: 'a, Base::State: 'a; fn choices<'a>(&'a self, from_state: Base::State) -> Self::Choices<'a> where Self: 'a, - Self::ActivityAction: 'a, + Self::Action: 'a, Self::ActivityError: 'a, Base::State: 'a, { - self.base - .choices(from_state.clone()) - .into_iter() - .flat_map(move |result| { - let from_state = from_state.clone(); - result - .map_err(Into::into) - .flat_result_map(move |(action, to_state)| { - self.prop - .modify_action(from_state.clone(), action, to_state) - .into_iter() - .map(|r| r.map_err(Into::into).map(|(a, s)| (a, s.into()))) - }) - .map(|x| x.flatten()) - }) + let choices = self.base.choices(from_state.clone()).into_iter(); + MappedChoices { + from_state, + choices, + modified_choices: None, + prop: &self.prop, + } + } +} + +pub struct MappedChoices<'a, Base, Prop> +where + Base: Domain + Activity + 'a, + Base::State: Clone, + Base::ActivityError: Into, + Prop: ActivityModifier + 'a, + Prop::ModifiedActionError: Into, +{ + from_state: Base::State, + choices: as IntoIterator>::IntoIter, + modified_choices: Option< as IntoIterator>::IntoIter>, + prop: &'a Prop, +} + +impl<'a, Base, Prop> Iterator for MappedChoices<'a, Base, Prop> +where + Base: Domain + Activity, + Base::State: Clone, + Base::ActivityError: Into, + Prop: ActivityModifier, + Prop::ModifiedActionError: Into, +{ + type Item = Result<(Prop::ModifiedAction, Base::State), Base::Error>; + + fn next(&mut self) -> Option { + loop { + if let Some(modified_choices) = &mut self.modified_choices { + if let Some(next_modified_action) = modified_choices.next() { + let next = next_modified_action.map_err(Into::into); + return Some(next); + } + } + self.modified_choices = None; + + let (from_action, to_state): (Base::Action, Base::State) = match self.choices.next()? { + Ok(next) => next, + Err(err) => return Some(Err(err.into())), + }; + + self.modified_choices = Some( + self.prop + .modify_action(self.from_state.clone(), from_action, to_state) + .into_iter(), + ); + } } } impl Activity for Lifted where Base: Domain, - Lifter: ProjectState - + LiftState - + ActionMap, + Lifter: + ProjectState + LiftState + ActionMap, Lifter::ActionMapError: Into, Lifter::ProjectionError: Into, Lifter::LiftError: Into, Prop: Activity, Base::State: Clone, - Prop::ActivityAction: Clone, + Prop::Action: Clone, Prop::ActivityError: Into, { - type ActivityAction = Lifter::ToAction; + type Action = Lifter::ToAction; type ActivityError = Base::Error; type Choices<'a> - = - impl Iterator> + 'a + = LiftedActivityChoices<'a, Base, Lifter, Prop> where Self: 'a, Base: 'a, Prop: 'a, - Self::ActivityAction: 'a, + Self::Action: 'a, Self::ActivityError: 'a, Base::State: 'a; fn choices<'a>(&'a self, from_state: Base::State) -> Self::Choices<'a> where Self: 'a, - Self::ActivityAction: 'a, + Self::Action: 'a, Self::ActivityError: 'a, Base::State: 'a, { - [self.lifter.project(&from_state)] - .into_iter() - .filter_map( - |r: Result, Lifter::ProjectionError>| r.transpose(), - ) - .flat_map( - move |r: Result| { - let from_state = from_state.clone(); - r.map_err(Into::into) - .flat_result_map(move |projected_state| { - let from_state = from_state.clone(); - self.prop.choices(projected_state).into_iter().flat_map( - move |r: Result< - (Prop::ActivityAction, Lifter::ProjectedState), - Prop::ActivityError, - >| { - let from_state = from_state.clone(); - r.map_err(Into::into) - .flat_result_map(move |(action, state)| { - let from_state = from_state.clone(); - self.lifter - .lift(&from_state, state) - .map_err(Into::into) - .flat_result_map( - move |lifted_state_opt: Option| { - let from_state = from_state.clone(); - let action = action.clone(); - lifted_state_opt.into_iter().flat_map( - move |lifted_state: Base::State| { - let from_state = from_state.clone(); - let action = action.clone(); - self.lifter - .map_action(from_state, action) - .into_iter() - .map(move |r| { - let lifted_state = - lifted_state.clone(); - r.map_err(Into::into).map( - move |a| { - ( - a.into(), - lifted_state, - ) - }, - ) - }) - }, - ) - }, - ) - .map(|x| x.flatten()) - }) - .map(|x| x.flatten()) - }, - ) - }) - .map(|x| x.flatten()) - }, - ) + match self.lifter.project(&from_state) { + Ok(Some(projected_state)) => { + let choices = self.prop.choices(projected_state).into_iter(); + return LiftedActivityChoices::Choices { + from_state, + choices, + // lifted_actions will be filled in while iterating + lifted_actions: None, + lifter: &self.lifter, + }; + } + Ok(None) => { + // This is not actually an error, but we can piggyback on the + // ProjectionError variant to have the desired effect for the + // iterator. + return LiftedActivityChoices::ProjectionError(None); + } + Err(err) => { + return LiftedActivityChoices::ProjectionError(Some(err)); + } + } + } +} + +pub enum LiftedActivityChoices<'a, Base, Lifter, Prop> +where + Base: Domain, + Lifter: + ProjectState + LiftState + ActionMap, + Lifter::ActionMapError: Into + 'a, + Lifter::ProjectionError: Into, + Lifter::ProjectedState: 'a, + Lifter::LiftError: Into, + Prop: Activity + 'a, + Base::State: Clone + 'a, + Prop::Action: Clone + 'a, + Prop::ActivityError: Into + 'a, +{ + ProjectionError(Option), + Choices { + from_state: Base::State, + choices: as IntoIterator>::IntoIter, + lifted_actions: Option<( + Base::State, + as IntoIterator>::IntoIter, + )>, + lifter: &'a Lifter, + }, +} + +impl<'a, Base, Lifter, Prop> Iterator for LiftedActivityChoices<'a, Base, Lifter, Prop> +where + Base: Domain, + Lifter: + ProjectState + LiftState + ActionMap, + Lifter::ActionMapError: Into, + Lifter::ProjectionError: Into, + Lifter::ProjectedState: 'a, + Lifter::LiftError: Into, + Prop: Activity + 'a, + Base::State: Clone + 'a, + Prop::Action: Clone + 'a, + Prop::ActivityError: Into + 'a, + Lifter::ToAction: 'a, +{ + type Item = Result<(Lifter::ToAction, Base::State), Base::Error>; + + fn next(&mut self) -> Option { + loop { + let (from_state, lifted_actions, choices, lifter) = match self { + Self::ProjectionError(error) => { + return error.take().map(Into::into).map(Err); + } + Self::Choices { + from_state, + lifted_actions, + choices, + lifter, + } => (from_state, lifted_actions, choices, lifter), + }; + + if let Some((state, lifted_actions)) = lifted_actions { + if let Some(next_lifted_action) = lifted_actions.next() { + let next = next_lifted_action + .map(|action| (action, state.clone())) + .map_err(Into::into); + return Some(next); + } + } + *lifted_actions = None; + + let next: Result<(Prop::Action, Lifter::ProjectedState), Prop::ActivityError> = + choices.next()?; + + let (action, state) = match next { + Ok(ok) => ok, + Err(err) => return Some(Err(err.into())), + }; + + let lifted_state = match lifter.lift(&from_state, state).map_err(Into::into) { + Ok(lifted_state) => lifted_state, + Err(err) => return Some(Err(err.into())), + }; + + let Some(lifted_state) = lifted_state else { + continue; + }; + + *lifted_actions = Some(( + lifted_state, + lifter.map_action(from_state.clone(), action).into_iter(), + )); + } } } @@ -365,20 +512,21 @@ mod tests { struct Interval(u64); impl Activity for Count { - type ActivityAction = Interval; + type Action = Interval; type ActivityError = NoError; - type Choices<'a> = impl Iterator> + 'a; + type Choices<'a> = Vec>; fn choices<'a>(&'a self, s: u64) -> Self::Choices<'a> where Self: 'a, - Self::ActivityAction: 'a, + Self::Action: 'a, Self::ActivityError: 'a, u64: 'a, { self.by_interval .iter() .map(move |interval| Ok((Interval(*interval), s + *interval))) + .collect() } } @@ -386,8 +534,7 @@ mod tests { impl ActivityModifier for Multiplier { type ModifiedAction = Interval; type ModifiedActionError = NoError; - type ModifiedChoices<'a> = - impl IntoIterator> + 'a; + type ModifiedChoices<'a> = [Result<(Interval, u64), Self::ModifiedActionError>; 1]; fn modify_action<'a>( &'a self, from_state: u64, @@ -407,8 +554,7 @@ mod tests { impl ActivityModifier for DoubleTheOdds { type ModifiedAction = Interval; type ModifiedActionError = NoError; - type ModifiedChoices<'a> = - impl IntoIterator> + 'a; + type ModifiedChoices<'a> = [Result<(Interval, u64), Self::ModifiedActionError>; 1]; fn modify_action<'a>( &'a self, from_state: u64, @@ -439,8 +585,7 @@ mod tests { impl ActivityModifier for MapToTestError { type ModifiedAction = Interval; type ModifiedActionError = TestError; - type ModifiedChoices<'a> = - impl IntoIterator> + 'a; + type ModifiedChoices<'a> = [Result<(Interval, u64), Self::ModifiedActionError>; 1]; fn modify_action<'a>(&'a self, _: u64, _: Interval, _: u64) -> Self::ModifiedChoices<'a> where Interval: 'a, @@ -552,7 +697,7 @@ mod tests { #[derive(Clone, Copy, Debug, Hash, PartialEq, Eq)] struct Buy(u64 /* price per unit */); impl Activity for Buy { - type ActivityAction = Buy; + type Action = Buy; type ActivityError = NoError; type Choices<'a> = Option>; fn choices<'a>(&'a self, mut from_state: Item) -> Option> @@ -571,7 +716,7 @@ mod tests { #[derive(Clone, Copy, Debug, Hash, PartialEq, Eq)] struct Sell(u64 /* price per unit */); impl Activity for Sell { - type ActivityAction = Sell; + type Action = Sell; type ActivityError = NoError; type Choices<'a> = Option>; fn choices<'a>(&'a self, mut from_state: Item) -> Self::Choices<'a> diff --git a/mapf/src/domain/closable/keyed_closed_set.rs b/mapf/src/domain/closable/keyed_closed_set.rs index f534c06..32eb28c 100644 --- a/mapf/src/domain/closable/keyed_closed_set.rs +++ b/mapf/src/domain/closable/keyed_closed_set.rs @@ -120,22 +120,6 @@ where let key = self.keyring.key_for(state); self.container.get(key.borrow()).into() } - - type ClosedSetIter<'a> - = impl Iterator + 'a - where - Self: 'a, - State: 'a, - T: 'a; - - fn iter_closed<'a>(&'a self) -> Self::ClosedSetIter<'a> - where - Self: 'a, - State: 'a, - T: 'a, - { - self.container.values() - } } impl ClosedStatusForKey for KeyedClosedSet { diff --git a/mapf/src/domain/closable/mod.rs b/mapf/src/domain/closable/mod.rs index 4c8d61b..9cc5552 100644 --- a/mapf/src/domain/closable/mod.rs +++ b/mapf/src/domain/closable/mod.rs @@ -108,18 +108,6 @@ pub trait ClosedSet { /// Get the status of the specified state. fn status<'a>(&'a self, state: &State) -> ClosedStatus<'a, T>; - - type ClosedSetIter<'a>: IntoIterator + 'a - where - Self: 'a, - State: 'a, - T: 'a; - - fn iter_closed<'a>(&'a self) -> Self::ClosedSetIter<'a> - where - Self: 'a, - State: 'a, - T: 'a; } /// This trait can supplement [`ClosedSet`] by allowing the closed items to be diff --git a/mapf/src/domain/closable/partial_keyed_closed_set.rs b/mapf/src/domain/closable/partial_keyed_closed_set.rs index 8e13693..de74851 100644 --- a/mapf/src/domain/closable/partial_keyed_closed_set.rs +++ b/mapf/src/domain/closable/partial_keyed_closed_set.rs @@ -140,22 +140,6 @@ where }; self.container.get(key).into() } - - type ClosedSetIter<'a> - = impl Iterator + 'a - where - Self: 'a, - State: 'a, - T: 'a; - - fn iter_closed<'a>(&'a self) -> Self::ClosedSetIter<'a> - where - Self: 'a, - State: 'a, - T: 'a, - { - self.container.values() - } } impl ClosedStatusForKey, T> for PartialKeyedClosedSet diff --git a/mapf/src/domain/closable/time_variant_keyed_closed_set.rs b/mapf/src/domain/closable/time_variant_keyed_closed_set.rs index 3d50588..77e9593 100644 --- a/mapf/src/domain/closable/time_variant_keyed_closed_set.rs +++ b/mapf/src/domain/closable/time_variant_keyed_closed_set.rs @@ -171,22 +171,6 @@ where .flatten() .into() } - - type ClosedSetIter<'a> - = impl Iterator + 'a - where - Self: 'a, - State: 'a, - T: 'a; - - fn iter_closed<'a>(&'a self) -> Self::ClosedSetIter<'a> - where - Self: 'a, - State: 'a, - T: 'a, - { - self.container.values().flat_map(|c| c.values()) - } } impl ClosedStatusForKey for TimeVariantKeyedClosedSet { diff --git a/mapf/src/domain/closable/time_variant_partial_keyed_closed_set.rs b/mapf/src/domain/closable/time_variant_partial_keyed_closed_set.rs index 51feff5..6a7c39e 100644 --- a/mapf/src/domain/closable/time_variant_partial_keyed_closed_set.rs +++ b/mapf/src/domain/closable/time_variant_partial_keyed_closed_set.rs @@ -180,22 +180,6 @@ where .flatten() .into() } - - type ClosedSetIter<'a> - = impl Iterator + 'a - where - Self: 'a, - State: 'a, - T: 'a; - - fn iter_closed<'a>(&'a self) -> Self::ClosedSetIter<'a> - where - Self: 'a, - State: 'a, - T: 'a, - { - self.container.values().flat_map(|c| c.values()) - } } impl ClosedStatusForKey, T> diff --git a/mapf/src/domain/connectable.rs b/mapf/src/domain/connectable.rs index b4f3745..60c76c6 100644 --- a/mapf/src/domain/connectable.rs +++ b/mapf/src/domain/connectable.rs @@ -76,7 +76,7 @@ where { type ConnectionError = Prop::ConnectionError; type Connections<'a> - = impl IntoIterator> + 'a + = ChainedConnections<'a, Base, Prop, State, Action, Target> where Self: 'a, Self::ConnectionError: 'a, @@ -92,13 +92,43 @@ where Action: 'a, Target: 'a, { - self.base - .connect(from_state.clone(), to_target) - .into_iter() - .map(|r| { - r.map(|(action, state)| (action.into(), state)) - .map_err(Into::into) - }) - .chain(self.prop.connect(from_state, to_target).into_iter()) + ChainedConnections:: { + base_connections: self.base.connect(from_state.clone(), to_target).into_iter(), + prop_connections: self.prop.connect(from_state, to_target).into_iter(), + } + } +} + +pub struct ChainedConnections<'a, Base, Prop, State, Action, Target> +where + Base: Connectable + 'a, + Prop: Connectable + 'a, + Base::ConnectionError: 'a, + State: 'a, + Action: 'a, + Target: 'a, +{ + base_connections: as IntoIterator>::IntoIter, + prop_connections: as IntoIterator>::IntoIter, +} + +impl<'a, Base, Prop, State, Action, Target> Iterator + for ChainedConnections<'a, Base, Prop, State, Action, Target> +where + Base: Connectable + 'a, + Prop: Connectable + 'a, + Base::ConnectionError: 'a + Into, + State: 'a, + Action: 'a, + Target: 'a, +{ + type Item = Result<(Action, State), Prop::ConnectionError>; + fn next(&mut self) -> Option { + if let Some(next) = self.base_connections.next() { + let next = next.map_err(Into::into); + return Some(next); + } + + self.prop_connections.next() } } diff --git a/mapf/src/domain/define_trait.rs b/mapf/src/domain/define_trait.rs index 36e5a5c..b30c58e 100644 --- a/mapf/src/domain/define_trait.rs +++ b/mapf/src/domain/define_trait.rs @@ -176,7 +176,7 @@ impl Domain for Lifted { } pub trait Lift { - /// Lifts from the domain of property into the base domain using Lifter. + /// Lifts from the domain of `Prop` into the base domain using Lifter. /// /// This can be used to incorporate subspace behaviors or projected domains /// into your domain. diff --git a/mapf/src/domain/initializable.rs b/mapf/src/domain/initializable.rs index aa894b6..369434b 100644 --- a/mapf/src/domain/initializable.rs +++ b/mapf/src/domain/initializable.rs @@ -115,7 +115,7 @@ where { type InitialError = Init::InitialError; type InitialStates<'a> - = impl Iterator> + 'a + = ManyInitIter<'a, StartIter, Goal, State, Init> where Self: 'a, Self::InitialError: 'a, @@ -128,13 +128,56 @@ where Self: 'a, Self::InitialError: 'a, StartIter: 'a, - Goal: 'a, + Goal: 'a + Clone, State: 'a, { - let to_goal = to_goal.clone(); - from_start - .into_iter() - .flat_map(move |start| self.0.initialize(start, &to_goal)) + ManyInitIter { + current_iter: None, + remaining_iters: from_start.into_iter(), + init: &self.0, + goal: to_goal.clone(), + } + } +} + +pub struct ManyInitIter<'a, StartIter, Goal, State, Init> +where + StartIter: 'a + IntoIterator, + Init: 'a + Initializable, + Goal: 'a, + State: 'a, + Init::InitialError: 'a, +{ + current_iter: Option< as IntoIterator>::IntoIter>, + remaining_iters: StartIter::IntoIter, + init: &'a Init, + goal: Goal, +} + +impl<'a, StartIter, Goal, State, Init> Iterator for ManyInitIter<'a, StartIter, Goal, State, Init> +where + StartIter: 'a + IntoIterator, + Init: 'a + Initializable, + Goal: 'a + Clone, + State: 'a, + Init::InitialError: 'a, +{ + type Item = Result; + fn next(&mut self) -> Option { + loop { + if let Some(iter) = &mut self.current_iter { + if let Some(next) = iter.next() { + return Some(next); + } + } + self.current_iter = None; + + if let Some(next_start) = self.remaining_iters.next() { + self.current_iter = Some(self.init.initialize(next_start, &self.goal).into_iter()); + } else { + return None; + } + } } } @@ -144,11 +187,16 @@ impl Initializable for LiftInit, - Init: Initializable, + Init: Initializable, { type InitialError = Init::InitialError; type InitialStates<'a> - = impl Iterator> + 'a + = LiftInitIter< + as IntoIterator>::IntoIter, + Init::State, + State, + Init::InitialError, + > where Self: 'a, Self::InitialError: 'a, @@ -164,10 +212,26 @@ where Goal: 'a, State: 'a, { - self.0 - .initialize(from_start, to_goal) - .into_iter() - .map(|s| s.into()) + LiftInitIter::<_, Init::State, State, Init::InitialError> { + iter: self.0.initialize(from_start, to_goal).into_iter(), + _ignore: Default::default(), + } + } +} + +pub struct LiftInitIter { + iter: InitIter, + _ignore: std::marker::PhantomData, +} + +impl Iterator for LiftInitIter +where + InitIter: Iterator>, + IterState: Into, +{ + type Item = Result; + fn next(&mut self) -> Option { + self.iter.next().map(|r| r.map(Into::into)) } } @@ -179,7 +243,7 @@ where { type InitialError = Base::Error; type InitialStates<'a> - = impl Iterator> + 'a + = IntoInitialStatesIter<'a, Base, Prop, Start, Goal> where Self: 'a, Base::Error: 'a, @@ -195,10 +259,34 @@ where Start: 'a, Goal: 'a, { - self.prop - .initialize(from_start, to_goal) - .into_iter() - .map(|r| r.map(Into::into).map_err(Into::into)) + IntoInitialStatesIter { + iter: self.prop.initialize(from_start, to_goal).into_iter(), + } + } +} + +pub struct IntoInitialStatesIter<'a, Base, Prop, Start, Goal> +where + Base: 'a + Domain, + Prop: 'a + Initializable, + Prop::InitialError: 'a + Into, + Start: 'a, + Goal: 'a, +{ + iter: as IntoIterator>::IntoIter, +} + +impl<'a, Base, Prop, Start, Goal> Iterator for IntoInitialStatesIter<'a, Base, Prop, Start, Goal> +where + Base: 'a + Domain, + Prop: 'a + Initializable, + Prop::InitialError: 'a + Into, + Start: 'a, + Goal: 'a, +{ + type Item = Result; + fn next(&mut self) -> Option { + self.iter.next().map(|r| r.map_err(Into::into)) } } diff --git a/mapf/src/graph/mod.rs b/mapf/src/graph/mod.rs index 00b095a..0bed7a8 100644 --- a/mapf/src/graph/mod.rs +++ b/mapf/src/graph/mod.rs @@ -93,7 +93,7 @@ pub trait Graph { Self::EdgeAttributes: 'a; /// Get the "lazy" edges that exist between from_key and to_key. Lazy Graphs - /// ("lazy" in the sense of lazy evaluation) can cover more space with less + /// ("lazy" in the sense of "lazy evaluation") can cover more space with less /// branching by not returning all possible edges in the `edges_from_vertex` /// function and instead only returning edges that lead to critical vertices. /// Then those critical vertices can be supplemented with this diff --git a/mapf/src/graph/occupancy/accessibility_graph.rs b/mapf/src/graph/occupancy/accessibility_graph.rs index 5a7a718..e0aeac2 100644 --- a/mapf/src/graph/occupancy/accessibility_graph.rs +++ b/mapf/src/graph/occupancy/accessibility_graph.rs @@ -23,7 +23,6 @@ use crate::{ Graph, }, motion::r2::Point, - util::ForkIter, }; use bitfield::{bitfield, Bit}; use std::{ @@ -65,7 +64,7 @@ impl Graph for AccessibilityGraph { where G: 'a; type EdgeIter<'a> - = impl Iterator + 'a + = AccessibilityGraphEdges where Self: 'a; @@ -85,23 +84,23 @@ impl Graph for AccessibilityGraph { Self::EdgeAttributes: 'a, { if self.accessibility.grid.is_occupied(key) { - return ForkIter::Left(None.into_iter()); + return AccessibilityGraphEdges::None; } let directions = match self.accessibility.constraints.get(key) { Some(constraints) => match constraints { CellAccessibility::Accessible(constraints) => *constraints, - CellAccessibility::Inaccessible => return ForkIter::Left(None.into_iter()), + CellAccessibility::Inaccessible => return AccessibilityGraphEdges::None, }, None => CellDirections::all(), }; let from_cell = *key; - ForkIter::Right( - directions - .iter_from(from_cell) - .map(move |to_cell: Cell| (from_cell, to_cell)), - ) + let directions = directions.into_iter(); + AccessibilityGraphEdges::Edges { + from_cell, + directions, + } } type LazyEdgeIter<'a> @@ -120,6 +119,31 @@ impl Graph for AccessibilityGraph { } } +pub enum AccessibilityGraphEdges { + None, + Edges { + from_cell: Cell, + directions: CellDirectionsIter, + }, +} + +impl Iterator for AccessibilityGraphEdges { + type Item = (Cell, Cell); + fn next(&mut self) -> Option { + let Self::Edges { + from_cell, + directions, + } = self + else { + return None; + }; + + directions + .next() + .map(|[i, j]| (*from_cell, from_cell.shifted(i, j))) + } +} + impl Reversible for AccessibilityGraph { type ReversalError = NoError; fn reversed(&self) -> Result diff --git a/mapf/src/graph/occupancy/mod.rs b/mapf/src/graph/occupancy/mod.rs index a13f62a..23ca977 100644 --- a/mapf/src/graph/occupancy/mod.rs +++ b/mapf/src/graph/occupancy/mod.rs @@ -20,8 +20,13 @@ use crate::{ util::triangular_for, }; use bitfield::{bitfield, Bit, BitMut}; -use std::collections::{hash_map, HashMap, HashSet}; -use std::ops::Sub; +use std::{ + collections::{ + hash_map::{Entry, Iter as HashMapIter}, + HashMap, HashSet, + }, + ops::Sub, +}; use util::LineSegment; pub type Point = nalgebra::geometry::Point2; @@ -367,11 +372,10 @@ impl Visibility { ); } - pub fn iter_points(&self) -> impl Iterator { - self.points - .iter() - .filter(|(_, (blocked_by, _))| blocked_by.is_none()) - .map(|(cell, (_, corner_status))| (cell, corner_status)) + pub fn iter_points(&self) -> VisiblePointsIter<'_> { + VisiblePointsIter { + iter: self.points.iter(), + } } pub fn debug_points(&self) -> &HashMap { @@ -391,78 +395,39 @@ impl Visibility { return &self.edges; } - pub fn calculate_visibility(&self, cell: Cell) -> impl Iterator + '_ { - let visibility_edges = self.edges.get(&cell); - [visibility_edges] - .into_iter() - .filter_map(|x| x) - .flat_map(|edges| { - edges - .into_iter() - .filter(|(_, blocked_by)| blocked_by.is_none()) - .map(|(cell, _)| *cell) - }) - .chain( - // This chain kicks in when visibility_edges returned None, which - // means we need to calculate the visibility for this cell. - [visibility_edges] - .into_iter() - .filter(|x| x.is_none()) - .flat_map(move |_| { - self.iter_points() - .filter(move |(v_cell, corner_status)| { - if !cell.in_visible_quadrant_of(*v_cell, **corner_status) { - return false; - } + pub fn calculate_visibility(&self, cell: Cell) -> VisibleCells<'_, G> { + let visibility_edges = match self.edges.get(&cell) { + Some(visibility_edges) => VisibilityEdges::Precalculated(visibility_edges.into_iter()), + None => VisibilityEdges::Unknown(self.iter_points()), + }; - let cell_size = self.grid.cell_size(); - let p0 = cell.center_point(cell_size); - let p1 = v_cell.center_point(cell_size); - return self - .grid - .is_sweep_occupied(p0, p1, 2.0 * self.agent_radius) - .is_none(); - }) - .map(|(v_cell, _)| *v_cell) - }), - ) - } - - pub fn neighbors(&self, of_cell: Cell) -> impl Iterator + '_ { - [of_cell] - .into_iter() - .filter(|of_cell| { - self.grid() - .is_square_occupied( - of_cell.center_point(self.grid().cell_size()), - 2.0 * self.agent_radius, - ) - .is_none() - }) - .flat_map(move |of_cell| { - [-1, 0, 1].into_iter().flat_map(move |i| { - [-1, 0, 1] - .into_iter() - .filter(move |j| !(i == 0 && *j == 0)) - .filter_map(move |j| { - let cell_size = self.grid().cell_size(); - let neighbor = of_cell.shifted(i, j); - if self - .grid() - .is_sweep_occupied( - of_cell.center_point(cell_size), - neighbor.center_point(cell_size), - 2.0 * self.agent_radius(), - ) - .is_none() - { - Some(neighbor) - } else { - None - } - }) - }) - }) + VisibleCells { + grid: &self.grid, + agent_radius: self.agent_radius, + visibility_edges, + from_cell: cell, + } + } + + pub fn neighbors(&self, of_cell: Cell) -> NeighborsIter<'_, G> { + let from_point = of_cell.center_point(self.grid.cell_size()); + let agent_diameter = 2.0 * self.agent_radius; + let directions = match self.grid.is_square_occupied(from_point, agent_diameter) { + Some(_) => { + // The initial cell is blocked, so we can't actually go in any + // direction. + None + } + None => Some(secondary_cardinal_directions().into_iter()), + }; + + NeighborsIter { + grid: &self.grid, + agent_diameter, + of_cell, + from_point, + directions, + } } /// Get a reference to the underlying occupancy grid. @@ -520,7 +485,7 @@ impl Visibility { if valid { match points.entry(cell) { - hash_map::Entry::Vacant(entry) => { + Entry::Vacant(entry) => { // If this corner point is currently vacant, then we // need to check whether it has any blockers. let blocked_by = grid.is_square_occupied( @@ -534,7 +499,7 @@ impl Visibility { .set(corner, true); new_points.push(cell); } - hash_map::Entry::Occupied(mut entry) => { + Entry::Occupied(mut entry) => { entry.get_mut().1.set(corner, true); let mut remove_connections = Vec::new(); for (other, _) in edges.entry(cell).or_default() { @@ -551,10 +516,10 @@ impl Visibility { } } else { match points.entry(cell) { - hash_map::Entry::Vacant(_) => { + Entry::Vacant(_) => { // Nothing needs to be done } - hash_map::Entry::Occupied(mut entry) => { + Entry::Occupied(mut entry) => { entry.get_mut().1.set(corner, false); if !entry.get().1.is_corner() { // This entry is no longer a corner, so we need @@ -637,7 +602,7 @@ impl Visibility { } let connection_entry = cell_connections.entry(*other); - if let hash_map::Entry::Occupied(_) = connection_entry { + if let Entry::Occupied(_) = connection_entry { // If this connection is already active then we don't need // to do anything here. continue; @@ -660,10 +625,10 @@ impl Visibility { // When .insert_entry becomes stable we can change this match block to // connection_entry.insert_entry(blocked_by); match connection_entry { - hash_map::Entry::Occupied(mut entry) => { + Entry::Occupied(mut entry) => { entry.insert(blocked_by); } - hash_map::Entry::Vacant(entry) => { + Entry::Vacant(entry) => { entry.insert(blocked_by); } } @@ -689,7 +654,7 @@ impl Visibility { triangular_for(points.iter(), |(cell_i, _), (cell_j, _)| { for (changed_cell, occupied) in confirmed_changes { let mut changed_blocker: Option> = None; - if let hash_map::Entry::Occupied(entry) = + if let Entry::Occupied(entry) = &mut edges.entry(**cell_i).or_default().entry(*cell_j) { if *occupied { @@ -738,6 +703,123 @@ impl Visibility { } } +pub struct VisibleCells<'a, G: Grid> { + grid: &'a G, + agent_radius: f64, + visibility_edges: VisibilityEdges<'a>, + from_cell: Cell, +} + +impl<'a, G: Grid> Iterator for VisibleCells<'a, G> { + type Item = Cell; + fn next(&mut self) -> Option { + loop { + match &mut self.visibility_edges { + VisibilityEdges::Precalculated(visibility_edges) => { + if let Some((cell, blocked_by)) = visibility_edges.next() { + if blocked_by.is_some() { + // Skip this cell because it is blocked + continue; + } + + return Some(*cell); + } + } + VisibilityEdges::Unknown(visible_points) => { + if let Some((to_cell, status)) = visible_points.next() { + if !self.from_cell.in_visible_quadrant_of(to_cell, *status) { + // Skip this cell because it's not visible from the + // initial cell. + continue; + } + + let cell_size = self.grid.cell_size(); + let p0 = self.from_cell.center_point(cell_size); + let p1 = to_cell.center_point(cell_size); + if self + .grid + .is_sweep_occupied(p0, p1, 2.0 * self.agent_radius) + .is_some() + { + continue; + } + + return Some(*to_cell); + } + } + } + + return None; + } + } +} + +enum VisibilityEdges<'a> { + Precalculated(HashMapIter<'a, Cell, Option>), + Unknown(VisiblePointsIter<'a>), +} + +pub struct VisiblePointsIter<'a> { + iter: HashMapIter<'a, Cell, (BlockedBy, CornerStatus)>, +} + +impl<'a> Iterator for VisiblePointsIter<'a> { + type Item = (&'a Cell, &'a CornerStatus); + fn next(&mut self) -> Option { + loop { + if let Some((cell, (blocked_by, status))) = self.iter.next() { + if blocked_by.is_some() { + // Skip this point because it is currently blocked + continue; + } + + return Some((cell, status)); + } else { + return None; + } + } + } +} + +pub struct NeighborsIter<'a, G> { + grid: &'a G, + agent_diameter: f64, + of_cell: Cell, + from_point: Point, + directions: Option>, +} + +impl<'a, G: Grid> Iterator for NeighborsIter<'a, G> { + type Item = Cell; + fn next(&mut self) -> Option { + let Some(directions) = self.directions.as_mut() else { + return None; + }; + + loop { + if let Some([i, j]) = directions.next() { + let neighbor = self.of_cell.shifted(i, j); + if self + .grid + .is_sweep_occupied( + self.from_point, + neighbor.center_point(self.grid.cell_size()), + self.agent_diameter, + ) + .is_some() + { + // Skip this point since it's blocked + continue; + } + + return Some(neighbor); + } + + return None; + } + } +} + pub struct UnstableVisibilityAPI<'a, G: Grid> { parent: &'a Visibility, } @@ -779,8 +861,8 @@ impl UniqueCellPairSet { pub struct VisibilityEdgeIter<'a, G: Grid> { visibility: &'a Visibility, - point_iter: hash_map::Iter<'a, Cell, HashMap>, - edge_iter: Option<(&'a Cell, hash_map::Iter<'a, Cell, BlockedBy>)>, + point_iter: HashMapIter<'a, Cell, HashMap>, + edge_iter: Option<(&'a Cell, HashMapIter<'a, Cell, BlockedBy>)>, pair_tracker: UniqueCellPairSet, } @@ -834,6 +916,19 @@ impl<'a, G: Grid> Iterator for VisibilityEdgeIter<'a, G> { } } +pub fn secondary_cardinal_directions() -> [[i64; 2]; 8] { + [ + [-1, -1], + [-1, 0], + [-1, 1], + [0, -1], + [0, 1], + [1, -1], + [1, 0], + [1, 1], + ] +} + pub mod sparse_grid; pub use sparse_grid::SparseGrid; pub mod visibility_graph; diff --git a/mapf/src/graph/occupancy/visibility_graph.rs b/mapf/src/graph/occupancy/visibility_graph.rs index 7cf95bc..de1a296 100644 --- a/mapf/src/graph/occupancy/visibility_graph.rs +++ b/mapf/src/graph/occupancy/visibility_graph.rs @@ -19,13 +19,13 @@ use crate::{ domain::Reversible, error::NoError, graph::{ - occupancy::{Cell, Grid, Point, Visibility}, + occupancy::{Cell, Grid, NeighborsIter, Point, Visibility, VisibleCells}, Edge, Graph, }, util::triangular_for, }; use std::{ - collections::{HashMap, HashSet}, + collections::{hash_set::Iter as HashSetIter, HashMap, HashSet}, sync::Arc, }; @@ -132,7 +132,7 @@ impl Graph for VisibilityGraph { where G: 'a; type EdgeIter<'a> - = impl Iterator + 'a + = VisibilityGraphEdges<'a, G> where Self: 'a; @@ -147,55 +147,12 @@ impl Graph for VisibilityGraph { where Cell: 'a, { - let from_cell = *from_cell; - [from_cell] - .into_iter() - .filter(|from_cell| { - self.visibility - .grid() - .is_square_occupied( - from_cell.center_point(self.visibility.grid().cell_size()), - 2.0 * self.visibility.agent_radius(), - ) - .is_none() - }) - .flat_map(|from_cell| { - self.visibility - .calculate_visibility(from_cell) - .map(move |to_cell| (from_cell, to_cell)) - .filter(|(from_cell, to_cell)| from_cell != to_cell) - }) - .chain({ - let interest = self.visibility_of_interest.get(&from_cell); - interest - .into_iter() - .flat_map(|x| x) - .map(move |point_of_interest| (from_cell, *point_of_interest)) - .chain( - [interest] - .into_iter() - .filter(|x| x.is_none()) - .flat_map(move |_| { - self.points_of_interest - .iter() - .filter(move |poi| { - let to_p = - poi.center_point(self.visibility.grid().cell_size()); - let from_p = from_cell - .center_point(self.visibility.grid().cell_size()); - self.visibility - .grid() - .is_sweep_occupied( - from_p, - to_p, - 2.0 * self.visibility.agent_radius(), - ) - .is_none() - }) - .map(move |poi| (from_cell, *poi)) - }), - ) - }) + VisibilityGraphEdges::new( + *from_cell, + &self.visibility_of_interest, + &self.points_of_interest, + &self.visibility, + ) } type LazyEdgeIter<'a> @@ -312,7 +269,7 @@ impl Graph for NeighborhoodGraph { where G: 'a; type EdgeIter<'a> - = impl Iterator + 'a + = VisibilityGraphEdges<'a, G> where Self: 'a; @@ -328,67 +285,13 @@ impl Graph for NeighborhoodGraph { where Cell: 'a, { - // dbg!("neighborhood graph"); - let from_cell = *from_cell; - let from_p = from_cell.center_point(self.visibility.grid().cell_size()); - [from_cell] - .into_iter() - .filter(move |_| { - // dbg!(from_cell); - self.visibility - .grid() - .is_square_occupied(from_p, 2.0 * self.visibility.agent_radius()) - .is_none() - }) - .flat_map(move |_| { - // dbg!(from_cell); - self.visibility - .calculate_visibility(from_cell) - .filter(move |to_cell| { - // dbg!(to_cell); - // Ignore adjacent cells because those will be given by the - // neighbors iterator below. If we repeat the same cell twice, - // we force the search queue to do unnecessary work. - (to_cell.x - from_cell.x).abs() > 1 || (to_cell.y - from_cell.y).abs() > 1 - }) - .map(move |to_cell| (from_cell, to_cell)) - .chain( - self.visibility - .neighbors(from_cell) - .map(move |to_cell| (from_cell, to_cell)) - .filter(|(from_cell, to_cell)| { - // dbg!((from_cell, to_cell)); - // dbg!(from_cell != to_cell) - from_cell != to_cell - }), - ) - .chain({ - let interest = self.visibility_of_interest.get(&from_cell); - interest - .into_iter() - .flat_map(|x| x) - .map(move |point_of_interest| (from_cell, *point_of_interest)) - .chain([interest].into_iter().filter(|x| x.is_none()).flat_map( - move |_| { - self.points_of_interest - .iter() - .filter(move |poi| { - let to_p = poi - .center_point(self.visibility.grid().cell_size()); - self.visibility - .grid() - .is_sweep_occupied( - from_p, - to_p, - 2.0 * self.visibility.agent_radius(), - ) - .is_none() - }) - .map(move |poi| (from_cell.clone(), *poi)) - }, - )) - }) - }) + VisibilityGraphEdges::new( + *from_cell, + &self.visibility_of_interest, + &self.points_of_interest, + &self.visibility, + ) + .with_neighbors(self.visibility.neighbors(*from_cell)) } type LazyEdgeIter<'a> @@ -443,6 +346,142 @@ impl Graph for NeighborhoodGraph { } } +impl Reversible for NeighborhoodGraph { + type ReversalError = NoError; + fn reversed(&self) -> Result { + // Visibility graphs are always bidirectional, so the reverse is the + // same as the forward. + Ok(self.clone()) + } +} + +pub struct VisibilityGraphEdges<'a, G: Grid> { + grid: &'a G, + agent_diameter: f64, + from_cell: Cell, + from_point: Point, + neighborhood: Option>, +} + +impl<'a, G: Grid> VisibilityGraphEdges<'a, G> { + fn new( + from_cell: Cell, + visibility_of_interest: &'a HashMap>, + points_of_interest: &'a HashSet, + visibility: &'a Visibility, + ) -> Self { + let grid = &visibility.grid; + let from_point = from_cell.center_point(grid.cell_size()); + let agent_diameter = 2.0 * visibility.agent_radius; + + let neighborhood = match grid.is_square_occupied(from_point, agent_diameter) { + Some(_) => { + // The initial cell is blocked, so it is cut off from its neighborhood + None + } + None => { + let (visibility_of_interest, points_of_interest) = + match visibility_of_interest.get(&from_cell) { + Some(visibility_of_interest) => (Some(visibility_of_interest.iter()), None), + None => (None, Some(points_of_interest.iter())), + }; + + Some(NeighborhoodGraphEdgesIters { + visible_cells: visibility.calculate_visibility(from_cell), + neighbors: None, + visibility_of_interest, + points_of_interest, + }) + } + }; + + Self { + grid, + agent_diameter, + from_cell, + from_point, + neighborhood, + } + } + + fn with_neighbors(mut self, neighbors: NeighborsIter<'a, G>) -> Self { + if let Some(neighborhood) = self.neighborhood.as_mut() { + neighborhood.neighbors = Some(neighbors); + } + + self + } +} + +struct NeighborhoodGraphEdgesIters<'a, G: Grid> { + visible_cells: VisibleCells<'a, G>, + neighbors: Option>, + visibility_of_interest: Option>, + points_of_interest: Option>, +} + +impl<'a, G: Grid> Iterator for VisibilityGraphEdges<'a, G> { + type Item = (Cell, Cell); + fn next(&mut self) -> Option { + let Some(neighborhood) = self.neighborhood.as_mut() else { + return None; + }; + + let from_cell = self.from_cell; + + loop { + if let Some(to_cell) = neighborhood.visible_cells.next() { + if neighborhood.neighbors.is_some() { + if (to_cell.x - from_cell.x).abs() <= 1 && (to_cell.y - from_cell.y).abs() <= 1 + { + // Ignore adjacent cells because those will be given by the + // neighbors iterator below. If we repeat the same cell twice, + // we force the search queue to do unnecessary work. + continue; + } + } + + return Some((from_cell, to_cell)); + } + + if let Some(neighbors) = neighborhood.neighbors.as_mut() { + if let Some(to_cell) = neighbors.next() { + if from_cell == to_cell { + // Skip if it's the same cell that we started from + continue; + } + + return Some((from_cell, to_cell)); + } + } + + if let Some(visibility_of_interest) = neighborhood.visibility_of_interest.as_mut() { + if let Some(visible_point) = visibility_of_interest.next() { + return Some((from_cell, *visible_point)); + } + } + + if let Some(points_of_interest) = neighborhood.points_of_interest.as_mut() { + if let Some(point_of_interest) = points_of_interest.next() { + let to_point = point_of_interest.center_point(self.grid.cell_size()); + if self + .grid + .is_sweep_occupied(self.from_point, to_point, self.agent_diameter) + .is_some() + { + // Ignore this point since it does not have visibility + continue; + } + + return Some((from_cell, *point_of_interest)); + } + } + + return None; + } + } +} + impl Edge for (Cell, Cell) { fn from_vertex(&self) -> &Cell { &self.0 @@ -457,15 +496,6 @@ impl Edge for (Cell, Cell) { } } -impl Reversible for NeighborhoodGraph { - type ReversalError = NoError; - fn reversed(&self) -> Result { - // Visibility graphs are always bidirectional, so the reverse is the - // same as the forward. - Ok(self.clone()) - } -} - #[cfg(test)] mod tests { #[test] diff --git a/mapf/src/graph/simple.rs b/mapf/src/graph/simple.rs index defc09a..ac517e7 100644 --- a/mapf/src/graph/simple.rs +++ b/mapf/src/graph/simple.rs @@ -21,6 +21,8 @@ use crate::{ graph::{Edge, Graph}, }; +use std::slice::Iter as SliceIter; + #[derive(Debug, Clone, Default)] pub struct SimpleGraph { pub vertices: Vec, @@ -110,7 +112,7 @@ impl Graph for SimpleGraph { Self::EdgeAttributes: 'a; type EdgeIter<'a> - = impl Iterator + 'a + = SimpleGraphEdges<'a, E> where V: 'a, E: 'a; @@ -124,12 +126,10 @@ impl Graph for SimpleGraph { V: 'a, E: 'a, { - let from_key = *from_key; - self.edges - .get(from_key) - .into_iter() - .flat_map(|outgoing| outgoing.iter()) - .map(move |(to_key, attr)| (from_key, *to_key, attr)) + SimpleGraphEdges { + edges: self.edges.get(*from_key).map(|edges| edges.iter()), + from_key: *from_key, + } } type LazyEdgeIter<'a> @@ -146,3 +146,19 @@ impl Graph for SimpleGraph { [] } } + +pub struct SimpleGraphEdges<'a, E> { + edges: Option>, + from_key: usize, +} + +impl<'a, E> Iterator for SimpleGraphEdges<'a, E> { + type Item = (usize, usize, &'a E); + fn next(&mut self) -> Option { + let from_key = self.from_key; + self.edges + .as_mut() + .map(|edges| edges.next().map(|(to_key, attr)| (from_key, *to_key, attr))) + .flatten() + } +} diff --git a/mapf/src/lib.rs b/mapf/src/lib.rs index 22a2192..3309e9c 100644 --- a/mapf/src/lib.rs +++ b/mapf/src/lib.rs @@ -14,17 +14,8 @@ * limitations under the License. * */ -#![feature( - associated_type_bounds, - type_alias_impl_trait, - impl_trait_in_assoc_type, - result_flattening -)] -// TODO(@mxgrey): Eliminate the need for this by reducing the complexity of -// struct names, e.g. the typename for the complex chain that implements Lifted -// in activity.rs. This will probably go hand-in-hand with removing the need for -// nightly features, and improve compile times all at once. -#![type_length_limit = "157633981"] + +extern crate self as mapf; pub mod domain; diff --git a/mapf/src/motion/environment.rs b/mapf/src/motion/environment.rs index 3d6caad..1f15c57 100644 --- a/mapf/src/motion/environment.rs +++ b/mapf/src/motion/environment.rs @@ -24,6 +24,8 @@ use crate::{ }; use std::{ collections::{hash_map::Entry, HashMap}, + iter::Enumerate, + slice::Iter as SliceIter, sync::Arc, }; @@ -51,7 +53,7 @@ impl Environment> for DynamicEnvironment { type Obstacles<'a> - = impl Iterator> + = SliceIter<'a, DynamicCircularObstacle> where W: 'a; @@ -320,7 +322,7 @@ impl<'e, W: Waypoint, K: Key> Environment { type Obstacles<'a> - = impl Iterator> + = CcbsEnvironmentObstaclesIter<'a, W> where W: 'a, K: 'a, @@ -335,25 +337,43 @@ impl<'e, W: Waypoint, K: Key> Environment(&'a self) -> Self::Obstacles<'a> { - self.view - .base - .obstacles - .iter() - .enumerate() - .map(|(i, obs)| self.view.overlay.obstacles.get(&i).unwrap_or(obs)) - .chain( - self.constraints - .iter() - .flat_map(|x| *x) - .filter_map(|constraint| { - if let Some(mask) = self.view.mask { - if constraint.mask == mask { - return None; - } - } - Some(&constraint.obstacle) - }), - ) + CcbsEnvironmentObstaclesIter { + obstacle_overlay: &self.view.overlay.obstacles, + mask: self.view.mask, + obstacles: self.view.base.obstacles.iter().enumerate(), + constraints: self.constraints.as_ref().map(|obs| obs.iter()), + } + } +} + +pub struct CcbsEnvironmentObstaclesIter<'a, W: Waypoint> { + obstacle_overlay: &'a HashMap>, + mask: Option, + obstacles: Enumerate>>, + constraints: Option>>, +} + +impl<'a, W: Waypoint> Iterator for CcbsEnvironmentObstaclesIter<'a, W> { + type Item = &'a DynamicCircularObstacle; + fn next(&mut self) -> Option { + if let Some((i, obs)) = self.obstacles.next() { + return Some(self.obstacle_overlay.get(&i).unwrap_or(obs)); + } + + loop { + if let Some(constraint) = self.constraints.as_mut().map(|c| c.next()).flatten() { + if let Some(mask) = self.mask { + if constraint.mask == mask { + // Skip this one since it's masked + continue; + } + } + + return Some(&constraint.obstacle); + } + + return None; + } } } diff --git a/mapf/src/motion/r2/direct_travel.rs b/mapf/src/motion/r2/direct_travel.rs index 4f3d61d..6ec3649 100644 --- a/mapf/src/motion/r2/direct_travel.rs +++ b/mapf/src/motion/r2/direct_travel.rs @@ -78,7 +78,7 @@ where .transpose() .map(|x| x.flatten()) }) - .flatten() + .and_then(|x| x) } } diff --git a/mapf/src/motion/r2/line_follow.rs b/mapf/src/motion/r2/line_follow.rs index c0ad752..a2f070a 100644 --- a/mapf/src/motion/r2/line_follow.rs +++ b/mapf/src/motion/r2/line_follow.rs @@ -31,10 +31,9 @@ use crate::{ }, se2, Duration, SafeArrivalTimes, SafeIntervalCache, SafeIntervalMotionError, SpeedLimiter, }, - util::ForkIter, }; use arrayvec::ArrayVec; -use smallvec::SmallVec; +use smallvec::{smallvec, SmallVec}; #[derive(Debug, Clone, Copy, PartialEq)] pub struct LineFollow { @@ -199,8 +198,7 @@ where { type AvoidanceAction = SmallVec<[SafeAction; 5]>; type AvoidanceActionIter<'a> - = impl IntoIterator> - + 'a + = SmallVec<[Result<(Self::AvoidanceAction, WaypointR2), Self::AvoidanceError>; 8]> where Target: 'a, Guidance: 'a, @@ -234,20 +232,14 @@ where ) { Ok(extrapolation) => extrapolation.1, Err(err) => { - return ForkIter::Left( - Some(Err(SafeIntervalMotionError::Extrapolator(err))).into_iter(), - ) + return smallvec![Err(SafeIntervalMotionError::Extrapolator(err))]; } }; let mut safe_arrival_times = match target_key { Some(target_key) => match safe_intervals.safe_intervals_for(&target_key) { Ok(r) => r, - Err(err) => { - return ForkIter::Left( - Some(Err(SafeIntervalMotionError::Cache(err))).into_iter(), - ) - } + Err(err) => return smallvec![Err(SafeIntervalMotionError::Cache(err))], }, None => SafeArrivalTimes::new(), }; @@ -266,7 +258,7 @@ where let ranked_hints = compute_safe_linear_path_wait_hints((&from_point, &to_point), None, &environment_view); - let paths: SmallVec<[_; 5]> = safe_arrival_times + safe_arrival_times .into_iter() .filter_map(move |arrival_time| { compute_safe_arrival_path( @@ -284,9 +276,7 @@ where let wp = *action.last().unwrap().movement().unwrap(); Ok((action, wp)) }) - .collect(); - - ForkIter::Right(paths.into_iter()) + .collect() } } diff --git a/mapf/src/motion/safe_interval.rs b/mapf/src/motion/safe_interval.rs index 427f0b7..e0c4630 100644 --- a/mapf/src/motion/safe_interval.rs +++ b/mapf/src/motion/safe_interval.rs @@ -234,22 +234,6 @@ where None => ClosedStatus::Open, } } - - type ClosedSetIter<'a> - = impl Iterator + 'a - where - Self: 'a, - State: 'a, - T: 'a; - - fn iter_closed<'a>(&'a self) -> Self::ClosedSetIter<'a> - where - Self: 'a, - State: 'a, - T: 'a, - { - self.container.values().flat_map(|c| c.iter()) - } } struct ClosedIntervals { @@ -344,12 +328,45 @@ impl ClosedIntervals { prior.into() } - fn iter<'a>(&'a self) -> impl Iterator + 'a { - self.indefinite_start.iter().chain( - self.intervals - .iter() - .filter_map(|(_, value)| value.as_ref()), - ) + // This had been used for a method in the ClosedSet trait that allows + // debuggers to iterate over all items in the closed set. That method was + // removed to ease the migration to the stable toolchain, but we might + // bring it back in the future, so we leave this as unused for now. + #[allow(unused)] + fn values<'a>(&'a self) -> ClosedIntervalsValuesIter<'a, T> { + ClosedIntervalsValuesIter { + indefinite_start: self.indefinite_start.as_ref(), + intervals: self.intervals.iter(), + } + } +} + +pub struct ClosedIntervalsValuesIter<'a, T> { + indefinite_start: Option<&'a T>, + intervals: std::slice::Iter<'a, (TimePoint, Option)>, +} + +impl<'a, T> Iterator for ClosedIntervalsValuesIter<'a, T> { + type Item = &'a T; + + fn next(&mut self) -> Option { + loop { + if let Some(indefinite_start) = self.indefinite_start.take() { + return Some(indefinite_start); + } + + match self.intervals.next() { + Some((_, value)) => { + if let Some(value) = value { + return Some(value); + } + // If there was no value, then move onto the next iteration + // of the loop. + } + // The values have been exhausted, so return None + None => return None, + } + } } } diff --git a/mapf/src/motion/se2/differential_drive_line_follow.rs b/mapf/src/motion/se2/differential_drive_line_follow.rs index 8b83815..1d2524a 100644 --- a/mapf/src/motion/se2/differential_drive_line_follow.rs +++ b/mapf/src/motion/se2/differential_drive_line_follow.rs @@ -33,10 +33,9 @@ use crate::{ CcbsEnvironment, Duration, MaybeTimed, SafeArrivalTimes, SafeIntervalCache, SafeIntervalMotionError, SpeedLimiter, Timed, }, - util::ForkIter, }; use arrayvec::ArrayVec; -use smallvec::SmallVec; +use smallvec::{smallvec, SmallVec}; use std::{borrow::Borrow, sync::Arc}; use time_point::TimePoint; @@ -384,8 +383,7 @@ where { type AvoidanceAction = SmallVec<[SafeAction; 5]>; type AvoidanceActionIter<'a> - = impl IntoIterator> - + 'a + = SmallVec<[Result<(Self::AvoidanceAction, WaypointSE2), Self::AvoidanceError>; 8]> where Target: 'a, Guidance: 'a, @@ -417,9 +415,7 @@ where Some(target_key) => match safe_intervals.safe_intervals_for(&target_key) { Ok(r) => r, Err(err) => { - return ForkIter::Left( - Some(Err(SafeIntervalMotionError::Cache(err))).into_iter(), - ) + return smallvec![Err(SafeIntervalMotionError::Cache(err))]; } }, None => SafeArrivalTimes::new(), @@ -437,9 +433,7 @@ where { Ok(arrival) => arrival, Err(err) => { - return ForkIter::Left( - Some(Err(SafeIntervalMotionError::Extrapolator(err))).into_iter(), - ) + return smallvec![Err(SafeIntervalMotionError::Extrapolator(err))]; } }; @@ -450,14 +444,14 @@ where if !is_safe_segment((&from_state.clone().into(), &wp0), None, &environment_view) { // We cannot rotate to face the target, so there is no way to // avoid conflicts from the start state. - return ForkIter::Left(None.into_iter()); + return smallvec![]; } } let to_position = match arrival.waypoints.last() { Some(p) => *p, // No motion is needed, the agent is already on the target - None => return ForkIter::Left(Some(Ok((SmallVec::new(), *from_state))).into_iter()), + None => return smallvec![Ok((SmallVec::new(), *from_state))], }; let maybe_oriented = to_target.maybe_oriented(); @@ -471,7 +465,7 @@ where // Add the time when the agent would normally arrive at the vertex. safe_arrival_times.insert(0, to_position.time); - let paths: SmallVec<[_; 5]> = safe_arrival_times + let paths: SmallVec<[_; 8]> = safe_arrival_times .into_iter() .filter_map(move |arrival_time| { compute_safe_arrival_path( @@ -527,7 +521,7 @@ where }) .collect(); - ForkIter::Right(paths.into_iter()) + paths } } diff --git a/mapf/src/motion/se2/space.rs b/mapf/src/motion/se2/space.rs index 2652a2f..115140c 100644 --- a/mapf/src/motion/se2/space.rs +++ b/mapf/src/motion/se2/space.rs @@ -27,7 +27,7 @@ use crate::{ se2::*, IntegrateWaypoints, MaybeTimed, TimePoint, Timed, DEFAULT_ROTATIONAL_THRESHOLD, }, - util::{wrap_to_pi, FlatResultMapTrait}, + util::{wrap_to_pi, ForkIter, IterError}, }; use smallvec::SmallVec; use std::borrow::Borrow; @@ -221,6 +221,12 @@ pub struct KeySE2 { pub orientation: i32, } +impl From> for KeySE2 { + fn from(value: StateSE2) -> Self { + value.key + } +} + impl KeySE2 { pub fn new(vertex: K, angle: f64) -> Self { let angle = wrap_to_pi(angle); @@ -402,73 +408,6 @@ impl StarburstSE2 { direction: 1.0, }) } - - fn starburst_impl<'a>( - from_vertex: G::Key, - to_vertex: G::Key, - graph: &'a G, - direction: f32, - ) -> impl Iterator>> + 'a - where - G: Graph, - G::Key: Clone, - G::Vertex: Positioned, - { - let from_start_clone = from_vertex.clone(); - graph - .vertex(&from_vertex) - .ok_or_else(move || InitializeSE2Error::MissingVertex(from_start_clone)) - .flat_result_map(move |from_p_ref| { - let from_p: Point = from_p_ref.borrow().point(); - let from_start = from_vertex.clone(); - graph - .edges_from_vertex(&from_start) - .into_iter() - .chain(graph.lazy_edges_between(&from_vertex, &to_vertex)) - .map(move |edge| { - graph - .vertex(edge.to_vertex()) - .ok_or_else(|| { - InitializeSE2Error::MissingVertex(edge.to_vertex().clone()) - }) - .map(move |to_p_ref| { - let to_p: Point = to_p_ref.borrow().point(); - (direction as f64 * (to_p - from_p)) - .try_normalize(1e-6) - .map(|v| f64::atan2(v[1], v[0])) - .unwrap_or(0.0) - }) - .map(move |angle| (from_p, angle)) - }) - }) - .map(|r| r.flatten()) - } - - fn starburst<'a>( - &'a self, - from_vertex: G::Key, - to_vertex: G::Key, - ) -> impl Iterator>> + 'a - where - G: Graph, - G::Key: Clone, - G::Vertex: Positioned, - { - Self::starburst_impl( - from_vertex.clone(), - to_vertex.clone(), - &self.graph, - self.direction, - ) - .chain(self.reverse.as_ref().into_iter().flat_map(move |r_graph| { - Self::starburst_impl( - from_vertex.clone(), - to_vertex.clone(), - &r_graph, - -1.0 * self.direction, - ) - })) - } } impl Initializable for StarburstSE2 @@ -482,7 +421,10 @@ where { type InitialError = InitializeSE2Error; type InitialStates<'a> - = impl Iterator> + 'a + = ForkIter< + StarburstInitialStates<'a, G, R, State>, + IterError>, + > where Self: 'a, G: 'a, @@ -499,18 +441,111 @@ where Goal: 'a, State: 'a, { - let from_start = from_start.borrow().clone(); - self.starburst(from_start.clone(), to_goal.borrow().clone()) - .map(move |r| { - let from_start = from_start.clone(); - r.map(move |(p, angle)| { - StateSE2::new( - from_start.clone(), - WaypointSE2::new(TimePoint::zero(), p.x, p.y, angle), - ) - }) - }) - .map(|r| r.map(Into::into)) + let from_key: G::Key = from_start.borrow().clone(); + let to_key: &G::Key = to_goal.borrow(); + let Some(from_vertex_ref) = self.graph.vertex(&from_key) else { + return ForkIter::Right(IterError::new(InitializeSE2Error::MissingVertex(from_key))); + }; + + let explicit_edges = self.graph.edges_from_vertex(&from_key).into_iter(); + let lazy_edges = self.graph.lazy_edges_between(&from_key, to_key).into_iter(); + let explicit_reverse = self + .reverse + .as_ref() + .map(|r| r.edges_from_vertex(&from_key).into_iter()); + let lazy_reverse = self + .reverse + .as_ref() + .map(|r| r.lazy_edges_between(&from_key, to_key).into_iter()); + + ForkIter::Left(StarburstInitialStates { + graph: &self.graph, + from_key, + from_vertex_ref, + explicit_edges, + lazy_edges, + explicit_reverse, + lazy_reverse, + direction: self.direction as f64, + _ignore: Default::default(), + }) + } +} + +impl<'a, G: Graph, const R: u32, State> Iterator for StarburstInitialStates<'a, G, R, State> +where + G::Key: Clone, + G::Vertex: Positioned, + StateSE2: Into, +{ + type Item = Result>; + fn next(&mut self) -> Option { + let edge = self.next_edge()?; + Some(self.make_state(edge).map(Into::into)) + } +} + +pub struct StarburstInitialStates<'a, G: Graph + 'a, const R: u32, State> { + graph: &'a G, + from_key: G::Key, + from_vertex_ref: G::VertexRef<'a>, + explicit_edges: as IntoIterator>::IntoIter, + lazy_edges: as IntoIterator>::IntoIter, + explicit_reverse: Option< as IntoIterator>::IntoIter>, + lazy_reverse: Option< as IntoIterator>::IntoIter>, + direction: f64, + _ignore: std::marker::PhantomData, +} + +impl<'a, G: Graph, const R: u32, State> StarburstInitialStates<'a, G, R, State> +where + G::Key: Clone, + G::Vertex: Positioned, +{ + fn make_waypoint(&self, to_vertex: G::VertexRef<'a>) -> (Point, f64) { + let from_p: Point = self.from_vertex_ref.borrow().point(); + let to_p: Point = to_vertex.borrow().point(); + let angle = self.direction + * (to_p - from_p) + .try_normalize(1e-8) + .map(|v| f64::atan2(v[1], v[0])) + .unwrap_or(0.0); + (from_p, angle) + } + + fn make_state( + &self, + edge: G::Edge<'a>, + ) -> Result, InitializeSE2Error> { + let to_vertex = self + .graph + .vertex(edge.to_vertex()) + .ok_or_else(|| InitializeSE2Error::MissingVertex(edge.to_vertex().clone()))?; + let (point, angle) = self.make_waypoint(to_vertex); + Ok(StateSE2::new( + self.from_key.clone(), + WaypointSE2::new(TimePoint::zero(), point.x, point.y, angle), + )) + } + + fn next_edge(&mut self) -> Option> { + if let Some(edge) = self.explicit_edges.next() { + return Some(edge); + } + + if let Some(edge) = self.lazy_edges.next() { + return Some(edge); + } + + if let Some(edge) = self.explicit_reverse.as_mut().map(|r| r.next()).flatten() { + return Some(edge); + } + + if let Some(edge) = self.lazy_reverse.as_mut().map(|r| r.next()).flatten() { + return Some(edge); + } + + None } } @@ -520,11 +555,14 @@ where G::Key: Clone, G::Vertex: Positioned, Start: Borrow, - Goal: Borrow, + Goal: Borrow + Clone, { type ArrivalKeyError = InitializeSE2Error; type ArrivalKeys<'a> - = impl Iterator, Self::ArrivalKeyError>> + 'a + = ForkIter< + StarburstInitialStates<'a, G, R, KeySE2>, + IterError, InitializeSE2Error>, + > where Self: 'a, G: 'a, @@ -536,14 +574,10 @@ where where Self: 'a, Self::ArrivalKeyError: 'a, + Start: 'a, + Goal: 'a, { - let goal = goal.borrow().clone(); - self - // goal and start are intentionally swapped because Starburst::for_goal - // reverses the graph, allowing us to query for edges that lead into the - // goal. - .starburst(goal.clone(), start.borrow().clone()) - .map(move |r| r.map(|(_, angle)| KeySE2::new(goal.clone(), angle))) + Initializable::<_, _, KeySE2>::initialize(self, goal.clone(), start) } } @@ -597,7 +631,7 @@ where { type InitialError = InitializeSE2Error; type InitialStates<'a> - = impl IntoIterator> + 'a + = SmallVec<[Result; 16]> where Self: 'a, G: 'a, @@ -616,18 +650,12 @@ where State: 'a, { let from_start_clone = from_start.clone(); - let mut all_states: SmallVec<[Result<_, _>; 16]> = self - .starburst - .starburst(from_start.clone(), to_goal.vertex.clone()) - .map(move |r| { - let from_start = from_start.clone(); - r.map(move |(p, angle)| { - StateSE2::new( - from_start.clone(), - WaypointSE2::new(TimePoint::zero(), p.x, p.y, angle), - ) - }) - }) + let mut all_states: SmallVec<[Result<_, _>; 16]> = + Initializable::<_, _, StateSE2>::initialize( + &self.starburst, + from_start, + to_goal, + ) .collect(); let from_start = from_start_clone; @@ -662,7 +690,7 @@ where } } - all_states.into_iter().map(|r| r.map(Into::into)) + all_states.into_iter().map(|r| r.map(Into::into)).collect() } } @@ -674,7 +702,7 @@ where { type ArrivalKeyError = InitializeSE2Error; type ArrivalKeys<'a> - = impl IntoIterator, Self::ArrivalKeyError>> + 'a + = SmallVec<[Result, Self::ArrivalKeyError>; 16]> where Self: 'a, G: 'a, diff --git a/mapf/src/templates/conflict_avoidance.rs b/mapf/src/templates/conflict_avoidance.rs index ff0d707..3975af0 100644 --- a/mapf/src/templates/conflict_avoidance.rs +++ b/mapf/src/templates/conflict_avoidance.rs @@ -41,8 +41,7 @@ where type Extrapolation = Avoider::AvoidanceAction; type ExtrapolationError = Avoider::AvoidanceError; type ExtrapolationIter<'a> - = impl Iterator> - + 'a + = Avoider::AvoidanceActionIter<'a> where Self: 'a, Self::Extrapolation: 'a, @@ -68,14 +67,12 @@ where Guidance: 'a, Key: 'a, { - self.avoider - .avoid_conflicts( - from_state, - to_target, - with_guidance, - for_keys, - &*self.environment, - ) - .into_iter() + self.avoider.avoid_conflicts( + from_state, + to_target, + with_guidance, + for_keys, + &*self.environment, + ) } } diff --git a/mapf/src/templates/graph_motion.rs b/mapf/src/templates/graph_motion.rs index a8b74f7..628e403 100644 --- a/mapf/src/templates/graph_motion.rs +++ b/mapf/src/templates/graph_motion.rs @@ -22,7 +22,6 @@ use crate::{ }, error::{StdError, ThisError}, graph::{Edge, Graph}, - util::FlatResultMapTrait, }; use std::borrow::Borrow; @@ -81,14 +80,13 @@ where E: Extrapolator, E::ExtrapolationError: StdError, { - type ActivityAction = E::Extrapolation; + type Action = E::Extrapolation; type ActivityError = GraphMotionError; type Choices<'a> - = - impl IntoIterator> + 'a + = GraphMotionChoices<'a, S, G, E> where Self: 'a, - Self::ActivityAction: 'a, + Self::Action: 'a, Self::ActivityError: 'a, S::State: 'a, G::EdgeAttributes: 'a, @@ -98,48 +96,19 @@ where fn choices<'a>(&'a self, from_state: S::State) -> Self::Choices<'a> where Self: 'a, - Self::ActivityAction: 'a, + Self::Action: 'a, Self::ActivityError: 'a, S::State: 'a, G::Key: 'a, { - self.graph - .edges_from_vertex(self.space.key_for(&from_state.clone()).borrow().borrow()) - .into_iter() - .flat_map(move |edge| { - let from_state = from_state.clone(); - let to_vertex = edge.to_vertex().clone(); - let edge = edge.attributes().clone(); - self.graph - .vertex(&to_vertex) - .ok_or_else(|| GraphMotionError::MissingVertex(to_vertex.clone())) - .flat_result_map(move |v| { - let from_state = from_state.clone(); - let to_vertex = to_vertex.clone(); - let extrapolations = self.extrapolator.extrapolate( - self.space.waypoint(&from_state).borrow(), - v.borrow(), - &edge, - ( - Some(self.space.key_for(&from_state).borrow().borrow()), - Some(&to_vertex), - ), - ); - - extrapolations.into_iter().map(move |r| { - let to_vertex = to_vertex.clone(); - r.map_err(GraphMotionError::Extrapolator).map( - move |(action, waypoint)| { - let to_vertex = to_vertex.clone(); - let state = - self.space.make_keyed_state(to_vertex.clone(), waypoint); - (action, state) - }, - ) - }) - }) - .map(|x| x.flatten()) - }) + let from_vertex = self.space.key_for(&from_state).borrow().borrow().clone(); + let edges_from_vertex = self.graph.edges_from_vertex(&from_vertex).into_iter(); + GraphMotionChoices { + current_iter: None, + edges_from_vertex, + motion: &self, + from_state, + } } } @@ -252,6 +221,96 @@ where } } +pub struct GraphMotionChoices<'a, S, G, E> +where + S: KeyedSpace, + S::Key: Borrow, + S::State: Clone, + G: Graph, + G::Key: Clone, + G::EdgeAttributes: Clone, + E: Extrapolator, + E::ExtrapolationError: StdError, +{ + current_iter: + Option as IntoIterator>::IntoIter>>, + edges_from_vertex: as IntoIterator>::IntoIter, + motion: &'a GraphMotion, + from_state: S::State, +} + +impl<'a, S, G, E> Iterator for GraphMotionChoices<'a, S, G, E> +where + S: KeyedSpace, + S::Key: Borrow, + S::State: Clone, + G: Graph, + G::Key: Clone, + G::EdgeAttributes: Clone, + E: Extrapolator, + E::ExtrapolationError: StdError, +{ + type Item = + Result<(E::Extrapolation, S::State), GraphMotionError>; + fn next(&mut self) -> Option { + loop { + if let Some(current_iter) = self.current_iter.as_mut() { + if let Some(result) = current_iter.extrapolations.next() { + match result { + Ok((action, waypoint)) => { + let to_vertex = current_iter.to_vertex.clone(); + let state = self.motion.space.make_keyed_state(to_vertex, waypoint); + return Some(Ok((action, state))); + } + Err(err) => { + return Some(Err(GraphMotionError::Extrapolator(err))); + } + } + } + } + self.current_iter = None; + + if let Some(edge) = self.edges_from_vertex.next() { + let to_vertex = edge.to_vertex(); + let Some(v) = self.motion.graph.vertex(to_vertex) else { + return Some(Err(GraphMotionError::MissingVertex(to_vertex.clone()))); + }; + + let extrapolations = self.motion.extrapolator.extrapolate( + self.motion.space.waypoint(&self.from_state).borrow(), + v.borrow(), + &edge.attributes(), + ( + Some( + self.motion + .space + .key_for(&self.from_state) + .borrow() + .borrow(), + ), + Some(&to_vertex), + ), + ); + + self.current_iter = Some(Extrapolations { + to_vertex: to_vertex.clone(), + extrapolations: extrapolations.into_iter(), + }); + // Move back to the start of this loop to immediately iterate + // over the new set of extrapolations. + continue; + } + + return None; + } + } +} + +struct Extrapolations { + to_vertex: K, + extrapolations: E, +} + #[derive(ThisError, Debug)] pub enum GraphMotionError { #[error("The graph is missing the requested vertex [{0:?}]")] diff --git a/mapf/src/templates/incremental_graph_motion.rs b/mapf/src/templates/incremental_graph_motion.rs index 8d92e82..d359b28 100644 --- a/mapf/src/templates/incremental_graph_motion.rs +++ b/mapf/src/templates/incremental_graph_motion.rs @@ -24,7 +24,6 @@ use crate::{ graph::{Edge, Graph}, motion::{MaybeTimed, TimePoint, Timed}, templates::graph_motion::{GraphMotionError, GraphMotionReversalError}, - util::{FlatResultMapTrait, ForkIter}, }; use std::{borrow::Borrow, fmt::Debug}; @@ -90,18 +89,13 @@ where E: IncrementalExtrapolator, E::IncrementalExtrapolationError: StdError, { - type ActivityAction = E::IncrementalExtrapolation; + type Action = E::IncrementalExtrapolation; type ActivityError = GraphMotionError; type Choices<'a> - = impl IntoIterator< - Item = Result< - (Self::ActivityAction, IncrementalState), - Self::ActivityError, - >, - > + 'a + = IncrementalGraphMotionChoices<'a, S, G, E> where Self: 'a, - Self::ActivityAction: 'a, + Self::Action: 'a, Self::ActivityError: 'a, S::State: 'a, G::EdgeAttributes: 'a; @@ -109,149 +103,35 @@ where fn choices<'a>(&'a self, from_state: IncrementalState) -> Self::Choices<'a> where Self: 'a, - Self::ActivityAction: 'a, + Self::Action: 'a, Self::ActivityError: 'a, IncrementalState: 'a, { - if let Some((to_key, with_guidance)) = from_state.target.clone() { - return ForkIter::Left( - [self - .graph - .vertex(&to_key) - .ok_or_else(|| GraphMotionError::MissingVertex(to_key.clone()))] - .into_iter() - .flat_map(move |r| { - let from_state = from_state.clone(); - let to_key = to_key.clone(); - let with_guidance = with_guidance.clone(); - r.flat_result_map(move |to_v_ref| { - let extrapolation = self.extrapolator.incremental_extrapolate( - self.space.waypoint(&from_state.base_state).borrow(), - to_v_ref.borrow(), - &with_guidance, - ( - Some(self.space.key_for(&from_state.base_state).borrow().borrow()), - Some(&to_key), - ), - ); - - let from_state = from_state.clone(); - let to_key = to_key.clone(); - let with_guidance = with_guidance.clone(); - extrapolation.into_iter().map(move |r| { - let from_state = from_state.clone(); - let to_key = to_key.clone(); - let with_guidance = with_guidance.clone(); - r.map_err(GraphMotionError::Extrapolator).map( - move |(action, waypoint, progress)| { - let to_key = to_key.clone(); - let with_guidance = with_guidance.clone(); - if progress.incomplete() { - let key = self - .space - .key_for(&from_state.base_state) - .borrow() - .borrow() - .clone(); - let state = self.space.make_keyed_state(key, waypoint); - let state = IncrementalState { - target: Some((to_key, with_guidance)), - base_state: state, - }; - (action, state) - } else { - let state = self.space.make_keyed_state(to_key, waypoint); - let state = IncrementalState { - target: None, - base_state: state, - }; - (action, state) - } - }, - ) - }) - }) - }) - .map(|x| x.flatten()), - ); - } + if let Some(target) = from_state.target { + // Just keep moving this state towards its target. + IncrementalGraphMotionChoices { + current_iter: None, + next_target: Some(target), + edges_from_vertex: None, + motion: &self, + from_base_state: from_state.base_state, + } + } else { + // No specific target, so we want to expand in every direction from + // this vertex. + let edges_from_vertex = self + .graph + .edges_from_vertex(self.space.key_for(&from_state.base_state).borrow().borrow()) + .into_iter(); - // TODO(@mxgrey): Consider ways to reduce code duplication between the - // left and right forks. - ForkIter::Right( - self.graph - .edges_from_vertex( - self.space - .key_for(&from_state.base_state.clone()) - .borrow() - .borrow(), - ) - .into_iter() - .flat_map(move |edge| { - let from_state = from_state.clone(); - let to_key = edge.to_vertex().clone(); - let with_guidance = edge.attributes().clone(); - self.graph - .vertex(&to_key) - .ok_or_else(|| GraphMotionError::MissingVertex(to_key.clone())) - .flat_result_map(move |v| { - let extrapolation = self.extrapolator.incremental_extrapolate( - self.space.waypoint(&from_state.base_state).borrow(), - v.borrow(), - &with_guidance, - ( - Some( - self.space - .key_for(&from_state.base_state) - .borrow() - .borrow(), - ), - Some(&to_key), - ), - ); - - let from_state = from_state.clone(); - let to_key = to_key.clone(); - let with_guidance = with_guidance.clone(); - extrapolation.into_iter().map(move |r| { - let from_state = from_state.clone(); - let to_key = to_key.clone(); - let with_guidance = with_guidance.clone(); - r.map_err(GraphMotionError::Extrapolator).map( - move |(action, waypoint, progress)| { - if progress.incomplete() { - let key = self - .space - .key_for(&from_state.base_state) - .borrow() - .borrow() - .clone(); - let state = self.space.make_keyed_state(key, waypoint); - let state = IncrementalState { - target: Some(( - to_key.clone(), - with_guidance.clone(), - )), - base_state: state, - }; - (action, state) - } else { - let state = self - .space - .make_keyed_state(to_key.clone(), waypoint); - let state = IncrementalState { - target: None, - base_state: state, - }; - (action, state) - } - }, - ) - }) - }) - .map(|x| x.flatten()) - }), - ) + IncrementalGraphMotionChoices { + current_iter: None, + next_target: None, + edges_from_vertex: Some(edges_from_vertex), + motion: &self, + from_base_state: from_state.base_state, + } + } } } @@ -377,6 +257,131 @@ where } } +pub struct IncrementalGraphMotionChoices<'a, S, G, E> +where + S: KeyedSpace, + G: Graph, + E: IncrementalExtrapolator, +{ + current_iter: Option< + Extrapolations< + G::Key, + G::EdgeAttributes, + as IntoIterator>::IntoIter, + >, + >, + next_target: Option<(G::Key, G::EdgeAttributes)>, + edges_from_vertex: Option< as IntoIterator>::IntoIter>, + motion: &'a IncrementalGraphMotion, + from_base_state: S::State, +} + +impl<'a, S, G, E> Iterator for IncrementalGraphMotionChoices<'a, S, G, E> +where + S: KeyedSpace, + S::Key: Borrow, + S::State: Clone, + G: Graph, + G::Key: Clone, + G::EdgeAttributes: Clone, + E: IncrementalExtrapolator, + E::IncrementalExtrapolationError: StdError, +{ + type Item = Result< + (E::IncrementalExtrapolation, IncrementalState), + GraphMotionError, + >; + + fn next(&mut self) -> Option { + loop { + if let Some(current_iter) = self.current_iter.as_mut() { + if let Some(result) = current_iter.extrapolations.next() { + match result { + Ok((action, waypoint, progress)) => { + let target = ¤t_iter.target; + if progress.incomplete() { + let key = self + .motion + .space + .key_for(&self.from_base_state) + .borrow() + .borrow() + .clone(); + let state = IncrementalState { + target: Some(current_iter.target.clone()), + base_state: self.motion.space.make_keyed_state(key, waypoint), + }; + return Some(Ok((action, state))); + } else { + let key = target.0.clone(); + let state = IncrementalState { + target: None, + base_state: self.motion.space.make_keyed_state(key, waypoint), + }; + return Some(Ok((action, state))); + } + } + Err(err) => { + return Some(Err(GraphMotionError::Extrapolator(err))); + } + } + } + } + self.current_iter = None; + + if let Some((to_key, with_guidance)) = Option::take(&mut self.next_target) { + let Some(to_v_ref) = self.motion.graph.vertex(&to_key) else { + return Some(Err(GraphMotionError::MissingVertex(to_key))); + }; + + let from_waypoint_ref = self.motion.space.waypoint(&self.from_base_state); + let from_waypoint = from_waypoint_ref.borrow(); + let extrapolations = self.motion.extrapolator.incremental_extrapolate( + from_waypoint, + to_v_ref.borrow(), + &with_guidance, + ( + Some( + self.motion + .space + .key_for(&self.from_base_state) + .borrow() + .borrow(), + ), + Some(&to_key), + ), + ); + + self.current_iter = Some(Extrapolations { + target: (to_key, with_guidance), + extrapolations: extrapolations.into_iter(), + }); + // Return to the start of the loop to begin pulling items out + // of the extrapolation iterator. + continue; + } + + if let Some(edges_from_vertex) = self.edges_from_vertex.as_mut() { + if let Some(edge) = edges_from_vertex.next() { + let to_vertex = edge.to_vertex().clone(); + let with_guidance = edge.attributes().clone(); + self.next_target = Some((to_vertex, with_guidance)); + // Return to the start of the loop to extrapolate toward + // this new target. + continue; + } + } + + return None; + } + } +} + +struct Extrapolations { + target: (Key, Guidance), + extrapolations: E, +} + pub struct IncrementalState { target: Option<(G::Key, G::EdgeAttributes)>, base_state: BaseState, diff --git a/mapf/src/templates/informed_search.rs b/mapf/src/templates/informed_search.rs index a12fa4d..fb216ed 100644 --- a/mapf/src/templates/informed_search.rs +++ b/mapf/src/templates/informed_search.rs @@ -293,21 +293,20 @@ impl Activity for InformedSearch, { - type ActivityAction = A::ActivityAction; + type Action = A::Action; type ActivityError = A::ActivityError; type Choices<'a> - = - impl IntoIterator> + 'a + = A::Choices<'a> where Self: 'a, - Self::ActivityAction: 'a, + Self::Action: 'a, Self::ActivityError: 'a, A::State: 'a; fn choices<'a>(&'a self, from_state: A::State) -> Self::Choices<'a> where Self: 'a, - Self::ActivityAction: 'a, + Self::Action: 'a, Self::ActivityError: 'a, A::State: 'a, { @@ -315,11 +314,10 @@ where } } -impl Weighted - for InformedSearch +impl Weighted for InformedSearch where A: Domain + Activity, - W: Weighted, + W: Weighted, W::WeightedError: Into, { type Cost = W::Cost; @@ -327,7 +325,7 @@ where fn cost( &self, from_state: &A::State, - action: &A::ActivityAction, + action: &A::Action, to_state: &A::State, ) -> Result, Self::WeightedError> { self.weight.cost(from_state, action, to_state) @@ -374,36 +372,32 @@ where } } -impl Connectable +impl Connectable for InformedSearch where A: Domain + Activity, A::State: Clone, - C: Connectable, + C: Connectable, C::ConnectionError: Into, { - type ConnectionError = anyhow::Error; + type ConnectionError = C::ConnectionError; type Connections<'a> - = - impl IntoIterator> + 'a + = C::Connections<'a> where Self: 'a, Self::ConnectionError: 'a, A::State: 'a, - A::ActivityAction: 'a, + A::Action: 'a, Goal: 'a; fn connect<'a>(&'a self, from_state: A::State, to_target: &'a Goal) -> Self::Connections<'a> where Self::ConnectionError: 'a, A::State: 'a, - A::ActivityAction: 'a, + A::Action: 'a, Goal: 'a, { - self.connector - .connect(from_state.clone(), to_target) - .into_iter() - .map(|r| r.map_err(Into::into)) + self.connector.connect(from_state.clone(), to_target) } } @@ -516,10 +510,9 @@ where } } -impl Backtrack - for InformedSearch +impl Backtrack for InformedSearch where - A: Domain + Activity + Backtrack, + A: Domain + Activity + Backtrack, { type BacktrackError = A::BacktrackError; fn flip_endpoints( @@ -535,9 +528,9 @@ where &self, parent_forward_state: &A::State, parent_reverse_state: &A::State, - reverse_action: &A::ActivityAction, + reverse_action: &A::Action, child_reverse_state: &A::State, - ) -> Result<(A::ActivityAction, A::State), Self::BacktrackError> { + ) -> Result<(A::Action, A::State), Self::BacktrackError> { self.activity.backtrack( parent_forward_state, parent_reverse_state, diff --git a/mapf/src/templates/lazy_graph_motion.rs b/mapf/src/templates/lazy_graph_motion.rs index 9d4f899..3de2057 100644 --- a/mapf/src/templates/lazy_graph_motion.rs +++ b/mapf/src/templates/lazy_graph_motion.rs @@ -20,7 +20,6 @@ use crate::{ error::ThisError, graph::{Edge, Graph}, templates::GraphMotion, - util::FlatResultMapTrait, }; use std::borrow::Borrow; @@ -56,7 +55,7 @@ where C: Connectable, { type Connections<'a> - = impl Iterator> + 'a + = LazyGraphMotionConnections<'a, S, G, E, R, C, State, Action, Goal> where Self: 'a, Self::ConnectionError: 'a, @@ -74,77 +73,26 @@ where Action: 'a, Goal: 'a, { - let from_spatial_state: S::State = from_state.clone().borrow().clone(); - let from_key: G::Key = self - .motion - .space - .key_for(&from_spatial_state) - .borrow() - .borrow() - .clone(); - self.keyring + let from_spatial_state = from_state.borrow().clone(); + let from_key_ref = self.motion.space.key_for(&from_spatial_state); + let from_key = from_key_ref.borrow().borrow(); + let arrival_keys = self + .keyring .get_arrival_keys(&from_key, to_target) - .into_iter() - .flat_map(move |r: Result| { - let from_spatial_state = from_spatial_state.clone(); - let from_key = from_key.clone(); - r.map_err(LazyGraphMotionError::Keyring) - .flat_result_map(move |to_vertex: G::Key| { - let from_key = from_key.clone(); - let from_spatial_state = from_spatial_state.clone(); - self.motion - .graph - .lazy_edges_between(&from_key, &to_vertex) - .into_iter() - .flat_map(move |edge| { - let from_key = from_key.clone(); - let from_state = from_spatial_state.clone(); - let to_vertex = to_vertex.clone(); - let edge: G::EdgeAttributes = edge.attributes().clone(); + .into_iter(); - self.motion - .graph - .vertex(&to_vertex) - .ok_or_else(|| { - LazyGraphMotionError::MissingVertex(to_vertex.clone()) - }) - .flat_result_map(move |v| { - let from_key = from_key.clone(); - let from_state = from_state.clone(); - let to_vertex = to_vertex.clone(); - let extrapolations = self.motion.extrapolator.extrapolate( - self.motion.space.waypoint(&from_state).borrow(), - v.borrow(), - &edge, - (Some(&from_key), Some(&to_vertex)), - ); - - extrapolations.into_iter().map(move |r| { - let to_vertex = to_vertex.clone(); - r.map_err(LazyGraphMotionError::Extrapolator).map( - move |(action, waypoint)| { - let to_vertex = to_vertex.clone(); - let state = self.motion.space.make_keyed_state( - to_vertex.clone(), - waypoint, - ); - (action.into(), state.into()) - }, - ) - }) - }) - .map(|x| x.flatten()) - }) - }) - .map(|x| x.flatten()) - .map(|x: Result<(Action, State), Self::ConnectionError>| x) - }) - .chain( - self.chain - .connect(from_state, to_target) - .into_iter() - .map(|r| r.map_err(LazyGraphMotionError::Chain)), - ) + LazyGraphMotionConnections::<'a, S, G, E, R, C, State, Action, Goal> { + current_iter: None, + lazy_edges: None, + arrival_keys, + chained_connections: self + .chain + .connect(from_state.clone(), to_target) + .into_iter(), + motion: &self.motion, + from_state: from_spatial_state.clone(), + _ignore: Default::default(), + } } } @@ -183,6 +131,141 @@ where } } +pub struct LazyGraphMotionConnections<'a, S, G, E, R, C, State, Action, Goal> +where + S: 'a + KeyedSpace, + S::Key: 'a + Borrow, + S::State: 'a + Into + Clone, + State: 'a + Borrow + Clone, + G: 'a + Graph, + G::Key: 'a + Clone, + G::EdgeAttributes: 'a + Clone, + E: 'a + Extrapolator, + E::Extrapolation: 'a + Into, + Action: 'a + Into, + R: 'a + ArrivalKeyring, + R::ArrivalKeyError: 'a, + C: 'a + Connectable, + State: 'a, + Action: 'a, + Goal: 'a, +{ + current_iter: + Option as IntoIterator>::IntoIter>>, + lazy_edges: Option< as IntoIterator>::IntoIter>, + arrival_keys: as IntoIterator>::IntoIter, + chained_connections: as IntoIterator>::IntoIter, + motion: &'a GraphMotion, + from_state: S::State, + _ignore: std::marker::PhantomData, +} + +impl<'a, S, G, E, R, C, State, Action, Goal> Iterator + for LazyGraphMotionConnections<'a, S, G, E, R, C, State, Action, Goal> +where + S: 'a + KeyedSpace, + S::Key: 'a + Borrow, + S::State: 'a + Into + Clone, + State: 'a + Borrow + Clone, + G: 'a + Graph, + G::Key: 'a + Clone, + G::EdgeAttributes: 'a + Clone, + E: 'a + Extrapolator, + E::Extrapolation: 'a + Into, + Action: 'a + Into, + R: 'a + ArrivalKeyring, + R::ArrivalKeyError: 'a, + C: 'a + Connectable, + State: 'a, + Action: 'a, + Goal: 'a, +{ + type Item = Result< + (Action, State), + LazyGraphMotionError, + >; + fn next(&mut self) -> Option { + loop { + if let Some(current_iter) = self.current_iter.as_mut() { + if let Some(result) = current_iter.extrapolations.next() { + match result { + Ok((action, waypoint)) => { + let to_vertex = current_iter.to_vertex.clone(); + let state = self.motion.space.make_keyed_state(to_vertex, waypoint); + return Some(Ok((action.into(), state.into()))); + } + Err(err) => { + return Some(Err(LazyGraphMotionError::Extrapolator(err))); + } + } + } + } + self.current_iter = None; + + if let Some(lazy_edges) = self.lazy_edges.as_mut() { + if let Some(edge) = lazy_edges.next() { + let to_vertex = edge.to_vertex(); + let Some(v) = self.motion.graph.vertex(to_vertex) else { + return Some(Err(LazyGraphMotionError::MissingVertex(to_vertex.clone()))); + }; + + let extrapolations = self.motion.extrapolator.extrapolate( + self.motion.space.waypoint(&self.from_state).borrow(), + v.borrow(), + &edge.attributes(), + ( + Some( + self.motion + .space + .key_for(&self.from_state) + .borrow() + .borrow(), + ), + Some(&to_vertex), + ), + ); + + self.current_iter = Some(Extrapolations { + to_vertex: to_vertex.clone(), + extrapolations: extrapolations.into_iter(), + }); + // Restart the loop so we can immediately start iterating + // over these extrapolations. + continue; + } + } + + if let Some(result) = self.arrival_keys.next() { + let to_vertex = match result { + Ok(to_vertex) => to_vertex, + Err(err) => { + return Some(Err(LazyGraphMotionError::Keyring(err))); + } + }; + + let from_key_ref = self.motion.space.key_for(&self.from_state); + let from_key = from_key_ref.borrow().borrow(); + let lazy_edges = self.motion.graph.lazy_edges_between(from_key, &to_vertex); + self.lazy_edges = Some(lazy_edges.into_iter()); + // Restart the loop so we can immediately start iterating over + // these lazy edges. + continue; + } + + if let Some(next_in_chain) = self.chained_connections.next() { + return Some(next_in_chain.map_err(LazyGraphMotionError::Chain)); + } + + return None; + } + } +} + +struct Extrapolations { + to_vertex: K, + extrapolations: E, +} + #[derive(Debug, ThisError)] pub enum LazyGraphMotionError { #[error("The graph is missing the requested vertex [{0:?}]")] diff --git a/mapf/src/util.rs b/mapf/src/util.rs index a640774..617352e 100644 --- a/mapf/src/util.rs +++ b/mapf/src/util.rs @@ -81,52 +81,6 @@ impl std::cmp::Ordering> Minimum { } } -pub enum FlatResultMapIter { - Ok(std::iter::FlatMap, U, F>), - Err(Option), -} - -impl Iterator for FlatResultMapIter -where - F: FnMut(T) -> U, -{ - type Item = Result; - - #[inline] - fn next(&mut self) -> Option { - match self { - Self::Ok(inner) => Ok(inner.next()).transpose(), - Self::Err(inner) => inner.take().map(|e| Err(e)), - } - } -} - -pub trait FlatResultMapTrait { - type Type; - type Error; - fn flat_result_map(self, f: F) -> FlatResultMapIter - where - Self: Sized, - U: IntoIterator, - F: FnMut(Self::Type) -> U; -} - -impl FlatResultMapTrait for Result { - type Type = T; - type Error = E; - fn flat_result_map(self, f: F) -> FlatResultMapIter - where - Self: Sized, - U: IntoIterator, - F: FnMut(T) -> U, - { - match self { - Ok(iter) => FlatResultMapIter::Ok(Some(iter).into_iter().flat_map(f)), - Err(err) => FlatResultMapIter::Err(Some(err)), - } - } -} - pub enum ForkIter { Left(L), Right(R), @@ -153,3 +107,24 @@ pub fn wrap_to_pi(mut value: f64) -> f64 { value } + +pub struct IterError { + error: Option, + _ignore: std::marker::PhantomData, +} + +impl IterError { + pub fn new(error: E) -> Self { + Self { + error: Some(error), + _ignore: Default::default(), + } + } +} + +impl Iterator for IterError { + type Item = Result; + fn next(&mut self) -> Option { + self.error.take().map(Err) + } +} From e5c8c4193a7cc4c32045716819e91f52ee2bdaf9 Mon Sep 17 00:00:00 2001 From: kalifun <37646342+kalifun@users.noreply.github.com> Date: Tue, 15 Jul 2025 17:50:49 +0800 Subject: [PATCH 05/12] fix: Spatial Canvas scaling boundary constraints (#33) Signed-off-by: Michael X. Grey Co-authored-by: Michael X. Grey Signed-off-by: Arjo Chakravarty --- mapf-viz/src/spatial_canvas.rs | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/mapf-viz/src/spatial_canvas.rs b/mapf-viz/src/spatial_canvas.rs index 00daf5d..d38b1ca 100644 --- a/mapf-viz/src/spatial_canvas.rs +++ b/mapf-viz/src/spatial_canvas.rs @@ -146,6 +146,10 @@ pub struct SpatialCanvas> { } impl> SpatialCanvas { + // Add scaling boundary constants + const MIN_ZOOM: f32 = 0.001; // Minimum scaling ratio + const MAX_ZOOM: f32 = 1000.0; // Maximum scaling ratio + pub fn new(program: Program) -> Self { Self { program, @@ -222,6 +226,11 @@ impl> SpatialCanvas> canvas::Program @@ -293,7 +302,8 @@ impl<'a, Message, Program: SpatialCanvasProgram> canvas::Program { let p0 = p; - self.zoom = self.zoom * (1.0 + y / 10.0); + let new_zoom = self.zoom * (1.0 + y / 10.0); + self.set_zoom_safe(new_zoom); let p_raw = Point::new(p_raw.x, p_raw.y); self.offset = p_raw - Transform::scale(self.zoom, -self.zoom).transform_point(p0); From 04e67d5ef1bfea09035faf183613ac2bcb005777 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 7 Apr 2026 05:19:45 +0000 Subject: [PATCH 06/12] feat: add benchmarking infrastructure for Moving AI datasets The project now includes: - mapf-bench crate: A tool for running multi-agent negotiation benchmarks on Moving AI grid maps. - scripts/benchmark.py: An automated script that downloads maps/scenarios, runs benchmarks for 2-50 agents, and generates reports with strict timeouts. - .gitignore update: The cache/ directory used for benchmarking data is now ignored. Generated-by: Gemini-CLI Signed-off-by: Arjo Chakravarty --- .gitignore | 2 + Cargo.toml | 1 + mapf-bench/Cargo.toml | 13 ++++ mapf-bench/src/main.rs | 65 ++++++++++++++++ mapf-bench/src/movingai.rs | 149 +++++++++++++++++++++++++++++++++++++ scripts/benchmark.py | 147 ++++++++++++++++++++++++++++++++++++ 6 files changed, 377 insertions(+) create mode 100644 mapf-bench/Cargo.toml create mode 100644 mapf-bench/src/main.rs create mode 100644 mapf-bench/src/movingai.rs create mode 100755 scripts/benchmark.py diff --git a/.gitignore b/.gitignore index 96ef6c0..26324e7 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,4 @@ /target Cargo.lock +cache/ +benchmark_report.json diff --git a/Cargo.toml b/Cargo.toml index 8dd3043..aff6efd 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -2,6 +2,7 @@ members = [ "mapf", "mapf-viz", + "mapf-bench", ] resolver = "2" diff --git a/mapf-bench/Cargo.toml b/mapf-bench/Cargo.toml new file mode 100644 index 0000000..94b5e2d --- /dev/null +++ b/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" diff --git a/mapf-bench/src/main.rs b/mapf-bench/src/main.rs new file mode 100644 index 0000000..aa239c9 --- /dev/null +++ b/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(()) +} diff --git a/mapf-bench/src/movingai.rs b/mapf-bench/src/movingai.rs new file mode 100644 index 0000000..51b57fb --- /dev/null +++ b/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>, +} + +impl Map { + pub fn from_file>(path: P) -> Result { + 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> { + 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, +} + +impl MovingAIScenario { + pub fn from_file>(path: P) -> Result { + 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, + } + } +} diff --git a/scripts/benchmark.py b/scripts/benchmark.py new file mode 100755 index 0000000..f35d694 --- /dev/null +++ b/scripts/benchmark.py @@ -0,0 +1,147 @@ +#!/usr/bin/env python3 + +import os +import subprocess +import zipfile +import urllib.request +import json +import time +import argparse +from pathlib import Path + +# Configuration +CACHE_DIR = Path("cache") +MAPS_ZIP_URL = "https://www.movingai.com/benchmarks/mapf/mapf-map.zip" +SCEN_ZIP_URL = "https://www.movingai.com/benchmarks/mapf/mapf-scen-random.zip" +DEFAULT_AGENT_COUNTS = [2, 5, 10, 15, 20, 25, 30, 35, 40, 45, 50] + +def download_file(url, dest): + if dest.exists(): + return + print(f"Downloading {url} to {dest}...") + urllib.request.urlretrieve(url, dest) + +def unzip_file(zip_path, extract_to): + print(f"Unzipping {zip_path} to {extract_to}...") + with zipfile.ZipFile(zip_path, 'r') as zip_ref: + zip_ref.extractall(extract_to) + +def run_benchmark(map_path, scen_path, num_agents, timeout): + cmd = [ + "cargo", "run", "-p", "mapf-bench", "--release", "--", + "--map", str(map_path), + "--scen", str(scen_path), + "--num-agents", str(num_agents), + "--timeout", str(timeout) + ] + + start_time = time.time() + try: + # Strict timeout enforcement via subprocess + result = subprocess.run(cmd, capture_output=True, text=True, timeout=timeout + 5) + duration = time.time() - start_time + + success = "Negotiation successful" in result.stdout + return { + "success": success, + "duration": duration, + "stdout": result.stdout, + "stderr": result.stderr + } + except subprocess.TimeoutExpired: + return { + "success": False, + "duration": timeout, + "error": "Timeout" + } + except Exception as e: + return { + "success": False, + "error": str(e) + } + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument("--timeout", type=int, default=30, help="Timeout in seconds per run") + parser.add_argument("--max-scenarios", type=int, default=1, help="Max random scenarios per map") + parser.add_argument("--maps", nargs="+", default=["empty-32-32.map", "room-32-32-4.map", "maze-32-32-2.map"]) + args = parser.parse_args() + + CACHE_DIR.mkdir(exist_ok=True) + + maps_zip = CACHE_DIR / "mapf-map.zip" + scen_zip = CACHE_DIR / "mapf-scen-random.zip" + + download_file(MAPS_ZIP_URL, maps_zip) + download_file(SCEN_ZIP_URL, scen_zip) + + maps_dir = CACHE_DIR / "maps" + scen_dir = CACHE_DIR / "scenarios" + + if not maps_dir.exists(): + unzip_file(maps_zip, maps_dir) + if not scen_dir.exists(): + unzip_file(scen_zip, scen_dir) + + report = {} + + print("Building mapf-bench in release mode...") + subprocess.run(["cargo", "build", "-p", "mapf-bench", "--release"], check=True) + + for map_name in args.maps: + map_path = maps_dir / map_name + if not map_path.exists(): + print(f"Map {map_name} not found.") + continue + + base_name = map_name.replace(".map", "") + + # Find all matching scenario files + scen_pattern = f"{base_name}-random-*.scen" + scen_files = list((scen_dir / "scen-random").glob(scen_pattern)) + scen_files.sort() + + if not scen_files: + print(f"No scenarios found for {map_name} with pattern {scen_pattern}") + continue + + selected_scens = scen_files[:args.max_scenarios] + + for scen_path in selected_scens: + scen_name = scen_path.name + print(f"\nBenchmarking {map_name} with scenario {scen_name}...") + + key = f"{map_name}:{scen_name}" + report[key] = [] + + for count in DEFAULT_AGENT_COUNTS: + print(f" Agents: {count}", end=" ", flush=True) + res = run_benchmark(map_path, scen_path, count, args.timeout) + if res["success"]: + print(f"✅ ({res['duration']:.2f}s)") + else: + error_msg = res.get("error", "FAILED") + print(f"❌ ({error_msg})") + + report[key].append({ + "agents": count, + "success": res["success"], + "duration": res.get("duration", 0), + "error": res.get("error") + }) + + # Save report + with open("benchmark_report.json", "w") as f: + json.dump(report, f, indent=2) + + # Print summary table + print("\nBenchmark Summary:") + print(f"{'Scenario':<40} | {'Agents':<6} | {'Status':<8} | {'Time':<8}") + print("-" * 75) + for key, results in report.items(): + for res in results: + status = "SUCCESS" if res["success"] else (res.get("error") if res.get("error") else "FAILED") + print(f"{key[:40]:<40} | {res['agents']:<6} | {status:<8} | {res['duration']:>7.2f}s") + +if __name__ == "__main__": + main() From 32962bc632127a92326c517905d38446bf03bebb Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 18 Jun 2026 02:28:21 +0000 Subject: [PATCH 07/12] docs: Add usage docs Signed-off-by: Arjo Chakravarty --- scripts/benchmark.py | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/scripts/benchmark.py b/scripts/benchmark.py index f35d694..0ebec76 100755 --- a/scripts/benchmark.py +++ b/scripts/benchmark.py @@ -1,5 +1,32 @@ #!/usr/bin/env python3 +# Copyright 2024 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# This tool downloads the moving ai benchmarks for mapf and runs the current +# solver against the benchmarks to measure MAPF performance. +# Usage is like so: +# +# From the project root run: +# +# python3 scripts/benchmark.py --timeout [number of seconds till timeout] +# \ --max-scenarios [number of random scenarios] +# \ --maps [maps to be run on] +# +# The benchmark mapfiles and names can be found here: +# https://www.movingai.com/benchmarks/mapf/ + import os import subprocess import zipfile From 7b3755b738451f2382cedfbe36e28c45e5b6f338 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 18 Jun 2026 03:00:58 +0000 Subject: [PATCH 08/12] docs: Add movingai related documentation Signed-off-by: Arjo Chakravarty --- mapf-bench/README.md | 5 +++++ mapf-bench/src/movingai.rs | 34 +++++++++++++++++++++++++++++++--- 2 files changed, 36 insertions(+), 3 deletions(-) create mode 100644 mapf-bench/README.md diff --git a/mapf-bench/README.md b/mapf-bench/README.md new file mode 100644 index 0000000..80bb058 --- /dev/null +++ b/mapf-bench/README.md @@ -0,0 +1,5 @@ +# `mapf-bench` + +The sole purpose of this crate is to run benchmarks using the `mapf` library. Currently the benchmarks we +run are run against [moving AI](https://www.movingai.com/benchmarks/mapf/)'s benchmarks. This benchmark is often +seen as the defacto "mapf" banchmark. This crate contains a parser to load the benchmark files into memory. diff --git a/mapf-bench/src/movingai.rs b/mapf-bench/src/movingai.rs index 51b57fb..e1dc9ce 100644 --- a/mapf-bench/src/movingai.rs +++ b/mapf-bench/src/movingai.rs @@ -1,8 +1,32 @@ +/* + * Copyright (C) 2026 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +//! This module contains a simple parser for the movingai benchmarks. +//! This benchmark is widely seen as the de-facto mapf benchmark. +//! +//! The benchmarks consist of *.map files and *.scen files. The *.map +//! files are occupancy grids where the *.scen files contain +//! exact mapf scenarios. + +use anyhow::Result; +use std::collections::HashMap; 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; @@ -44,7 +68,11 @@ impl Map { } } - Ok(Map { width, height, grid }) + Ok(Map { + width, + height, + grid, + }) } pub fn to_occupancy_map(&self) -> HashMap> { From 59edc4cfebbdb8d7b4f1fb9dbbec2d94ffc7030c Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 18 Jun 2026 04:22:08 +0000 Subject: [PATCH 09/12] address feedback Signed-off-by: Arjo Chakravarty --- mapf-bench/src/movingai.rs | 41 +++++++++++++++++++------------------- 1 file changed, 20 insertions(+), 21 deletions(-) diff --git a/mapf-bench/src/movingai.rs b/mapf-bench/src/movingai.rs index e1dc9ce..bf4c327 100644 --- a/mapf-bench/src/movingai.rs +++ b/mapf-bench/src/movingai.rs @@ -19,7 +19,7 @@ //! This benchmark is widely seen as the de-facto mapf benchmark. //! //! The benchmarks consist of *.map files and *.scen files. The *.map -//! files are occupancy grids where the *.scen files contain +//! files are occupancy grids while the *.scen files contain //! exact mapf scenarios. use anyhow::Result; @@ -94,15 +94,13 @@ impl Map { } 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 _bucket: usize, + pub _map_file: String, + pub _map_width: usize, + pub _map_height: usize, + pub start: [i64; 2], + pub goal: [i64; 2], + pub _optimal_length: f64, } pub struct MovingAIScenario { @@ -126,16 +124,17 @@ impl MovingAIScenario { continue; } + let start: [i64; 2] = [parts[4].parse()?, parts[5].parse()?]; + let goal: [i64; 2] = [parts[6].parse()?, parts[7].parse()?]; + 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()?, + _bucket: parts[0].parse()?, + _map_file: parts[1].to_string(), + _map_width: parts[2].parse()?, + _map_height: parts[3].parse()?, + start, + goal, + _optimal_length: parts[8].parse()?, }); } @@ -156,9 +155,9 @@ impl MovingAIScenario { agents.insert( format!("agent_{}", i), Agent { - start: [entry.start_x as i64, entry.start_y as i64], + start: entry.start, yaw: 0.0, - goal: [entry.goal_x as i64, entry.goal_y as i64], + goal: entry.goal, radius, speed, spin, From c0e86fb99834a2ba362d3e49e2a1eb30a69ba7ca Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 18 Jun 2026 04:33:48 +0000 Subject: [PATCH 10/12] Update benchmarks Signed-off-by: Arjo Chakravarty --- mapf-bench/README.md | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/mapf-bench/README.md b/mapf-bench/README.md index 80bb058..f4e19dc 100644 --- a/mapf-bench/README.md +++ b/mapf-bench/README.md @@ -2,4 +2,8 @@ The sole purpose of this crate is to run benchmarks using the `mapf` library. Currently the benchmarks we run are run against [moving AI](https://www.movingai.com/benchmarks/mapf/)'s benchmarks. This benchmark is often -seen as the defacto "mapf" banchmark. This crate contains a parser to load the benchmark files into memory. +seen as the defacto "mapf" banchmark. This crate contains abinary to load the benchmark files into memory and run +a single benchmark. + +It is advised to use the python benchmark script located in the `scripts/` folder tot automate the downloading and +running of the benchmakrs. From 89e5a9c809a2ccbaa0c75eb6e71c94f71312090d Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 26 Jun 2026 05:58:08 +0000 Subject: [PATCH 11/12] Adds an argparse description Signed-off-by: Arjo Chakravarty --- scripts/benchmark.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/scripts/benchmark.py b/scripts/benchmark.py index 0ebec76..480a625 100755 --- a/scripts/benchmark.py +++ b/scripts/benchmark.py @@ -88,7 +88,9 @@ def run_benchmark(map_path, scen_path, num_agents, timeout): } def main(): - parser = argparse.ArgumentParser() + parser = argparse.ArgumentParser( + description="Download Moving AI benchmarks for MAPF and run the solver against them to measure performance." + ) parser.add_argument("--timeout", type=int, default=30, help="Timeout in seconds per run") parser.add_argument("--max-scenarios", type=int, default=1, help="Max random scenarios per map") parser.add_argument("--maps", nargs="+", default=["empty-32-32.map", "room-32-32-4.map", "maze-32-32-2.map"]) From 4b0d50478929237a8fe287d2ac00f0cfd7084b11 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 26 Jun 2026 06:25:46 +0000 Subject: [PATCH 12/12] More docs Signed-off-by: Arjo Chakravarty --- mapf-bench/README.md | 53 +++++++++++++++++++++++++++++++++----- mapf-bench/src/main.rs | 9 ++++++- mapf-bench/src/movingai.rs | 5 ++++ scripts/benchmark.py | 12 ++++++++- 4 files changed, 71 insertions(+), 8 deletions(-) diff --git a/mapf-bench/README.md b/mapf-bench/README.md index f4e19dc..340310d 100644 --- a/mapf-bench/README.md +++ b/mapf-bench/README.md @@ -1,9 +1,50 @@ # `mapf-bench` -The sole purpose of this crate is to run benchmarks using the `mapf` library. Currently the benchmarks we -run are run against [moving AI](https://www.movingai.com/benchmarks/mapf/)'s benchmarks. This benchmark is often -seen as the defacto "mapf" banchmark. This crate contains abinary to load the benchmark files into memory and run -a single benchmark. +`mapf-bench` is a binary crate designed to run single Multi-Agent Path Finding (MAPF) benchmark scenarios using the `mapf` library's negotiation solver. -It is advised to use the python benchmark script located in the `scripts/` folder tot automate the downloading and -running of the benchmakrs. +Currently, it supports benchmarks from the [Moving AI MAPF benchmark collection](https://www.movingai.com/benchmarks/mapf.html). + +## Relationship with `scripts/benchmark.py` + +While `mapf-bench` can be run directly to test a single scenario, it is intended to be used in conjunction with the Python orchestrator script located at `scripts/benchmark.py`. + +* **`scripts/benchmark.py`**: Automates downloading the Moving AI benchmark files (maps and scenarios), unzipping them, building `mapf-bench` in release mode, and running it across multiple configurations (different maps, scenarios, and agent counts) to generate a comprehensive performance report. +* **`mapf-bench`**: The low-level runner that loads a specific map and scenario file, sets up the negotiation scenario, and runs the solver for a single run. + +## Intended Workflow + +To run a full benchmark suite: + +1. Use the Python script from the root directory: + ```bash + python3 scripts/benchmark.py --timeout 30 --max-scenarios 1 --maps empty-32-32.map + ``` + This script will handle building this crate and running it. + +To run a single scenario manually for debugging: + +1. Build the crate: + ```bash + cargo build -p mapf-bench --release + ``` +2. Run the binary, providing paths to the map and scenario files: + ```bash + cargo run -p mapf-bench --release -- --map cache/maps/empty-32-32.map --scen cache/scenarios/scen-random/empty-32-32-random-1.scen --num-agents 10 + ``` + +## Command Line Arguments + +`mapf-bench` accepts the following arguments: + +* `-m`, `--map `: Path to the Moving AI map file (`.map`). +* `-s`, `--scen `: Path to the Moving AI scenario file (`.scen`). +* `-r`, `--radius `: Radius of the agents (default: 0.45). +* `--speed `: Speed of the agents (default: 1.0). (Note: no short option to avoid conflict with `--scen`). +* `-n`, `--num-agents `: Number of agents to include in the scenario (default: 10). +* `-t`, `--timeout `: Timeout in seconds (default: 30). Note: This timeout is currently enforced by the parent process (`benchmark.py`) rather than the binary itself. + +## Map Format + +The benchmark loader parses Moving AI `.map` files. The characters in the grid are interpreted as follows: +* `.` : Passable terrain. +* Any other character : Obstacle (impassable). diff --git a/mapf-bench/src/main.rs b/mapf-bench/src/main.rs index aa239c9..e842e85 100644 --- a/mapf-bench/src/main.rs +++ b/mapf-bench/src/main.rs @@ -5,24 +5,31 @@ use clap::Parser; use std::path::PathBuf; use std::time::Instant; +/// Run a single MAPF benchmark scenario using the negotiation solver. #[derive(Parser, Debug)] #[command(author, version, about, long_about = None)] struct Args { + /// Path to the Moving AI map file (.map) #[arg(short, long)] map: PathBuf, + /// Path to the Moving AI scenario file (.scen) #[arg(short, long)] scen: PathBuf, + /// Radius of the agents #[arg(short, long, default_value_t = 0.45)] radius: f64, - #[arg(short, long, default_value_t = 1.0)] + /// Speed of the agents + #[arg(long, default_value_t = 1.0)] speed: f64, + /// Number of agents to include in the benchmark #[arg(short, long, default_value_t = 10)] num_agents: usize, + /// Timeout in seconds (enforced by parent process) #[arg(short, long, default_value_t = 30)] timeout: u64, } diff --git a/mapf-bench/src/movingai.rs b/mapf-bench/src/movingai.rs index bf4c327..4bd0760 100644 --- a/mapf-bench/src/movingai.rs +++ b/mapf-bench/src/movingai.rs @@ -34,6 +34,11 @@ use std::collections::BTreeMap; pub struct Map { pub width: usize, pub height: usize, + /// The grid representation of the map. + /// + /// The characters in the grid represent different terrain types based on the Moving AI format: + /// - `.` : Passable terrain + /// - `@` : Obstacle pub grid: Vec>, } diff --git a/scripts/benchmark.py b/scripts/benchmark.py index 480a625..baa0252 100755 --- a/scripts/benchmark.py +++ b/scripts/benchmark.py @@ -89,7 +89,17 @@ def run_benchmark(map_path, scen_path, num_agents, timeout): def main(): parser = argparse.ArgumentParser( - description="Download Moving AI benchmarks for MAPF and run the solver against them to measure performance." + description=( + "Download Moving AI benchmarks for MAPF and run the solver against them to measure performance.\n\n" + "This script automates the benchmarking workflow:\n" + "1. Downloads map and scenario zip files from Moving AI website if not present in cache.\n" + "2. Unzips them into the cache directory.\n" + "3. Builds the `mapf-bench` Rust crate in release mode.\n" + "4. Runs the benchmark for specified maps and scenarios with a range of agent counts.\n" + "5. Generates a JSON report (`benchmark_report.json`) and prints a summary table.\n\n" + "For more details on the underlying benchmark runner, see `mapf-bench/README.md`." + ), + formatter_class=argparse.RawDescriptionHelpFormatter ) parser.add_argument("--timeout", type=int, default=30, help="Timeout in seconds per run") parser.add_argument("--max-scenarios", type=int, default=1, help="Max random scenarios per map")