From 081cac761695e08ded4468e7dfd3de1a6accd8cb Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 6 Mar 2026 09:04:26 +0000 Subject: [PATCH 01/11] Merge mapf_post into mapf and align types Generated-by: Gemini-CLI Signed-off-by: Arjo Chakravarty --- mapf/Cargo.toml | 9 +- mapf/src/lib.rs | 2 + mapf/src/post/mod.rs | 1508 +++++++++++++++++++++++ mapf/src/post/spatial_allocation/mod.rs | 352 ++++++ 4 files changed, 1869 insertions(+), 2 deletions(-) create mode 100644 mapf/src/post/mod.rs create mode 100644 mapf/src/post/spatial_allocation/mod.rs diff --git a/mapf/Cargo.toml b/mapf/Cargo.toml index 6920707..9d188da 100644 --- a/mapf/Cargo.toml +++ b/mapf/Cargo.toml @@ -1,7 +1,7 @@ [package] name = "mapf" version = "0.3.0" -edition = "2021" +edition = "2024" description = "Base traits and utilities for multi-agent planning" license = "Apache-2.0" repository = "https://github.com/open-rmf/mapf" @@ -11,7 +11,7 @@ keywords = ["mapf", "multi", "agent", "planning", "search"] maintenance = {status = "experimental"} [dependencies] -nalgebra = "0.31.1" +nalgebra = "0.32" time-point = "0.1" sorted-vec = "0.8.2" cached = "0.40" @@ -27,6 +27,11 @@ smallvec = "1.10" serde = { version="1.0", features = ["derive"] } serde_yaml = "0.9" slotmap = "1.0" +parry2d = { version = "0.21", package = "parry2d-f64" } +petgraph = "0.6" +bresenham = "0.1.1" +rand = "0.8" +csv = "1.1" [dev-dependencies] approx = "0.5" diff --git a/mapf/src/lib.rs b/mapf/src/lib.rs index 3309e9c..b545942 100644 --- a/mapf/src/lib.rs +++ b/mapf/src/lib.rs @@ -37,6 +37,8 @@ pub mod error; pub mod premade; +pub mod post; + mod util; pub mod prelude { diff --git a/mapf/src/post/mod.rs b/mapf/src/post/mod.rs new file mode 100644 index 0000000..a5e361a --- /dev/null +++ b/mapf/src/post/mod.rs @@ -0,0 +1,1508 @@ +use core::panic; +use std::collections::{HashMap, HashSet}; +use std::sync::Arc; + +use parry2d::na::{Isometry2, Point2}; +use parry2d::query::cast_shapes_nonlinear; +use parry2d::query::{NonlinearRigidMotion, ShapeCastStatus}; +use parry2d::shape::Shape; + +pub use parry2d::na; +pub use parry2d::shape; +use petgraph::algo::toposort; +use petgraph::data::Build; +use petgraph::graph::DiGraph; +use petgraph::visit::EdgeRef; + +pub mod spatial_allocation; + +/// A simple class to estimate a Robot's Semantic position based on its location +/// on a trajectory. It assumes you start near the start of the robot trajectory. +pub struct WaypointFollower { + trajectory: Trajectory, + current_pose_on_trajectory: usize, + agent_id: usize, +} + +impl WaypointFollower { + /// Construct the follower for `agent_id` based on a trajectory + pub fn from_trajectory(agent_id: usize, trajectory: Trajectory) -> Self { + Self { + trajectory, + current_pose_on_trajectory: 0, + agent_id, + } + } + + /// Update position of waypoint follower by finding the closest point on the trajectory. + /// + /// This avoids getting stuck if a waypoint is missed, by projecting the robot's position + /// onto the trajectory segments. + pub fn update_position_estimate(&mut self, pose: &Isometry2, _uncertainty: f64) { + if self.current_pose_on_trajectory >= self.trajectory.poses.len().saturating_sub(1) { + return; + } + + let robot_pos = Point2::from(pose.translation.vector); + + let mut closest_dist_sq = f64::MAX; + let mut best_segment_start_idx = self.current_pose_on_trajectory; + + // Iterate from the current segment onwards to find the closest segment. + // This prevents the follower from going backwards along the trajectory. + for i in self.current_pose_on_trajectory..self.trajectory.poses.len().saturating_sub(1) { + let p1 = Point2::from(self.trajectory.poses[i].translation.vector); + let p2 = Point2::from(self.trajectory.poses[i + 1].translation.vector); + let segment_vec = p2 - p1; + let robot_to_p1 = robot_pos - p1; + + let t = if segment_vec.norm_squared() > 1e-6 { + (robot_to_p1.dot(&segment_vec)) / segment_vec.norm_squared() + } else { + 0.0 // Handle zero-length segments + }; + + let closest_point_on_segment = if t < 0.0 { + p1 + } else if t > 1.0 { + p2 + } else { + p1 + t * segment_vec + }; + + let dist_sq = na::distance_squared(&robot_pos, &closest_point_on_segment); + + if dist_sq < closest_dist_sq { + closest_dist_sq = dist_sq; + + // If the robot is at or past the end of the segment, it has passed this segment's start waypoint. + if t >= 1.0 { + best_segment_start_idx = i + 1; + } else { + best_segment_start_idx = i; + } + } + } + self.current_pose_on_trajectory = best_segment_start_idx; + } + + /// Gives the next waypoint we should consider. + pub fn next_waypoint(&mut self) -> Isometry2 { + if self.current_pose_on_trajectory + 1 >= self.trajectory.poses.len() { + return *self.trajectory.poses.last().unwrap(); + } + self.trajectory.poses[self.current_pose_on_trajectory + 1] + } + + /// Returns a list of (x,y) coordinates left for the robot to go through + pub fn remaining_trajectory(&mut self) -> Vec<(f64, f64)> { + let mut v = vec![]; + for p in self.current_pose_on_trajectory + 1..self.trajectory.poses.len() { + v.push(( + self.trajectory.poses[p].translation.x, + self.trajectory.poses[p].translation.y, + )); + } + v + } + + /// Returns the list of (x,y) coordinates that are currently safe for the robot to traverse + pub fn remaining_safe_trajectory_segment( + &mut self, + seg: &CurrentlyAllocatedTrajSegment, + ) -> Vec<(f64, f64)> { + let mut v = vec![]; + for p in self.current_pose_on_trajectory + 1..seg.end_id { + v.push(( + self.trajectory.poses[p].translation.x, + self.trajectory.poses[p].translation.y, + )); + } + v + } + + /// Returns the current semantic waypoint + pub fn get_semantic_waypoint(&mut self) -> SemanticWaypoint { + SemanticWaypoint { + agent: self.agent_id, + trajectory_index: self.current_pose_on_trajectory, + } + } +} + +#[cfg(test)] +#[test] +fn test_waypoint_follower() { + use parry2d::na::Vector2; + + let example_trajectory = Trajectory { + poses: vec![ + Isometry2::new(Vector2::new(0.0, 0.0), 0.0), + Isometry2::new(Vector2::new(1.0, 0.0), 0.0), + Isometry2::new(Vector2::new(2.0, 0.0), 0.0), + ], + }; + + let mut follower = WaypointFollower::from_trajectory(0, example_trajectory.clone()); + let next_wp = follower.next_waypoint(); + assert!((next_wp.translation.x - example_trajectory.poses[1].translation.x).abs() < 0.01); + let semantic_pose = follower.get_semantic_waypoint(); + assert_eq!(semantic_pose.trajectory_index, 0); + // Now lets move closer to the target but not reach it. + follower.update_position_estimate(&Isometry2::new(Vector2::new(0.1, 0.0), 0.0), 0.1); + + // We should still be at the previous location + let next_wp = follower.next_waypoint(); + assert!((next_wp.translation.x - example_trajectory.poses[1].translation.x).abs() < 0.01); + let semantic_pose = follower.get_semantic_waypoint(); + assert_eq!(semantic_pose.trajectory_index, 0); + + // Lets update our position to be close to the next waypoint, but not past it. + follower.update_position_estimate(&Isometry2::new(Vector2::new(0.95, 0.0), 0.0), 0.1); + let next_wp = follower.next_waypoint(); + assert!((next_wp.translation.x - example_trajectory.poses[1].translation.x).abs() < 0.01); + let semantic_pose = follower.get_semantic_waypoint(); + assert_eq!(semantic_pose.trajectory_index, 0); + + // Let's reach the next waypoint. + follower.update_position_estimate(&Isometry2::new(Vector2::new(1.0, 0.0), 0.0), 0.1); + let next_wp = follower.next_waypoint(); + assert!((next_wp.translation.x - example_trajectory.poses[2].translation.x).abs() < 0.01); + let semantic_pose = follower.get_semantic_waypoint(); + assert_eq!(semantic_pose.trajectory_index, 1); + + // Lets update our position to be close to the final pose. + follower.update_position_estimate(&Isometry2::new(Vector2::new(1.95, 0.0), 0.0), 0.1); + let next_wp = follower.next_waypoint(); + assert!((next_wp.translation.x - example_trajectory.poses[2].translation.x).abs() < 0.01); + let semantic_pose = follower.get_semantic_waypoint(); + assert_eq!(semantic_pose.trajectory_index, 1); + + // Let's reach the final pose + follower.update_position_estimate(&Isometry2::new(na::Vector2::new(2.0, 0.0), 0.0), 0.1); + let next_wp = follower.next_waypoint(); + assert!((next_wp.translation.x - example_trajectory.poses[2].translation.x).abs() < 0.01); + let semantic_pose = follower.get_semantic_waypoint(); + assert_eq!(semantic_pose.trajectory_index, 2); + } + + #[cfg(test)] + #[test] + fn test_trajectory_conversion() { + use crate::motion::se2::WaypointSE2; + use crate::motion::Trajectory as MapfTrajectory; + use time_point::TimePoint; + + let w1 = WaypointSE2::new(TimePoint::from_secs_f64(0.0), 0.0, 0.0, 0.0); + let w2 = WaypointSE2::new(TimePoint::from_secs_f64(1.0), 1.0, 0.0, 0.0); + let mapf_traj = MapfTrajectory::new(w1, w2).unwrap(); + + let post_traj = Trajectory::from(&mapf_traj); + assert_eq!(post_traj.len(), 2); + assert!((post_traj.poses[0].translation.x - 0.0).abs() < 1e-6); + assert!((post_traj.poses[1].translation.x - 1.0).abs() < 1e-6); + } + + /// Semantic waypoint: A point on the trajectory that needs to be passed. + +#[derive(Clone, Copy, Debug, Hash, PartialEq, Eq)] +pub struct SemanticWaypoint { + pub agent: usize, + pub trajectory_index: usize, +} + +/// These are error codes for safe next state retrieval +#[derive(Clone, Copy, Debug, Hash, PartialEq, Eq)] +#[non_exhaustive] +pub enum SafeNextStatesError { + /// An incorreect number of agents were added + NumberOfAgentsNotMatching, + /// The semantic state is incorrect. Probably the same agent is reporting state twice. + InvalidSemanticState, + /// Agent was not found. Probably a different agent was reporting stete at different times. + AgentNotFound, + /// Report a bug if this ever ever crops up + InternalStateMisMatch, +} + +/// Semantic Plan represents the plan in terms of SemanticWaypoints +#[derive(Clone, Debug, Default)] +pub struct SemanticPlan { + /// Number of agents + pub num_agents: usize, + /// These serve as graph nodes, describing each waypoint + pub waypoints: Vec, + /// For book keeping + agent_time_to_wp_id: HashMap, + /// Key comes after all of Values + depends_on_all_of: HashMap>, + /// Next states. Key is the state + potential_successors: HashMap>, +} + +impl SemanticPlan { + fn add_waypoint(&mut self, waypoint: &SemanticWaypoint) { + self.agent_time_to_wp_id + .insert(*waypoint, self.waypoints.len()); + self.waypoints.push(*waypoint); + } + + fn last_time(&self) -> usize { + self.waypoints + .iter() + .map(|wp| wp.trajectory_index) + .max() + .unwrap_or(0) + } + + /// Use this to add an edge + fn requires_comes_after(&mut self, after: &SemanticWaypoint, before: &SemanticWaypoint) { + let Some(after_id) = self.agent_time_to_wp_id.get(before) else { + return; + }; + let Some(before_id) = self.agent_time_to_wp_id.get(after) else { + return; + }; + + let Some(to_vals) = self.depends_on_all_of.get_mut(after_id) else { + self.depends_on_all_of.insert(*after_id, vec![*before_id]); + return; + }; + to_vals.push(*before_id); + + let Some(to_vals) = self.potential_successors.get_mut(before_id) else { + self.potential_successors + .insert(*before_id, vec![*after_id]); + return; + }; + to_vals.push(*after_id); + } + + /// Given a Semantic State Estimate, determine if its safe for the current agent to proceed + /// The state estimate should consist of each agents semantic waypoint + /// If there is a mismatch between the number of agents and number of + pub fn is_safe_to_proceed( + &self, + current_state: &[SemanticWaypoint], + agent: usize, + ) -> Result { + // Lots of O(n^2) behaviour that should really be O(n) or O(1) because our function signature takes current semantic state as a vec + // TODO(arjo): add a new CurrentSemanticState struct. Require that it is a sorted vec or structure with correct + if current_state.len() != self.num_agents { + return Err(SafeNextStatesError::NumberOfAgentsNotMatching); + } + + // Get the agents current waypoint + let agent_curr_waypoint: Vec<_> = + current_state.iter().filter(|x| x.agent == agent).collect(); + if agent_curr_waypoint.len() > 1 { + return Err(SafeNextStatesError::InvalidSemanticState); + } + if agent_curr_waypoint.is_empty() { + return Err(SafeNextStatesError::AgentNotFound); + } + + // Try to get the next waypoint for the agent + let Some(agent_next_waypoint) = self.get_next_for_agent(agent_curr_waypoint[0]) else { + return Ok(false); + }; + + // Find out what dependencies the waypoint should come after + let Some(waypoints_that_should_have_been_crossed) = self.comes_before(&agent_next_waypoint) + else { + return Err(SafeNextStatesError::InternalStateMisMatch); + }; + + let waypoints_that_should_have_been_crossed = waypoints_that_should_have_been_crossed + .iter() + .map(|wp_id| self.waypoints[*wp_id]); + + // Now for each waypoint find out if the other robot has crossed it + let res = waypoints_that_should_have_been_crossed + .filter(|wp| { + let agent_to_check = wp.agent; + let agent_curr_loc: Vec<_> = current_state + .iter() + .filter(|x| x.agent == agent_to_check) + .collect(); + if agent_to_check == agent { + return false; + } + if agent_curr_loc.len() > 1 { + panic!("malformed") + } + if agent_curr_loc.is_empty() { + panic!("Made-up agent") + } + if agent_curr_loc[0].trajectory_index <= wp.trajectory_index { + return true; + } + false + }) + .count(); + + Ok(res == 0) + } + + fn get_next_for_agent(&self, waypoint: &SemanticWaypoint) -> Option { + let mut waypoint = *waypoint; + waypoint.trajectory_index += 1; + self.agent_time_to_wp_id + .get(&waypoint) + .map(|p| self.waypoints[*p]) + } + + /// Check which waypoints should come before the given waypoint + pub fn comes_before(&self, waypoint: &SemanticWaypoint) -> Option<&Vec> { + let Some(waypoint_id) = self.agent_time_to_wp_id.get(waypoint) else { + return None; + }; + + self.depends_on_all_of.get(waypoint_id) + } + + /// Returns a Pet-graph directed graph for further manipulation of the entire + /// semantic plan + pub fn to_petgraph(&self) -> DiGraph { + let mut digraph = DiGraph::new(); + let mut node_index_map = HashMap::new(); + for waypoint in &self.waypoints { + let node_index = digraph.add_node(*waypoint); + node_index_map.insert(waypoint, node_index); + } + for (after, befores) in &self.depends_on_all_of { + for before in befores { + let Some(after) = self.waypoints.get(*after) else { + continue; + }; + let Some(before) = self.waypoints.get(*before) else { + continue; + }; + let Some(&after) = node_index_map.get(after) else { + continue; + }; + let Some(&before) = node_index_map.get(before) else { + continue; + }; + digraph.add_edge(before, after, ()); + } + } + digraph + } + + /// Returns the traffic dependencies at the current time. + pub fn get_current_cut(&self, current_state: &[SemanticWaypoint]) -> Self { + let mut max_time_stamp = 0; + + // the minimum hashmap contains the lower time bound based on the + let mut minimum = HashMap::new(); + for agent in current_state { + max_time_stamp = agent.trajectory_index.max(max_time_stamp); + minimum.insert(agent.agent, agent.trajectory_index); + } + + let mut semantic_graph = Self::default(); + for waypoint in &self.waypoints { + let Some(&minimum_wp) = minimum.get(&waypoint.agent) else { + continue; + }; + if waypoint.trajectory_index < minimum_wp { + continue; + } + if waypoint.trajectory_index > max_time_stamp { + continue; + } + semantic_graph.add_waypoint(waypoint); + } + for (after, befores) in &self.depends_on_all_of { + for before in befores { + let Some(after) = self.waypoints.get(*after) else { + continue; + }; + let Some(before) = self.waypoints.get(*before) else { + continue; + }; + + if !semantic_graph.agent_time_to_wp_id.contains_key(after) { + continue; + } + + if !semantic_graph.agent_time_to_wp_id.contains_key(before) { + continue; + } + + semantic_graph.requires_comes_after(after, before); + } + } + semantic_graph + } + + /// Returns the traffic dependencies at the current time. + pub fn current_traffic_deps( + &self, + current_state: &[SemanticWaypoint], + ) -> DiGraph { + let mut max_time_stamp = 0; + let mut minimum = HashMap::new(); + for agent in current_state { + max_time_stamp = agent.trajectory_index.max(max_time_stamp); + minimum.insert(agent.agent, agent.trajectory_index); + } + + let mut digraph = DiGraph::new(); + let mut node_index_map = HashMap::new(); + for waypoint in &self.waypoints { + let Some(&minimum_wp) = minimum.get(&waypoint.agent) else { + continue; + }; + if waypoint.trajectory_index < minimum_wp { + continue; + } + if waypoint.trajectory_index > max_time_stamp { + continue; + } + let node_index = digraph.add_node(*waypoint); + node_index_map.insert(waypoint, node_index); + } + for (after, befores) in &self.depends_on_all_of { + for before in befores { + let Some(after) = self.waypoints.get(*after) else { + continue; + }; + let Some(before) = self.waypoints.get(*before) else { + continue; + }; + let Some(&after) = node_index_map.get(after) else { + continue; + }; + let Some(&before) = node_index_map.get(before) else { + continue; + }; + digraph.add_edge(before, after, ()); + } + } + digraph + } + + /// Generate a DOT representation of the graph for visualization + /// Also colors the current state of the world. + pub fn to_dot_with_results(&self, waypoints: &[SemanticWaypoint]) -> String { + let mut dot = String::from("digraph SemanticPlan {\n"); + let mut color = "white"; + for (id, waypoint) in self.waypoints.iter().enumerate() { + for wp in waypoints { + if wp.agent == waypoint.agent { + if wp.trajectory_index < waypoint.trajectory_index { + color = "lightyellow"; + } else if wp.trajectory_index == waypoint.trajectory_index { + color = "lightgreen"; + } + } + } + dot.push_str(&format!( + " {} [label=\"Agent: {}, Index: {}\", color = \"{}\"];\n", + id, waypoint.agent, waypoint.trajectory_index, color + )); + } + for (after, befores) in &self.depends_on_all_of { + for before in befores { + dot.push_str(&format!(" {} -> {};\n", before, after)); + } + } + dot.push_str("}\n"); + dot + } + /// Generate a DOT representation of the graph for visualization + /// This is more for debugging than for actual use + pub fn to_dot(&self) -> String { + let mut dot = String::from("digraph SemanticPlan {\n"); + for (id, waypoint) in self.waypoints.iter().enumerate() { + dot.push_str(&format!( + " {} [label=\"Agent: {}, Index: {}\"];\n", + id, waypoint.agent, waypoint.trajectory_index + )); + } + for (after, befores) in &self.depends_on_all_of { + for before in befores { + dot.push_str(&format!(" {} -> {};\n", before, after)); + } + } + dot.push_str("}\n"); + dot + } + + /// Check if node participates in intersection. + pub fn is_intersection_participant( + &self, + waypoint: &SemanticWaypoint, + ) -> Option { + let Some(wp_id) = self.agent_time_to_wp_id.get(waypoint) else { + return None; + }; + + let Some(p) = self.depends_on_all_of.get(wp_id) else { + return None; + }; + + let Some(q) = self.potential_successors.get(wp_id) else { + return None; + }; + if self.is_follower(waypoint).is_empty() { + if p.len() > 1 { + let mut children = HashSet::new(); + for &wp in p { + let wp = self.waypoints[wp]; + if wp.agent == waypoint.agent { + continue; + } + children.insert(wp); + } + return Some(IntersectionType::Next(children)); + } + if q.len() > 1 { + let mut children = HashSet::new(); + for &wp in p { + let wp = self.waypoints[wp]; + if wp.agent == waypoint.agent { + continue; + } + children.insert(wp); + } + return Some(IntersectionType::Lead(children)); + } + } + None + } + + /// Returns if the waypoint is participating in a follower behaviour + pub fn is_follower(&self, waypoint: &SemanticWaypoint) -> Vec { + if waypoint.trajectory_index + 1 > self.last_time() { + // TODO(arjoc) We cant lead when weve reached the end. But we could + // be a follower. We need to update the check in a way that supports this behaviour + return vec![]; + } + + let mut next_waypoint = *waypoint; + next_waypoint.trajectory_index += 1; + + let Some(wp_id) = self.agent_time_to_wp_id.get(waypoint) else { + return vec![]; + }; + let Some(next_wp_id) = self.agent_time_to_wp_id.get(&next_waypoint) else { + return vec![]; + }; + + let Some(t_deps) = self.depends_on_all_of.get(wp_id) else { + return vec![]; + }; + + let Some(t_deps_next) = self.depends_on_all_of.get(next_wp_id) else { + return vec![]; + }; + + let mut depends_on = HashMap::new(); + for potential_follower in t_deps { + if self.waypoints[*potential_follower].agent == waypoint.agent { + continue; + } + + let Some(followers) = depends_on.get_mut(&self.waypoints[*potential_follower].agent) + else { + depends_on.insert( + self.waypoints[*potential_follower].agent, + vec![self.waypoints[*potential_follower].trajectory_index], + ); + continue; + }; + + followers.push(self.waypoints[*potential_follower].trajectory_index); + } + + let mut next_depends_on = HashMap::new(); + for potential_follower in t_deps_next { + if self.waypoints[*potential_follower].agent == waypoint.agent { + continue; + } + + let Some(followers) = + next_depends_on.get_mut(&self.waypoints[*potential_follower].agent) + else { + next_depends_on.insert( + self.waypoints[*potential_follower].agent, + vec![self.waypoints[*potential_follower].trajectory_index], + ); + continue; + }; + followers.push(self.waypoints[*potential_follower].trajectory_index); + } + + let mut followers = vec![]; + for (agent, times) in depends_on { + let Some(prev_times) = next_depends_on.get(&agent) else { + continue; + }; + + if times.iter().max() <= prev_times.clone().iter().max() { + followers.push(SemanticWaypoint { + agent, + trajectory_index: *times.iter().max().unwrap(), + }); + } + } + + followers + } + + pub fn figure_out_leader_follower_zone(&self) -> LeaderFollowerZones { + let mut follow_graph = DiGraph::new(); + + let mut wp_to_nodeid = HashMap::new(); + for wp in &self.waypoints { + let node_id = follow_graph.add_node(*wp); + wp_to_nodeid.insert(*wp, node_id); + } + + // Determine follower relationship + for wp in &self.waypoints { + let vec = self.is_follower(wp); + for leader in vec { + let Some(leader) = wp_to_nodeid.get(&leader) else { + continue; + }; + let Some(follower) = wp_to_nodeid.get(wp) else { + continue; + }; + follow_graph.add_edge(*leader, *follower, ()); + } + } + + // At each timestep cluster the follower graphs + let p = cluster(&follow_graph); + let p: Vec<_> = p + .iter() + .map(|(k, v)| { + ( + *k, + v.iter() + .map(|node_index| follow_graph[*node_index]) + .collect::>(), + ) + }) + .collect(); + + let mut wp_to_cluster: HashMap = HashMap::new(); + for (cluster_id, agent_waypoints) in &p { + for agent_waypoint in agent_waypoints { + wp_to_cluster.insert(*agent_waypoint, *cluster_id); + } + } + + // Then for each cluster perform a topo-sort and figure out who is the leader + let mut cluster_to_leader = HashMap::new(); + let mut leader_to_cluster = HashMap::new(); + + let mut allocation_strategy: HashMap = + HashMap::new(); + + for (cluster_id, agent_waypoint) in &p { + let mut sub_graph = DiGraph::new(); + let mut wp_to_nodeid = HashMap::new(); + for agent_waypoint in agent_waypoint { + let node_id = sub_graph.add_node(*agent_waypoint); + wp_to_nodeid.insert(*agent_waypoint, node_id); + } + + for edge in follow_graph.edge_references() { + let target = follow_graph[edge.target()]; + let source = follow_graph[edge.source()]; + + let Some(source_id) = wp_to_nodeid.get(&source) else { + continue; + }; + + let Some(target_id) = wp_to_nodeid.get(&target) else { + continue; + }; + + sub_graph.add_edge(*source_id, *target_id, ()); + } + + // Extract prime leader + let ts = toposort(&sub_graph, None); + let ts: Vec<_> = ts.unwrap().iter().map(|v| sub_graph[*v]).collect(); + if let Some(&leader) = ts.first() + && ts.len() > 1 + { + cluster_to_leader.insert(cluster_id, leader); + leader_to_cluster.insert(leader, cluster_id); + allocation_strategy.insert( + ts[0], + (AllocationStrategy::Leader(0.0, 0.0), 0, *cluster_id), + ); + for i in 1..ts.len() { + allocation_strategy.insert( + ts[i], + ( + AllocationStrategy::Follower(ts[i - 1].agent), + i, + *cluster_id, + ), + ); + } + } + } + + // Now that we have prime leaders, extract the exact follow groups and determine follow + // sections in the plan + let mut leaders: Vec<_> = leader_to_cluster.keys().cloned().collect(); + + leaders.sort_by(|&a, &b| b.trajectory_index.cmp(&a.trajectory_index)); + + // Get a new "leader segment". + let mut leader_to_leader_segments: HashMap = HashMap::new(); + let mut leader_segment_to_leader: HashMap> = + HashMap::new(); + let mut last_lead_segment = 1usize; + + for leader in leaders { + let mut hypothetical_leader = leader; + hypothetical_leader.trajectory_index += 1; + if let Some(leader_segment) = leader_to_leader_segments.get(&hypothetical_leader) + && let Some(leaders) = leader_segment_to_leader.get_mut(leader_segment) + { + leaders.insert(leader); + leader_to_leader_segments.insert(leader, *leader_segment); + continue; + } + leader_segment_to_leader.insert( + last_lead_segment, + HashSet::from_iter([leader].iter().cloned()), + ); + leader_to_leader_segments.insert(leader, last_lead_segment); + last_lead_segment += 1; + } + + // This table contains the lead segment that we will allocate free space up to for the leader. + let mut allocate_till: HashMap = HashMap::new(); + for (_, lead_members) in leader_segment_to_leader.iter() { + let mut lead_member_buckets: HashMap> = HashMap::new(); + + for lead in lead_members { + if let Some(agents) = lead_member_buckets.get_mut(&lead.agent) { + agents.push(lead); + } + lead_member_buckets.insert(lead.agent, vec![lead]); + } + + // TODO(arjoc): Dont need a hashmap + for (_, mut p) in lead_member_buckets { + p.sort_by(|&a, &b| a.trajectory_index.cmp(&b.trajectory_index)); + + // TODO(arjoc): verify if the lead segment actually needs sorting or a max is sufficient. + let Some(&sem_wp) = p.first() else { + continue; + }; + + if p.len() <= 1 { + allocate_till.insert(*sem_wp, *sem_wp); + continue; + } + + let mut last_upd = 0; + for next_wp in 1..p.len() { + if p[next_wp].trajectory_index - p[next_wp - 1].trajectory_index > 1 + || next_wp == p.len() - 1 + { + let lead_segment = *p[next_wp]; + for id in last_upd..next_wp { + allocate_till.insert(*p[id], lead_segment); + } + last_upd = next_wp; + } + } + } + } + + LeaderFollowerZones { + allocation_strategy, + } + } + + pub fn get_claim_dict( + &self, + current_positions: &[SemanticWaypoint], + ) -> HashMap { + let mut allocate_till: HashMap = HashMap::new(); + + let mut agent_to_pos = HashMap::new(); + for &wp in current_positions { + // Keep track of where other agents are + agent_to_pos.insert(wp.agent, wp); + } + + // Claim every waypoint that can be claimed. + for wp in current_positions { + let mut new_wp = *wp; + loop { + new_wp.trajectory_index += 1; + if !self.agent_time_to_wp_id.contains_key(&new_wp) { + break; + } + let Some(deps) = self.comes_before(&new_wp) else { + break; + }; + let mut continue_to_mark = true; + for dep in deps { + let dep_wp = self.waypoints[*dep]; + let Some(other_agent) = agent_to_pos.get(&dep_wp.agent) else { + continue; + }; + + // HACK FOR TYPE I DEP + if other_agent.agent == wp.agent { + continue; + } + + if other_agent.trajectory_index <= dep_wp.trajectory_index { + // stop blocking + continue_to_mark = false; + } + } + if !continue_to_mark { + break; + } + } + allocate_till.insert(*wp, new_wp); + } + + // Prepare output + let mut final_reservation_table = HashMap::new(); + for (start, end) in allocate_till.iter() { + final_reservation_table.insert( + start.agent, + CurrentlyAllocatedTrajSegment { + start_id: start.trajectory_index, + end_id: end.trajectory_index, + }, + ); + } + final_reservation_table + } + + pub fn check_for_violation(&self, current_pos: &[SemanticWaypoint]) -> bool { + let mut agent_to_pos = HashMap::new(); + for &wp in current_pos { + agent_to_pos.insert(wp.agent, wp); + } + + for wp in current_pos { + if let Some(deps) = self.comes_before(wp) { + for &dep_idx in deps { + let dep_wp = self.waypoints[dep_idx]; + + if dep_wp.agent == wp.agent { + continue; + } + + if let Some(other_agent) = agent_to_pos.get(&dep_wp.agent) + && other_agent.trajectory_index <= dep_wp.trajectory_index + { + return true; + } + } + } + } + false + } +} + +/// The currently allocated trajectory segment based on the overall +/// progress of all other robots. +pub struct CurrentlyAllocatedTrajSegment { + pub start_id: usize, + pub end_id: usize, +} + +/// Representation of the type of dependency. Is it a leader, a follower, an +/// intersection or in the vicinity. +pub enum AllocationStrategy { + Leader(f64, f64), // Clears space till the end of the leader + Follower(usize), // Follows x + IntersectionLead(usize), + IntersectionWait(SemanticWaypoint), + Vicinity, +} + +/// Leader-follower zones describe space time regions in which leader-follower relationships hold +pub(crate) struct LeaderFollowerZones { + pub(crate) allocation_strategy: HashMap, +} + +#[derive(Debug, Clone)] +pub enum IntersectionType { + Lead(HashSet), + Next(HashSet), +} + +fn cluster(g: &DiGraph) -> HashMap> { + let mut node_sets = petgraph::unionfind::UnionFind::new(g.node_count()); + for edge in g.edge_references() { + let (a, b) = (edge.source(), edge.target()); + + // union the two nodes of the edge + node_sets.union(a.index(), b.index()); + } + + // 4. Organize nodes into clusters (groups) using a HashMap. + // The keys will be the canonical set IDs, and values will be lists of nodes in that cluster. + let mut clusters = HashMap::new(); + + for node_idx in g.node_indices() { + let raw_idx = node_idx.index(); + // Find the canonical root (set ID) for this node + let set_id = node_sets.find(raw_idx); + + // Add the node to the corresponding cluster list + clusters + .entry(set_id) + .or_insert_with(Vec::new) + .push(node_idx); + } + + clusters +} + +/// Pose Trajectory +#[derive(Debug, Clone)] +pub struct Trajectory { + pub poses: Vec>, +} + +impl Trajectory { + pub fn len(&self) -> usize { + self.poses.len() + } +} + +impl From<&crate::motion::Trajectory> for Trajectory +where + W: crate::motion::r2::Positioned + crate::motion::se2::MaybeOriented + crate::motion::Waypoint, +{ + fn from(traj: &crate::motion::Trajectory) -> Self { + let mut poses = Vec::new(); + for wp in traj.iter() { + let p = wp.point(); + let yaw = wp.maybe_oriented().map(|o| o.angle()).unwrap_or(0.0); + poses.push(Isometry2::new(na::Vector2::new(p.x, p.y), yaw)); + } + Self { poses } + } +} + +/// Input of time discretized MAPF result +/// The trajectories are time discretized, and the +/// footprints are the shapes of the agents. +/// We expect all trajectories to be of the same length. +#[derive(Clone)] +pub struct MapfResult { + /// The trajectories of the agents + pub trajectories: Vec, + /// The shapes of the agents + pub footprints: Vec>, + /// The time discretization of the trajectories + pub discretization_timestep: f64, +} + +impl std::fmt::Debug for MapfResult { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + f.debug_struct("MapfResult") + .field("trajectories", &self.trajectories) + .field("discretization_timestep", &self.discretization_timestep) + .finish() + } +} + +/// Sweep the objects to check their motion +fn calculate_nonlinear_rigid_motion( + isometry1: &Isometry2, + isometry2: &Isometry2, + delta_time: f64, // Time elapsed between the two isometries + local_center: Point2, // Local center of rotation +) -> NonlinearRigidMotion { + // 1. Calculate Linear Velocity: + let linear_velocity = + (isometry2.translation.vector - isometry1.translation.vector) / delta_time; + + // 2. Calculate Angular Velocity: + // This is more complex and involves extracting the rotation difference. + let rotation1 = isometry1.rotation; + let rotation2 = isometry2.rotation; + + // Calculate the relative rotation (rotation that transforms rotation1 to rotation2) + let relative_rotation = rotation2 * rotation1.inverse(); + + // Convert the relative rotation to an axis-angle representation + let angle = relative_rotation.angle(); + + // Angular velocity is the axis scaled by the angle and divided by the time step. + let angular_velocity = angle / delta_time; + + NonlinearRigidMotion { + start: *isometry1, // Or *isometry2, depending on your starting point + local_center, + linvel: linear_velocity, + angvel: angular_velocity, + } +} + +/// Check if the robots collide while executing a motion. +fn collides( + ti1: &Isometry2, + ti2: &Isometry2, + shape_i: &dyn Shape, + tj1: &Isometry2, + tj2: &Isometry2, + shape_j: &dyn Shape, + delta_time: f64, +) -> bool { + let motion_i = calculate_nonlinear_rigid_motion(ti1, ti2, delta_time, Point2::origin()); + let motion_j = calculate_nonlinear_rigid_motion(tj1, tj2, delta_time, Point2::origin()); + let time_of_impact = cast_shapes_nonlinear( + &motion_i, shape_i, &motion_j, shape_j, 0.0, delta_time, true, + ); + if let Ok(Some(toi)) = time_of_impact { + // Check if the time of impact is within the delta_time + toi.status == ShapeCastStatus::Converged && toi.time_of_impact <= delta_time + || toi.status == ShapeCastStatus::PenetratingOrWithinTargetDist + } else { + false + } +} + +/// Simple implementation of mapf_post: A MapF -> semantic plan +/// Method. +/// TODO(arjoc): This can be improved. +/// +/// Based on https://whoenig.github.io/publications/2019_RA-L_Hoenig.pdf +pub fn mapf_post(mapf_result: &MapfResult) -> SemanticPlan { + let mut semantic_plan = SemanticPlan::default(); + semantic_plan.num_agents = mapf_result.trajectories.len(); + // Type 1 edges + for agent in 0..mapf_result.trajectories.len() { + for trajectory_index in 0..mapf_result.trajectories[agent].len() { + semantic_plan.add_waypoint(&SemanticWaypoint { + agent, + trajectory_index, + }); + + if trajectory_index < 1 { + semantic_plan + .depends_on_all_of + .insert(semantic_plan.waypoints.len() - 1, vec![]); + continue; + } + semantic_plan.depends_on_all_of.insert( + semantic_plan.waypoints.len() - 1, + vec![semantic_plan.waypoints.len() - 2], + ); + semantic_plan.potential_successors.insert( + semantic_plan.waypoints.len() - 2, + vec![semantic_plan.waypoints.len() - 1], + ); + } + } + + // Type 2 edges + for agent1 in 0..mapf_result.trajectories.len() { + for trajectory_index1 in 1..mapf_result.trajectories[agent1].len() { + for agent2 in 0..mapf_result.trajectories.len() { + if agent1 == agent2 { + continue; + } + for trajectory_index2 in + trajectory_index1 + 1..mapf_result.trajectories[agent2].len() + { + if collides( + &mapf_result.trajectories[agent1].poses[trajectory_index1 - 1], + &mapf_result.trajectories[agent1].poses[trajectory_index1], + &*mapf_result.footprints[agent1], + &mapf_result.trajectories[agent2].poses[trajectory_index2 - 1], + &mapf_result.trajectories[agent2].poses[trajectory_index2], + &*mapf_result.footprints[agent2], + mapf_result.discretization_timestep, + ) { + semantic_plan.requires_comes_after( + &SemanticWaypoint { + agent: agent1, + trajectory_index: trajectory_index1 - 1, + }, + &SemanticWaypoint { + agent: agent2, + trajectory_index: trajectory_index2 - 1, + }, + ); + } + } + } + } + } + semantic_plan +} + +// --- +// Unit Tests +// --- + +#[cfg(test)] +mod tests { + use parry2d::{na::Vector2, utils::hashset::HashSet}; + + use super::*; + use std::collections::HashMap; // Make sure HashMap is in scope + + #[test] + fn test_add_waypoint() { + let mut plan = SemanticPlan::default(); + let wp1 = SemanticWaypoint { + agent: 0, + trajectory_index: 0, + }; + let wp2 = SemanticWaypoint { + agent: 0, + trajectory_index: 1, + }; + let wp3 = SemanticWaypoint { + agent: 1, + trajectory_index: 0, + }; + + plan.add_waypoint(&wp1); + assert_eq!(plan.waypoints.len(), 1); + assert_eq!(plan.waypoints[0], wp1); + assert_eq!(plan.agent_time_to_wp_id.get(&wp1), Some(&0)); + + plan.add_waypoint(&wp2); + assert_eq!(plan.waypoints.len(), 2); + assert_eq!(plan.waypoints[1], wp2); + assert_eq!(plan.agent_time_to_wp_id.get(&wp2), Some(&1)); + + plan.add_waypoint(&wp3); + assert_eq!(plan.waypoints.len(), 3); + assert_eq!(plan.waypoints[2], wp3); + assert_eq!(plan.agent_time_to_wp_id.get(&wp3), Some(&2)); + + // Adding an existing waypoint (by value) should still add it again with a new ID + // This behavior is derived from how `add_waypoint` is implemented. + let wp1_again = SemanticWaypoint { + agent: 0, + trajectory_index: 0, + }; + plan.add_waypoint(&wp1_again); + assert_eq!(plan.waypoints.len(), 4); + assert_eq!(plan.waypoints[3], wp1_again); + assert_eq!(plan.agent_time_to_wp_id.get(&wp1_again), Some(&3)); // Now maps to the new ID + } + + #[test] + fn test_add_comes_after_basic() { + let mut plan = SemanticPlan::default(); + let wp0_0 = SemanticWaypoint { + agent: 0, + trajectory_index: 0, + }; // ID 0 + let wp0_1 = SemanticWaypoint { + agent: 0, + trajectory_index: 1, + }; // ID 1 + let wp1_0 = SemanticWaypoint { + agent: 1, + trajectory_index: 0, + }; // ID 2 + + plan.add_waypoint(&wp0_0); + plan.add_waypoint(&wp0_1); + plan.add_waypoint(&wp1_0); + + // wp1_0 (ID 2) comes after wp0_0 (ID 0) + plan.requires_comes_after(&wp0_0, &wp1_0); + let mut expected_comes_after: HashMap> = HashMap::new(); + expected_comes_after.insert(2, vec![0]); + assert_eq!(plan.depends_on_all_of, expected_comes_after); + + // wp1_0 (ID 2) also comes after wp0_1 (ID 1) + plan.requires_comes_after(&wp0_1, &wp1_0); + let mut expected_comes_after_2: HashMap> = HashMap::new(); + expected_comes_after_2.insert(2, vec![0, 1]); + + // Retrieve the actual vector and sort it for consistent comparison + let mut actual_vec = plan.depends_on_all_of.get(&2).unwrap().clone(); + actual_vec.sort_unstable(); + + // Retrieve the expected vector and sort it + let mut expected_vec = expected_comes_after_2.get(&2).unwrap().clone(); + expected_vec.sort_unstable(); + + assert_eq!(actual_vec, expected_vec); + } + + #[test] + fn test_add_comes_after_non_existent_waypoints() { + let mut plan = SemanticPlan::default(); + let wp_a = SemanticWaypoint { + agent: 0, + trajectory_index: 0, + }; + let wp_b = SemanticWaypoint { + agent: 0, + trajectory_index: 1, + }; + let wp_c = SemanticWaypoint { + agent: 0, + trajectory_index: 2, + }; + + plan.add_waypoint(&wp_a); // ID 0 + + // Attempt to add a dependency where 'before' waypoint (wp_b) is not in the plan + plan.requires_comes_after(&wp_b, &wp_a); + assert!(plan.depends_on_all_of.is_empty()); + + // Attempt to add a dependency where 'after' waypoint (wp_c) is not in the plan + plan.requires_comes_after(&wp_a, &wp_c); + assert!(plan.depends_on_all_of.is_empty()); + + // Add wp_c to the plan + plan.add_waypoint(&wp_c); // ID 1 + + // Now, this dependency should be added successfully: wp_c (ID 1) comes after wp_a (ID 0) + plan.requires_comes_after(&wp_a, &wp_c); + let mut expected_comes_after: HashMap> = HashMap::new(); + expected_comes_after.insert(1, vec![0]); + assert_eq!(plan.depends_on_all_of, expected_comes_after); + } + + #[test] + fn test_add_comes_after_self_dependency() { + let mut plan = SemanticPlan::default(); + let wp1 = SemanticWaypoint { + agent: 0, + trajectory_index: 0, + }; + + plan.add_waypoint(&wp1); // ID 0 + + // Test self-dependency: wp1 (ID 0) comes after wp1 (ID 0) + plan.requires_comes_after(&wp1, &wp1); + let mut expected_comes_after: HashMap> = HashMap::new(); + expected_comes_after.insert(0, vec![0]); + assert_eq!(plan.depends_on_all_of, expected_comes_after); + } + + #[test] + fn test_collision_detection() { + let isometry1 = Isometry2::new(Vector2::new(0.0, 0.0), 0.0); + let isometry2 = Isometry2::new(Vector2::new(1.0, 1.0), 0.0); + let shape1 = Arc::new(parry2d::shape::Ball::new(0.5)); + let shape2 = Arc::new(parry2d::shape::Ball::new(0.5)); + let delta_time = 1.0; + + assert!(collides( + &isometry1, &isometry2, &*shape1, &isometry1, &isometry2, &*shape2, delta_time + )); + } + + #[test] + fn test_post_processing() { + // Create a cross junction MapfResult + let mapf_result = MapfResult { + trajectories: vec![ + vec![ + Isometry2::new(Vector2::new(0.0, 0.0), 0.0), + Isometry2::new(Vector2::new(1.0, 0.0), 0.0), + Isometry2::new(Vector2::new(2.0, 0.0), 0.0), + Isometry2::new(Vector2::new(2.0, 0.0), 0.0), + ], // Trajectory for agent1 (horizontal path) + vec![ + Isometry2::new(Vector2::new(1.0, -1.0), 0.0), + Isometry2::new(Vector2::new(1.0, -1.0), 0.0), + Isometry2::new(Vector2::new(1.0, 0.0), 0.0), + Isometry2::new(Vector2::new(1.0, 1.0), 0.0), + ], // Trajectory for agent2 (vertical path) + ] + .into_iter() + .map(|poses| Trajectory { poses }) + .collect(), + footprints: vec![ + Arc::new(parry2d::shape::Ball::new(0.49)), // Footprint for agent1 + Arc::new(parry2d::shape::Ball::new(0.49)), // Footprint for agent2 + ], + discretization_timestep: 1.0, // Example timestep + }; + + // Generate the semantic plan + let semantic_plan = mapf_post(&mapf_result); + // Check the number of waypoints + assert_eq!(semantic_plan.waypoints.len(), 8); // 4 for each agent + let res = semantic_plan + .comes_before(&SemanticWaypoint { + agent: 1, + trajectory_index: 2, + }) + .unwrap(); + assert_eq!(res.len(), 3); // 3 waypoints should come before this one + let t1 = semantic_plan.waypoints[res[0]]; + let t2 = semantic_plan.waypoints[res[1]]; + let t3 = semantic_plan.waypoints[res[2]]; + + let desired = HashSet::from_iter(vec![t1, t2, t3]); + assert!(desired.contains(&SemanticWaypoint { + agent: 0, + trajectory_index: 1 + })); + assert!(desired.contains(&SemanticWaypoint { + agent: 1, + trajectory_index: 1 + })); + } + + #[test] + fn test_get_claims_dict_follow() { + use std::sync::Arc; + + let mapf_result = MapfResult { + trajectories: vec![ + vec![ + Isometry2::new(Vector2::new(0.0, 0.0), 0.0), + Isometry2::new(Vector2::new(1.0, 0.0), 0.0), + Isometry2::new(Vector2::new(2.0, 0.0), 0.0), + Isometry2::new(Vector2::new(3.0, 0.0), 0.0), + Isometry2::new(Vector2::new(4.0, 0.0), 0.0), + Isometry2::new(Vector2::new(4.0, 0.0), 0.0), + ], // Trajectory for agent1 + vec![ + Isometry2::new(Vector2::new(1.0, 0.0), 0.0), + Isometry2::new(Vector2::new(2.0, 0.0), 0.0), + Isometry2::new(Vector2::new(3.0, 0.0), 0.0), + Isometry2::new(Vector2::new(4.0, 0.0), 0.0), + Isometry2::new(Vector2::new(5.0, 0.0), 0.0), + Isometry2::new(Vector2::new(5.0, 0.0), 0.0), + ], // Trajectory for agent2 + ] + .into_iter() + .map(|poses| Trajectory { poses }) + .collect(), + footprints: vec![ + Arc::new(parry2d::shape::Ball::new(0.49)), // Footprint for agent1 + Arc::new(parry2d::shape::Ball::new(0.49)), // Footprint for agent2 + ], + discretization_timestep: 1.0, // Example timestep + }; + + let semantic_plan = mapf_post(&mapf_result); + + let reserved_parts = semantic_plan.get_claim_dict(&vec![ + SemanticWaypoint { + agent: 0, + trajectory_index: 0, + }, + SemanticWaypoint { + agent: 1, + trajectory_index: 3, + }, + ]); + + assert_eq!(reserved_parts.len(), 2); + assert_eq!(reserved_parts.get(&0).unwrap().end_id, 4); + assert_eq!(reserved_parts.get(&1).unwrap().end_id, 6); + + let reserved_parts = semantic_plan.get_claim_dict(&vec![ + SemanticWaypoint { + agent: 0, + trajectory_index: 3, + }, + SemanticWaypoint { + agent: 1, + trajectory_index: 3, + }, + ]); + + assert_eq!(reserved_parts.len(), 2); + assert_eq!(reserved_parts.get(&0).unwrap().end_id, 4); + assert_eq!(reserved_parts.get(&1).unwrap().end_id, 6); + } + + #[test] + fn test_check_for_violation() { + use std::sync::Arc; + + let mapf_result = MapfResult { + trajectories: vec![ + vec![ + Isometry2::new(Vector2::new(0.0, 0.0), 0.0), + Isometry2::new(Vector2::new(1.0, 0.0), 0.0), + Isometry2::new(Vector2::new(2.0, 0.0), 0.0), + Isometry2::new(Vector2::new(3.0, 0.0), 0.0), + Isometry2::new(Vector2::new(4.0, 0.0), 0.0), + Isometry2::new(Vector2::new(4.0, 0.0), 0.0), + ], // Trajectory for agent1 + vec![ + Isometry2::new(Vector2::new(1.0, 0.0), 0.0), + Isometry2::new(Vector2::new(2.0, 0.0), 0.0), + Isometry2::new(Vector2::new(3.0, 0.0), 0.0), + Isometry2::new(Vector2::new(4.0, 0.0), 0.0), + Isometry2::new(Vector2::new(5.0, 0.0), 0.0), + Isometry2::new(Vector2::new(5.0, 0.0), 0.0), + ], // Trajectory for agent2 + ] + .into_iter() + .map(|poses| Trajectory { poses }) + .collect(), + footprints: vec![ + Arc::new(parry2d::shape::Ball::new(0.49)), + Arc::new(parry2d::shape::Ball::new(0.49)), + ], + discretization_timestep: 1.0, + }; + + let semantic_plan = mapf_post(&mapf_result); + + // Case 1: Safe state + let safe_state = vec![ + SemanticWaypoint { + agent: 0, + trajectory_index: 0, + }, + SemanticWaypoint { + agent: 1, + trajectory_index: 0, + }, + ]; + assert!(!semantic_plan.check_for_violation(&safe_state)); + + // Case 2: Violation + let violation_state = vec![ + SemanticWaypoint { + agent: 0, + trajectory_index: 1, + }, + SemanticWaypoint { + agent: 1, + trajectory_index: 0, + }, + ]; + assert!(semantic_plan.check_for_violation(&violation_state)); + + // Case 3: Safe again + let safe_state_2 = vec![ + SemanticWaypoint { + agent: 0, + trajectory_index: 1, + }, + SemanticWaypoint { + agent: 1, + trajectory_index: 1, + }, + ]; + assert!(!semantic_plan.check_for_violation(&safe_state_2)); + } +} diff --git a/mapf/src/post/spatial_allocation/mod.rs b/mapf/src/post/spatial_allocation/mod.rs new file mode 100644 index 0000000..fd6333d --- /dev/null +++ b/mapf/src/post/spatial_allocation/mod.rs @@ -0,0 +1,352 @@ +use std::collections::{HashMap, HashSet}; + +use bresenham::Bresenham; + +use super::{ + IntersectionType, LeaderFollowerZones, MapfResult, SemanticPlan, SemanticWaypoint, mapf_post, +}; + +#[derive(Clone)] +pub struct CurrentPosition { + pub semantic_position: SemanticWaypoint, + pub real_position: (f64, f64), +} +#[derive(Clone)] +pub struct Grid2D { + static_obstacles: Vec>, + cell_size: f64, + width: usize, + height: usize, +} + +impl Grid2D { + pub fn new(static_obstacles: Vec>, cell_size: f64) -> Self { + let width = static_obstacles.len(); + let height = if width > 0 { + static_obstacles[0].len() + } else { + 0 + }; + Self { + static_obstacles, + cell_size, + width, + height, + } + } + + pub fn to_world_coords(&self, x: isize, y: isize) -> (f64, f64) { + ( + self.cell_size * x as f64, + self.cell_size * (self.height as isize - 1 - y) as f64, + ) + } + + pub fn from_world_coords(&self, x: f64, y: f64) -> (isize, isize) { + ( + (x / self.cell_size) as isize, + (self.height as isize - 1 - (y / self.cell_size) as isize), + ) + } + + pub fn allocate_trajectory( + &self, + trajectories: &MapfResult, + positions: &[CurrentPosition], + ) -> AllocationField { + let semantic_plan = mapf_post(trajectories); + let semantic_positions: Vec = positions.iter().map(|p| p.semantic_position).collect(); + let assignment = semantic_plan.get_claim_dict(&semantic_positions); + + let mut allocation_field = AllocationField::create(semantic_plan, 1000, 1000); + + for (agent, traj) in trajectories.trajectories.iter().enumerate() { + let Some(region) = assignment.get(&agent) else { + panic!("Unknown Agent - something went wrong in get_claim_dict"); + }; + for i in region.start_id..region.end_id + 1 { + if i >= traj.poses.len() { + continue; + } + + //TODO(arjoc) Add a function parameter getting robot's current real position when i = start_id + let start_pose = traj.poses[i]; + let start_coords = + self.from_world_coords(start_pose.translation.x, start_pose.translation.y); + allocation_field.mark( + start_coords.0, + start_coords.1, + &TrajectoryAllocation { + agent, + priority: i, + dist_from_center: 0.0, + }, + ); + /*self.blur(&TrajectoryAllocation { + agent, + priority: i, + dist_from_center: 0.0 + }, &start_coords, &mut allocation_field, 1.0);*/ + if i + 1 >= traj.poses.len() { + continue; + } + + let end_pose = traj.poses[i + 1]; + let end_coords = + self.from_world_coords(end_pose.translation.x, end_pose.translation.y); + + for (x, y) in Bresenham::new(start_coords, end_coords) { + allocation_field.mark( + x, + y, + &TrajectoryAllocation { + agent, + priority: i, + dist_from_center: 0.0, + }, + ); + self.blur( + &TrajectoryAllocation { + agent, + priority: i, + dist_from_center: 0.0, + }, + &(x, y), + &mut allocation_field, + 1.0, + ); + } + } + } + allocation_field + } + + fn blur( + &self, + trajectory_alloc: &TrajectoryAllocation, + start_cell: &(isize, isize), + grid_space: &mut AllocationField, + max_dist: f64, + ) { + let mut stack = vec![(*start_cell, 0.0f64)]; + let mut visited = HashSet::new(); + grid_space.mark(start_cell.0, start_cell.1, trajectory_alloc); + while let Some(((x, y), dist)) = stack.pop() { + for i in -1..=1 { + for j in -1..=1 { + if x + i < 0 || y + j < 0 { + continue; + } + let nx = (x + i) as usize; + let ny = (y + j) as usize; + + if visited.contains(&(nx, ny)) { + continue; + } + + if nx >= self.static_obstacles.len() { + continue; + } + + if ny >= self.static_obstacles[nx].len() { + continue; + } + + if self.static_obstacles[nx][ny] == 100 { + continue; + } + + if dist + 1.0 > max_dist { + continue; + } + stack.push(((nx as isize, ny as isize), dist + 1.0)); + let alloc = TrajectoryAllocation { + agent: trajectory_alloc.agent, + priority: trajectory_alloc.priority, + dist_from_center: dist + 1.0, + }; + grid_space.mark(nx as isize, ny as isize, &alloc); + visited.insert((nx, ny)); + } + } + } + } +} + +#[derive(Clone, Debug)] +pub struct TrajectoryAllocation { + agent: usize, + priority: usize, + dist_from_center: f64, +} + +impl TrajectoryAllocation { + fn to_wp(&self) -> SemanticWaypoint { + SemanticWaypoint { + agent: self.agent, + trajectory_index: self.priority, + } + } +} + +/// An allocation field. Each point on the grid is allocated to a specific robot +/// based on its original MAPF plan +pub struct AllocationField { + grid_space: Vec>>, + plan: SemanticPlan, + leader_follower: LeaderFollowerZones, + cell_by_agent: HashMap>, +} + +impl AllocationField { + fn create(plan: SemanticPlan, width: usize, height: usize) -> Self { + let leader_follower = plan.figure_out_leader_follower_zone(); + Self { + grid_space: vec![vec![None; height]; width], + plan, + leader_follower, + cell_by_agent: HashMap::default(), + } + } + + fn width(&self) -> usize { + self.grid_space.len() + } + + fn height(&self) -> usize { + if self.width() == 0 { + 0 + } else { + self.grid_space[0].len() + } + } + + fn update_spot(&mut self, alloc: &TrajectoryAllocation, x: usize, y: usize) { + if let Some(prev_alloc) = &self.grid_space[x][y] + && let Some(p) = self.cell_by_agent.get_mut(&prev_alloc.agent) { + p.remove(&(x, y)); + } + self.grid_space[x][y] = Some(alloc.clone()); + if let Some(allocated_list) = self.cell_by_agent.get_mut(&alloc.agent) { + allocated_list.insert((x, y)); + } else { + self.cell_by_agent + .insert(alloc.agent, HashSet::from_iter([(x, y)].iter().cloned())); + } + } + + fn mark(&mut self, x: isize, y: isize, alloc: &TrajectoryAllocation) { + let x = x as usize; + let y = y as usize; + + if x >= self.width() { + return; + } + + if y >= self.height() { + return; + } + + let Some(previous_alloc) = self.grid_space[x][y].clone() else { + self.update_spot(alloc, x, y); + return; + }; + + // We keep reservations from earlier times first + if previous_alloc.priority < alloc.priority { + // Existing (earlier) reservation wins; leave it unchanged. + return; + } + // In the event that a reservation is of the same priority/time step + // we need to apply rules for allocation. + // In the event the two agents have no relationship at that time step + // partition the space by who ever is nearer. + // In the event the relationship is a leader follower one give priority + // to the leader. + // In the event the relationship is an intersection based relationship, + // give priority to the agent that is leaving the junction. + else if previous_alloc.priority == alloc.priority { + // Follower logic for tie-breaking. + if let Some((_mode, priority, cluster_id)) = self + .leader_follower + .allocation_strategy + .get(&previous_alloc.to_wp()) + && let Some((_mode2, priority2, cluster_id2)) = + self.leader_follower.allocation_strategy.get(&alloc.to_wp()) + && cluster_id == cluster_id2 { + if priority <= priority2 { + self.grid_space[x][y] = Some(previous_alloc.clone()); + return; + } else { + self.update_spot(alloc, x, y); + return; + } + } + + // Logic for intersections + if let Some(intersection_type) = self + .plan + .is_intersection_participant(&previous_alloc.to_wp()) + { + if let IntersectionType::Lead(followers) = intersection_type { + if followers.contains(&alloc.to_wp()) { + self.grid_space[x][y] = Some(alloc.clone()); + return; + } + } else if let IntersectionType::Next(leaders) = intersection_type + && leaders.contains(&alloc.to_wp()) { + self.update_spot(&previous_alloc, x, y); + return; + } + } + // Vicinity rule. + if previous_alloc.dist_from_center < alloc.dist_from_center { + self.grid_space[x][y] = Some(previous_alloc.clone()); + } else { + self.update_spot(alloc, x, y); + } + } else { + self.update_spot(alloc, x, y); + } + } + + pub fn get_allocation(&self, x: isize, y: isize) -> Option { + if x < 0 || y < 0 { + return None; + } + let x = x as usize; + let y = y as usize; + + if x >= self.width() || y >= self.height() { + return None; + } + + let Some(p) = self.grid_space[x][y].clone() else { + return None; + }; + Some(p.agent) + } + + pub fn get_alloc_for_agent(&self, agent: usize) -> Option> { + self.cell_by_agent + .get(&agent) + .map(|p| Vec::from_iter(p.iter().cloned())) + } + + pub fn get_alloc_priority(&self, x: isize, y: isize) -> Option { + if x < 0 || y < 0 { + return None; + } + let x = x as usize; + let y = y as usize; + + if x >= self.width() || y >= self.height() { + return None; + } + + let Some(p) = self.grid_space[x][y].clone() else { + return None; + }; + Some(p.priority) + } +} From 2d88e900d07382296dd5ac9e06081a66182478da Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 10 Mar 2026 04:35:39 +0000 Subject: [PATCH 02/11] Integrate Scenario with solver and post-processor This adds solve(), derive_mapf_result(), and derive_semantic_plan() to the Scenario struct for tighter integration between MAPF solving and trajectory derivation. Generated-by: Gemini-CLI Signed-off-by: Arjo Chakravarty --- mapf/Cargo.toml | 2 +- mapf/src/negotiation/mod.rs | 67 ++++++++++++++++++++++++++++++++++++- 2 files changed, 67 insertions(+), 2 deletions(-) diff --git a/mapf/Cargo.toml b/mapf/Cargo.toml index 9d188da..64586c0 100644 --- a/mapf/Cargo.toml +++ b/mapf/Cargo.toml @@ -11,7 +11,7 @@ keywords = ["mapf", "multi", "agent", "planning", "search"] maintenance = {status = "experimental"} [dependencies] -nalgebra = "0.32" +nalgebra = "0.33" time-point = "0.1" sorted-vec = "0.8.2" cached = "0.40" diff --git a/mapf/src/negotiation/mod.rs b/mapf/src/negotiation/mod.rs index 49f7f78..32c8d7e 100644 --- a/mapf/src/negotiation/mod.rs +++ b/mapf/src/negotiation/mod.rs @@ -32,7 +32,7 @@ use crate::{ se2::{DifferentialDriveLineFollow, WaypointSE2}, trajectory::TrajectoryIter, BoundingBox, CcbsConstraint, CcbsEnvironment, CircularProfile, Duration, - DynamicCircularObstacle, DynamicEnvironment, TimePoint, Timed, TravelEffortCost, + DynamicCircularObstacle, DynamicEnvironment, Motion, TimePoint, Timed, TravelEffortCost, }, planner::{halt::QueueLengthLimit, Planner}, premade::{SippSE2, StateSippSE2}, @@ -653,6 +653,71 @@ impl NegotiationNode { } } +impl Scenario { + pub fn solve( + &self, + queue_length_limit: Option, + ) -> Result { + let (solution, _, _) = negotiate(self, queue_length_limit)?; + Ok(solution) + } + + pub fn derive_mapf_result( + &self, + solution: &NegotiationNode, + timestep: f64, + ) -> crate::post::MapfResult { + let mut max_finish_time = TimePoint::zero(); + for proposal in solution.proposals.values() { + max_finish_time = max_finish_time.max(proposal.meta.trajectory.finish_motion().time()); + } + + let mut trajectories = Vec::new(); + let mut footprints = Vec::new(); + + for (i, (_name, agent)) in self.agents.iter().enumerate() { + footprints.push(std::sync::Arc::new(crate::post::shape::Ball::new(agent.radius)) + as std::sync::Arc); + + let mut poses = Vec::new(); + if let Some(proposal) = solution.proposals.get(&i) { + let traj = &proposal.meta.trajectory; + let motion = traj.motion(); + let start_time = traj.initial_motion().time(); + + let duration = max_finish_time - start_time; + let steps = (duration.as_secs_f64() / timestep).ceil() as usize; + + for step in 0..=steps { + let mut t = start_time + Duration::from_secs_f64(step as f64 * timestep); + if t > max_finish_time { + t = max_finish_time; + } + if let Ok(pos) = motion.compute_position(&t) { + poses.push(pos); + } + } + } + trajectories.push(crate::post::Trajectory { poses }); + } + + crate::post::MapfResult { + trajectories, + footprints, + discretization_timestep: timestep, + } + } + + pub fn derive_semantic_plan( + &self, + solution: &NegotiationNode, + timestep: f64, + ) -> crate::post::SemanticPlan { + let result = self.derive_mapf_result(solution, timestep); + crate::post::mapf_post(&result) + } +} + #[derive(Clone)] struct QueueEntry { node: NegotiationNode, From d3a5f32493971bab85a5d73ff4f0af0cd332d5bc Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 17 Mar 2026 06:07:49 +0000 Subject: [PATCH 03/11] Include moving obstacles as agents in MapfResult for traffic dependencies This allows obstacles in a Scenario to be represented as agents in the MAPFPost output, enabling traffic dependency analysis for moving objects. Generated-by: Gemini-CLI Signed-off-by: Arjo Chakravarty --- mapf/src/negotiation/mod.rs | 24 ++++- mapf/src/negotiation/scenario.rs | 170 ++++++++++++++++++++++++++++++- 2 files changed, 189 insertions(+), 5 deletions(-) diff --git a/mapf/src/negotiation/mod.rs b/mapf/src/negotiation/mod.rs index 32c8d7e..3f85c4e 100644 --- a/mapf/src/negotiation/mod.rs +++ b/mapf/src/negotiation/mod.rs @@ -55,7 +55,7 @@ pub enum NegotiationError { } pub fn negotiate( - scenario: &Scenario, + scenario: &mut Scenario, queue_length_limit: Option, ) -> Result< ( @@ -98,6 +98,7 @@ pub fn negotiate( agents.push(agent.clone()); } + scenario.id_to_name = name_map.clone(); (name_map, agents) }; @@ -655,7 +656,7 @@ impl NegotiationNode { impl Scenario { pub fn solve( - &self, + &mut self, queue_length_limit: Option, ) -> Result { let (solution, _, _) = negotiate(self, queue_length_limit)?; @@ -672,6 +673,12 @@ impl Scenario { max_finish_time = max_finish_time.max(proposal.meta.trajectory.finish_motion().time()); } + for obstacle in &self.obstacles { + if let Some(last) = obstacle.trajectory.last() { + max_finish_time = max_finish_time.max(TimePoint::from_secs_f64(last.0)); + } + } + let mut trajectories = Vec::new(); let mut footprints = Vec::new(); @@ -701,6 +708,19 @@ impl Scenario { trajectories.push(crate::post::Trajectory { poses }); } + for obstacle in &self.obstacles { + footprints.push(std::sync::Arc::new(crate::post::shape::Ball::new(obstacle.radius)) + as std::sync::Arc); + + let mut poses = Vec::new(); + let steps = (max_finish_time.as_secs_f64() / timestep).ceil() as usize; + for step in 0..=steps { + let t = step as f64 * timestep; + poses.push(obstacle.interpolate(t, self.cell_size)); + } + trajectories.push(crate::post::Trajectory { poses }); + } + crate::post::MapfResult { trajectories, footprints, diff --git a/mapf/src/negotiation/scenario.rs b/mapf/src/negotiation/scenario.rs index 69073d4..904e81b 100644 --- a/mapf/src/negotiation/scenario.rs +++ b/mapf/src/negotiation/scenario.rs @@ -18,10 +18,11 @@ use crate::{ graph::occupancy::Cell, motion::{ - se2::{GoalSE2, Orientation, StartSE2, WaypointSE2}, + se2::{GoalSE2, Orientation, Position, StartSE2, WaypointSE2}, TimePoint, Trajectory, }, }; +use nalgebra::{Isometry2, Vector2}; use serde::{Deserialize, Serialize}; use std::collections::{BTreeMap, HashMap}; @@ -69,7 +70,7 @@ impl Agent { } } -#[derive(Serialize, Deserialize)] +#[derive(Serialize, Deserialize, Clone)] pub struct Obstacle { /// Trajectory of the obstacle in terms of (time (s), x cell, y cell) pub trajectory: Vec<(f64, i64, i64)>, @@ -97,9 +98,78 @@ impl Obstacle { indefinite_finish: trajectory.has_indefinite_finish_time(), } } + + pub fn to_agent(&self) -> Option { + let first = self.trajectory.first()?; + let last = self.trajectory.last()?; + let yaw = if self.trajectory.len() > 1 { + let second = &self.trajectory[1]; + let dx = (second.1 - first.1) as f64; + let dy = (second.2 - first.2) as f64; + dy.atan2(dx) + } else { + 0.0 + }; + + Some(Agent { + start: [first.1, first.2], + yaw, + goal: [last.1, last.2], + radius: self.radius, + speed: default_speed(), + spin: default_spin(), + }) + } + + pub fn interpolate(&self, time: f64, cell_size: f64) -> Position { + if self.trajectory.is_empty() { + return Position::identity(); + } + + if time <= self.trajectory[0].0 { + let (_, x, y) = self.trajectory[0]; + let p = Cell::new(x, y).center_point(cell_size); + return Position::translation(p.x, p.y); + } + + if time >= self.trajectory.last().unwrap().0 { + let (_, x, y) = *self.trajectory.last().unwrap(); + let p = Cell::new(x, y).center_point(cell_size); + return Position::translation(p.x, p.y); + } + + // Binary search for the interval + let result = self.trajectory.binary_search_by(|(t, _, _)| { + t.partial_cmp(&time).unwrap_or(std::cmp::Ordering::Equal) + }); + + let idx = match result { + Ok(i) => i, + Err(i) => i, + }; + + if idx == 0 { + let (_, x, y) = self.trajectory[0]; + let p = Cell::new(x, y).center_point(cell_size); + return Position::translation(p.x, p.y); + } + + let (t0, x0, y0) = self.trajectory[idx - 1]; + let (t1, x1, y1) = self.trajectory[idx]; + + let f = (time - t0) / (t1 - t0); + let p0 = Cell::new(x0, y0).center_point(cell_size); + let p1 = Cell::new(x1, y1).center_point(cell_size); + + let x = p0.x + f * (p1.x - p0.x); + let y = p0.y + f * (p1.y - p0.y); + let yaw = (p1.y - p0.y).atan2(p1.x - p0.x); + + Isometry2::new(Vector2::new(x, y), yaw) + } } -#[derive(Serialize, Deserialize)] +#[derive(Serialize, Deserialize, Clone)] pub struct Scenario { pub agents: BTreeMap, pub obstacles: Vec, @@ -109,6 +179,31 @@ pub struct Scenario { pub cell_size: f64, #[serde(skip_serializing_if = "Option::is_none", default)] pub camera_bounds: Option<[[f32; 2]; 2]>, + #[serde(skip_serializing)] + pub id_to_name: HashMap, +} + +impl Scenario { + pub fn add_mobile_objects_as_agents(&mut self) { + let mut last_id = 0; + for key in self.agents.keys() { + if let Ok(id) = key.parse::() { + last_id = last_id.max(id); + } + } + + if !self.agents.is_empty() { + last_id += 1; + } + + let obstacles = std::mem::take(&mut self.obstacles); + for obstacle in obstacles { + if let Some(agent) = obstacle.to_agent() { + self.agents.insert(last_id.to_string(), agent); + last_id += 1; + } + } + } } pub fn default_radius() -> f64 { @@ -134,3 +229,72 @@ pub fn bool_false() -> bool { pub fn is_false(b: &bool) -> bool { !b } + +#[cfg(test)] +mod tests { + use super::*; + use crate::negotiation::NegotiationNode; + use std::collections::HashMap; + use crate::domain::Cost; + + #[test] + fn test_derive_mapf_result_with_obstacles() { + let scenario = Scenario { + agents: BTreeMap::new(), + obstacles: vec![ + Obstacle { + trajectory: vec![ + (0.0, 0, 0), + (1.0, 1, 0), + ], + radius: 0.5, + indefinite_start: false, + indefinite_finish: false, + } + ], + occupancy: HashMap::new(), + cell_size: 1.0, + camera_bounds: None, + id_to_name: HashMap::new(), + }; + + // We need a mock solution node. + // Proposals can be empty if there are no agents. + let solution = NegotiationNode { + negotiation: crate::negotiation::Negotiation::default(), + proposals: HashMap::new(), + environment: crate::motion::CcbsEnvironment::new(std::sync::Arc::new( + crate::motion::DynamicEnvironment::new(crate::motion::CircularProfile::new(0.0, 0.0, 0.0).unwrap()) + )), + keys: std::collections::HashSet::new(), + conceded: None, + cost: Cost(0.0), + depth: 0, + outcome: crate::negotiation::NodeOutcome::Success, + id: 0, + parent: None, + }; + + let timestep = 0.5; + let mapf_result = scenario.derive_mapf_result(&solution, timestep); + + // One obstacle should result in one trajectory + assert_eq!(mapf_result.trajectories.len(), 1); + assert_eq!(mapf_result.footprints.len(), 1); + + // Obstacle trajectory from t=0 to t=1 with timestep 0.5 should have 3 poses (0.0, 0.5, 1.0) + assert_eq!(mapf_result.trajectories[0].poses.len(), 3); + + // Check first and last poses + let p0 = mapf_result.trajectories[0].poses[0].translation.vector; + let p2 = mapf_result.trajectories[0].poses[2].translation.vector; + + // Cell (0,0) center is (0.5, 0.5) + assert!((p0.x - 0.5).abs() < 1e-6); + assert!((p0.y - 0.5).abs() < 1e-6); + + // Cell (1,0) center is (1.5, 0.5) + assert!((p2.x - 1.5).abs() < 1e-6); + assert!((p2.y - 0.5).abs() < 1e-6); + } +} From 093e80748b3301f8824f75da59c9e4483ba80786 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 19 Mar 2026 03:23:32 +0000 Subject: [PATCH 04/11] Downgrade mapf to Rust 2021 for broader compatibility - Changed edition to 2021 in mapf/Cargo.toml - Refactored 'let chains' in post module to nested 'if let' blocks - Removed unused petgraph import - Updated formatting across multiple files to match 2021 edition Signed-off-by: Arjo Chakravarty --- mapf/Cargo.toml | 2 +- mapf/src/algorithm/a_star.rs | 2 +- mapf/src/algorithm/dijkstra/backward.rs | 8 +-- mapf/src/algorithm/dijkstra/forward.rs | 6 +- mapf/src/domain/closable/keyed_closed_set.rs | 2 +- .../closable/partial_keyed_closed_set.rs | 26 ++++--- .../closable/time_variant_keyed_closed_set.rs | 2 +- .../time_variant_partial_keyed_closed_set.rs | 4 +- mapf/src/domain/state_map.rs | 2 +- .../graph/occupancy/accessibility_graph.rs | 4 +- mapf/src/graph/occupancy/mod.rs | 6 +- mapf/src/graph/occupancy/sparse_grid.rs | 2 +- mapf/src/graph/occupancy/visibility_graph.rs | 4 +- mapf/src/motion/conflict.rs | 6 +- mapf/src/motion/environment.rs | 4 +- mapf/src/motion/r2/line_follow.rs | 14 ++-- mapf/src/motion/r2/space.rs | 2 +- mapf/src/motion/r2/timed_position.rs | 3 +- mapf/src/motion/safe_interval.rs | 5 +- .../se2/differential_drive_line_follow.rs | 15 ++--- mapf/src/motion/se2/quickest_path.rs | 8 +-- mapf/src/motion/se2/space.rs | 4 +- mapf/src/motion/se2/timed_position.rs | 3 +- mapf/src/motion/trajectory.rs | 2 +- mapf/src/motion/waypoint.rs | 2 +- mapf/src/negotiation/mod.rs | 24 ++++--- mapf/src/negotiation/scenario.rs | 35 +++++----- mapf/src/post/mod.rs | 67 +++++++++---------- mapf/src/post/spatial_allocation/mod.rs | 29 +++++--- mapf/src/premade/search_r2.rs | 4 +- mapf/src/premade/search_se2.rs | 4 +- mapf/src/premade/sipp_se2.rs | 6 +- 32 files changed, 158 insertions(+), 149 deletions(-) diff --git a/mapf/Cargo.toml b/mapf/Cargo.toml index 64586c0..04e3cdb 100644 --- a/mapf/Cargo.toml +++ b/mapf/Cargo.toml @@ -1,7 +1,7 @@ [package] name = "mapf" version = "0.3.0" -edition = "2024" +edition = "2021" description = "Base traits and utilities for multi-agent planning" license = "Apache-2.0" repository = "https://github.com/open-rmf/mapf" diff --git a/mapf/src/algorithm/a_star.rs b/mapf/src/algorithm/a_star.rs index db9c98b..79512e9 100644 --- a/mapf/src/algorithm/a_star.rs +++ b/mapf/src/algorithm/a_star.rs @@ -17,7 +17,7 @@ use crate::{ algorithm::{ - tree::*, Algorithm, Coherent, MinimumCostBound, Path, QueueLength, SearchStatus, Solvable, + Algorithm, Coherent, MinimumCostBound, Path, QueueLength, SearchStatus, Solvable, tree::*, }, domain::{ Activity, Closable, CloseResult, ClosedSet, Configurable, Connectable, Domain, Informed, diff --git a/mapf/src/algorithm/dijkstra/backward.rs b/mapf/src/algorithm/dijkstra/backward.rs index cc8b04d..13ff59a 100644 --- a/mapf/src/algorithm/dijkstra/backward.rs +++ b/mapf/src/algorithm/dijkstra/backward.rs @@ -17,11 +17,11 @@ use crate::{ algorithm::{ + Algorithm, Coherent, Path, SearchStatus, Solvable, dijkstra::{ - forward::{DijkstraSearchError, Memory}, Dijkstra, + forward::{DijkstraSearchError, Memory}, }, - Algorithm, Coherent, Path, SearchStatus, Solvable, }, domain::{ Activity, ArrivalKeyring, Backtrack, Closable, ClosedStatusForKey, Configurable, @@ -175,11 +175,11 @@ pub enum BackwardConfigurationError { mod tests { use super::*; use crate::{ + Planner, domain::KeyedCloser, graph::{SharedGraph, SimpleGraph}, - motion::{se2::*, TravelTimeCost}, + motion::{TravelTimeCost, se2::*}, templates::{GraphMotion, LazyGraphMotion, UninformedSearch}, - Planner, }; use std::sync::Arc; diff --git a/mapf/src/algorithm/dijkstra/forward.rs b/mapf/src/algorithm/dijkstra/forward.rs index ce6aeff..af427a7 100644 --- a/mapf/src/algorithm/dijkstra/forward.rs +++ b/mapf/src/algorithm/dijkstra/forward.rs @@ -16,7 +16,7 @@ */ use crate::{ - algorithm::{tree::*, Algorithm, Coherent, Path, SearchStatus, Solvable}, + algorithm::{Algorithm, Coherent, Path, SearchStatus, Solvable, tree::*}, domain::{ Activity, ArrivalKeyring, Closable, CloseResult, ClosedSet, ClosedStatus, ClosedStatusForKey, Configurable, Connectable, Domain, Initializable, Keyed, Keyring, @@ -26,7 +26,7 @@ use crate::{ }; use std::{ borrow::Borrow, - collections::{hash_map::Entry, HashMap}, + collections::{HashMap, hash_map::Entry}, ops::Add, sync::{Arc, Mutex, RwLock}, }; @@ -127,7 +127,7 @@ where Err(_) => { return Err(Self::algo_err( DijkstraImplError::PoisonedMutex, - )) + )); } }; diff --git a/mapf/src/domain/closable/keyed_closed_set.rs b/mapf/src/domain/closable/keyed_closed_set.rs index 32eb28c..cf8fe64 100644 --- a/mapf/src/domain/closable/keyed_closed_set.rs +++ b/mapf/src/domain/closable/keyed_closed_set.rs @@ -25,7 +25,7 @@ use crate::{ }; use std::{ borrow::Borrow, - collections::{hash_map::Entry, HashMap}, + collections::{HashMap, hash_map::Entry}, }; /// [`KeyedCloser`] implements the [`Closable`] trait by providing a diff --git a/mapf/src/domain/closable/partial_keyed_closed_set.rs b/mapf/src/domain/closable/partial_keyed_closed_set.rs index de74851..fa3b538 100644 --- a/mapf/src/domain/closable/partial_keyed_closed_set.rs +++ b/mapf/src/domain/closable/partial_keyed_closed_set.rs @@ -25,7 +25,7 @@ use crate::{ }; use std::{ borrow::Borrow, - collections::{hash_map::Entry, HashMap}, + collections::{HashMap, hash_map::Entry}, }; /// Factory for [`PartialKeyedClosedSet`]. Provide this to your domain, e.g. @@ -197,15 +197,21 @@ mod tests { #[test] fn test_partial_keyed_closed_set() { let mut closed_set = PartialKeyedClosedSet::new(SelfPartialKeyring::::new()); - assert!(closed_set - .close(&TestState::new(Some(1), 0.24), 0) - .accepted()); - assert!(closed_set - .status(&TestState::new(Some(1), 0.55)) - .is_closed()); - assert!(closed_set - .close(&TestState::new(Some(1), 0.1), 32) - .rejected()); + assert!( + closed_set + .close(&TestState::new(Some(1), 0.24), 0) + .accepted() + ); + assert!( + closed_set + .status(&TestState::new(Some(1), 0.55)) + .is_closed() + ); + assert!( + closed_set + .close(&TestState::new(Some(1), 0.1), 32) + .rejected() + ); assert!(closed_set.close(&TestState::new(None, 0.4), 0).accepted()); assert!(closed_set.status(&TestState::new(None, 123.4)).is_open()); 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 77e9593..cfc6cab 100644 --- a/mapf/src/domain/closable/time_variant_keyed_closed_set.rs +++ b/mapf/src/domain/closable/time_variant_keyed_closed_set.rs @@ -26,7 +26,7 @@ use crate::{ }; use std::{ borrow::Borrow, - collections::{hash_map::Entry, HashMap}, + collections::{HashMap, hash_map::Entry}, }; pub const DEFAULT_TIME_THRESH: i64 = 100_000_000; 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 6a7c39e..64cee34 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 @@ -17,7 +17,7 @@ use super::{ AsTimeInvariant, AsTimeVariant, Closable, CloseResult, ClosedSet, ClosedStatus, - ClosedStatusForKey, PartialKeyedCloser, DEFAULT_TIME_THRESH, + ClosedStatusForKey, DEFAULT_TIME_THRESH, PartialKeyedCloser, }; use crate::{ domain::{Keyed, Keyring, PartialKeyed, Reversible}, @@ -26,7 +26,7 @@ use crate::{ }; use std::{ borrow::Borrow, - collections::{hash_map::Entry, HashMap}, + collections::{HashMap, hash_map::Entry}, }; /// Factory for [`TimeVariantPartialKeyedClosedSet`]. Provide this to your domain, diff --git a/mapf/src/domain/state_map.rs b/mapf/src/domain/state_map.rs index 6eb68d7..4f2a139 100644 --- a/mapf/src/domain/state_map.rs +++ b/mapf/src/domain/state_map.rs @@ -27,7 +27,7 @@ pub trait ProjectState: StateSubspace { /// Project a state down to the target state space. fn project(&self, state: &State) - -> Result, Self::ProjectionError>; + -> Result, Self::ProjectionError>; } pub trait LiftState: StateSubspace { diff --git a/mapf/src/graph/occupancy/accessibility_graph.rs b/mapf/src/graph/occupancy/accessibility_graph.rs index e0aeac2..e9e0289 100644 --- a/mapf/src/graph/occupancy/accessibility_graph.rs +++ b/mapf/src/graph/occupancy/accessibility_graph.rs @@ -19,12 +19,12 @@ use crate::{ domain::Reversible, error::NoError, graph::{ - occupancy::{Cell, Grid}, Graph, + occupancy::{Cell, Grid}, }, motion::r2::Point, }; -use bitfield::{bitfield, Bit}; +use bitfield::{Bit, bitfield}; use std::{ collections::{HashMap, HashSet}, sync::Arc, diff --git a/mapf/src/graph/occupancy/mod.rs b/mapf/src/graph/occupancy/mod.rs index 23ca977..026ff93 100644 --- a/mapf/src/graph/occupancy/mod.rs +++ b/mapf/src/graph/occupancy/mod.rs @@ -19,11 +19,11 @@ use crate::{ motion::{MaybeTimed, TimePoint}, util::triangular_for, }; -use bitfield::{bitfield, Bit, BitMut}; +use bitfield::{Bit, BitMut, bitfield}; use std::{ collections::{ - hash_map::{Entry, Iter as HashMapIter}, HashMap, HashSet, + hash_map::{Entry, Iter as HashMapIter}, }, ops::Sub, }; @@ -274,7 +274,7 @@ pub trait Grid: std::fmt::Debug { /// Change the occupancy value of a set of cells. Get back any changes that /// have occurred to the corners of the occupancy. fn change_cells(&mut self, changes: &HashMap) - -> (ConfirmedChanges, ChangedCorners); + -> (ConfirmedChanges, ChangedCorners); /// Get the size (width and height) of a cell. fn cell_size(&self) -> f64; diff --git a/mapf/src/graph/occupancy/sparse_grid.rs b/mapf/src/graph/occupancy/sparse_grid.rs index 07a1f80..4cf2b04 100644 --- a/mapf/src/graph/occupancy/sparse_grid.rs +++ b/mapf/src/graph/occupancy/sparse_grid.rs @@ -17,7 +17,7 @@ use super::util::{LineSegment, SearchF64}; use super::{Cell, ChangedCorners, ConfirmedChanges, Corner, CornerStatus, Grid, Point, Vector}; -use std::collections::{btree_map, hash_map, hash_set, BTreeMap, BTreeSet, HashMap, HashSet}; +use std::collections::{BTreeMap, BTreeSet, HashMap, HashSet, btree_map, hash_map, hash_set}; #[derive(Debug, Clone)] pub struct SparseGrid { diff --git a/mapf/src/graph/occupancy/visibility_graph.rs b/mapf/src/graph/occupancy/visibility_graph.rs index de1a296..398039a 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, NeighborsIter, Point, Visibility, VisibleCells}, Edge, Graph, + occupancy::{Cell, Grid, NeighborsIter, Point, Visibility, VisibleCells}, }, util::triangular_for, }; use std::{ - collections::{hash_set::Iter as HashSetIter, HashMap, HashSet}, + collections::{HashMap, HashSet, hash_set::Iter as HashSetIter}, sync::Arc, }; diff --git a/mapf/src/motion/conflict.rs b/mapf/src/motion/conflict.rs index 66822ef..d51e9f1 100644 --- a/mapf/src/motion/conflict.rs +++ b/mapf/src/motion/conflict.rs @@ -18,11 +18,11 @@ use crate::{ error::ThisError, motion::{ - r2::{Point, Positioned, WaypointR2}, - se2::MaybeOriented, Arclength, BoundingBox, CircularProfile, Duration, DynamicCircularObstacle, Environment, IntegrateWaypoints, Interpolation, Measurable, Motion, TimePoint, Timed, Trajectory, Waypoint, + r2::{Point, Positioned, WaypointR2}, + se2::MaybeOriented, }, }; use arrayvec::ArrayVec; @@ -95,7 +95,7 @@ where None => { return SmallVec::from_iter([Err( SafeActionIntegrateWaypointError::MissingInitialWaypoint, - )]) + )]); } }; diff --git a/mapf/src/motion/environment.rs b/mapf/src/motion/environment.rs index 1f15c57..d292bf7 100644 --- a/mapf/src/motion/environment.rs +++ b/mapf/src/motion/environment.rs @@ -18,12 +18,12 @@ use crate::{ domain::Key, motion::{ - r2::{Point, WaypointR2}, Trajectory, Waypoint, + r2::{Point, WaypointR2}, }, }; use std::{ - collections::{hash_map::Entry, HashMap}, + collections::{HashMap, hash_map::Entry}, iter::Enumerate, slice::Iter as SliceIter, sync::Arc, diff --git a/mapf/src/motion/r2/line_follow.rs b/mapf/src/motion/r2/line_follow.rs index a2f070a..468eab7 100644 --- a/mapf/src/motion/r2/line_follow.rs +++ b/mapf/src/motion/r2/line_follow.rs @@ -18,22 +18,22 @@ use super::{Position, Positioned, WaypointR2}; use crate::{ domain::{ - backtrack_times, flip_endpoint_times, Backtrack, ConflictAvoider, ExtrapolationProgress, - Extrapolator, IncrementalExtrapolator, Key, Reversible, + Backtrack, ConflictAvoider, ExtrapolationProgress, Extrapolator, IncrementalExtrapolator, + Key, Reversible, backtrack_times, flip_endpoint_times, }, error::{NoError, ThisError}, graph::Graph, motion::{ - self, + self, Duration, SafeArrivalTimes, SafeIntervalCache, SafeIntervalMotionError, SpeedLimiter, conflict::{ - compute_safe_arrival_path, compute_safe_linear_path_wait_hints, SafeAction, - WaitForObstacle, + SafeAction, WaitForObstacle, compute_safe_arrival_path, + compute_safe_linear_path_wait_hints, }, - se2, Duration, SafeArrivalTimes, SafeIntervalCache, SafeIntervalMotionError, SpeedLimiter, + se2, }, }; use arrayvec::ArrayVec; -use smallvec::{smallvec, SmallVec}; +use smallvec::{SmallVec, smallvec}; #[derive(Debug, Clone, Copy, PartialEq)] pub struct LineFollow { diff --git a/mapf/src/motion/r2/space.rs b/mapf/src/motion/r2/space.rs index dbc6967..af643b0 100644 --- a/mapf/src/motion/r2/space.rs +++ b/mapf/src/motion/r2/space.rs @@ -20,9 +20,9 @@ use crate::{ error::{NoError, ThisError}, graph::Graph, motion::{ + IntegrateWaypoints, MaybeTimed, TimePoint, Timed, r2::*, se2::{MaybeOriented, Orientation, StateSE2}, - IntegrateWaypoints, MaybeTimed, TimePoint, Timed, }, }; use std::borrow::Borrow; diff --git a/mapf/src/motion/r2/timed_position.rs b/mapf/src/motion/r2/timed_position.rs index 7d400d9..9feec37 100644 --- a/mapf/src/motion/r2/timed_position.rs +++ b/mapf/src/motion/r2/timed_position.rs @@ -19,9 +19,8 @@ use super::{MaybePositioned, Position, Positioned, Velocity}; use crate::{ error::NoError, motion::{ - self, + self, IntegrateWaypoints, InterpError, Interpolation, MaybeTimed, TimePoint, Timed, se2::{MaybeOriented, WaypointSE2}, - IntegrateWaypoints, InterpError, Interpolation, MaybeTimed, TimePoint, Timed, }, }; use arrayvec::ArrayVec; diff --git a/mapf/src/motion/safe_interval.rs b/mapf/src/motion/safe_interval.rs index e0c4630..8f1aab3 100644 --- a/mapf/src/motion/safe_interval.rs +++ b/mapf/src/motion/safe_interval.rs @@ -20,17 +20,16 @@ use crate::{ error::ThisError, graph::Graph, motion::{ - compute_safe_arrival_times, + CcbsEnvironment, SafeArrivalTimes, TimePoint, Timed, compute_safe_arrival_times, r2::{Positioned, WaypointR2}, se2::WaypointSE2, - CcbsEnvironment, SafeArrivalTimes, TimePoint, Timed, }, util::Minimum, }; use smallvec::SmallVec; use std::{ borrow::Borrow, - collections::{hash_map::Entry, HashMap}, + collections::{HashMap, hash_map::Entry}, sync::{Arc, RwLock}, }; diff --git a/mapf/src/motion/se2/differential_drive_line_follow.rs b/mapf/src/motion/se2/differential_drive_line_follow.rs index 1d2524a..5122751 100644 --- a/mapf/src/motion/se2/differential_drive_line_follow.rs +++ b/mapf/src/motion/se2/differential_drive_line_follow.rs @@ -17,25 +17,24 @@ use crate::{ domain::{ - backtrack_times, flip_endpoint_times, Backtrack, ConflictAvoider, Connectable, - ExtrapolationProgress, Extrapolator, IncrementalExtrapolator, Key, Reversible, + Backtrack, ConflictAvoider, Connectable, ExtrapolationProgress, Extrapolator, + IncrementalExtrapolator, Key, Reversible, backtrack_times, flip_endpoint_times, }, error::{NoError, ThisError}, graph::Graph, motion::{ - self, + self, CcbsEnvironment, Duration, MaybeTimed, SafeArrivalTimes, SafeIntervalCache, + SafeIntervalMotionError, SpeedLimiter, Timed, conflict::{ - compute_safe_arrival_path, compute_safe_linear_path_wait_hints, is_safe_segment, - SafeAction, WaitForObstacle, + SafeAction, WaitForObstacle, compute_safe_arrival_path, + compute_safe_linear_path_wait_hints, is_safe_segment, }, r2::{MaybePositioned, Positioned, WaypointR2}, se2::{MaybeOriented, Orientation, Point, Position, StateSE2, WaypointSE2}, - CcbsEnvironment, Duration, MaybeTimed, SafeArrivalTimes, SafeIntervalCache, - SafeIntervalMotionError, SpeedLimiter, Timed, }, }; use arrayvec::ArrayVec; -use smallvec::{smallvec, SmallVec}; +use smallvec::{SmallVec, smallvec}; use std::{borrow::Borrow, sync::Arc}; use time_point::TimePoint; diff --git a/mapf/src/motion/se2/quickest_path.rs b/mapf/src/motion/se2/quickest_path.rs index ebf1331..eba7035 100644 --- a/mapf/src/motion/se2/quickest_path.rs +++ b/mapf/src/motion/se2/quickest_path.rs @@ -16,10 +16,12 @@ */ use crate::{ + Graph, Planner, algorithm::{BackwardDijkstra, Path}, domain::{Connectable, Extrapolator, Informed, Key, KeyedCloser, Reversible, Weighted}, error::{Anyhow, ThisError}, motion::{ + MaybeTimed, SpeedLimiter, TimePoint, Timed, r2::{ DiscreteSpaceTimeR2, InitializeR2, LineFollow, MaybePositioned, Positioned, StateR2, WaypointR2, @@ -28,11 +30,9 @@ use crate::{ DifferentialDriveLineFollow, DifferentialDriveLineFollowError, DifferentialDriveLineFollowMotion, KeySE2, MaybeOriented, MergeIntoGoal, StateSE2, }, - MaybeTimed, SpeedLimiter, TimePoint, Timed, }, planner::halt::StepLimit, templates::{GraphMotion, LazyGraphMotion, UninformedSearch}, - Graph, Planner, }; use arrayvec::ArrayVec; use std::{ @@ -317,12 +317,12 @@ mod tests { use super::*; use crate::{ graph::{ - occupancy::{Cell, SparseGrid, Visibility, VisibilityGraph}, SharedGraph, SimpleGraph, + occupancy::{Cell, SparseGrid, Visibility, VisibilityGraph}, }, motion::{ - se2::{GoalSE2, Point, WaypointSE2}, CircularProfile, TimePoint, TravelTimeCost, + se2::{GoalSE2, Point, WaypointSE2}, }, }; use approx::assert_relative_eq; diff --git a/mapf/src/motion/se2/space.rs b/mapf/src/motion/se2/space.rs index 115140c..18d26bd 100644 --- a/mapf/src/motion/se2/space.rs +++ b/mapf/src/motion/se2/space.rs @@ -23,11 +23,11 @@ use crate::{ error::{NoError, ThisError}, graph::{Edge, Graph}, motion::{ + DEFAULT_ROTATIONAL_THRESHOLD, IntegrateWaypoints, MaybeTimed, TimePoint, Timed, r2::{MaybePositioned, Point, Positioned}, se2::*, - IntegrateWaypoints, MaybeTimed, TimePoint, Timed, DEFAULT_ROTATIONAL_THRESHOLD, }, - util::{wrap_to_pi, ForkIter, IterError}, + util::{ForkIter, IterError, wrap_to_pi}, }; use smallvec::SmallVec; use std::borrow::Borrow; diff --git a/mapf/src/motion/se2/timed_position.rs b/mapf/src/motion/se2/timed_position.rs index 0a19c9d..b52c178 100644 --- a/mapf/src/motion/se2/timed_position.rs +++ b/mapf/src/motion/se2/timed_position.rs @@ -19,10 +19,9 @@ use super::{Position, Vector, Velocity}; use crate::{ error::NoError, motion::{ - self, + self, IntegrateWaypoints, InterpError, Interpolation, MaybeTimed, TimePoint, Timed, r2::{MaybePositioned, Positioned}, se2::{MaybeOriented, Orientation, Oriented}, - IntegrateWaypoints, InterpError, Interpolation, MaybeTimed, TimePoint, Timed, }, }; use arrayvec::ArrayVec; diff --git a/mapf/src/motion/trajectory.rs b/mapf/src/motion/trajectory.rs index 363e14f..6c67825 100644 --- a/mapf/src/motion/trajectory.rs +++ b/mapf/src/motion/trajectory.rs @@ -15,7 +15,7 @@ * */ -use super::{timed::TimeCmp, Duration, InterpError, Motion, TimePoint, Waypoint}; +use super::{Duration, InterpError, Motion, TimePoint, Waypoint, timed::TimeCmp}; use cached::{Cached, UnboundCache}; use sorted_vec::{FindOrInsert, SortedSet}; use std::cell::RefCell; diff --git a/mapf/src/motion/waypoint.rs b/mapf/src/motion/waypoint.rs index 69d712b..4f69ac8 100644 --- a/mapf/src/motion/waypoint.rs +++ b/mapf/src/motion/waypoint.rs @@ -15,7 +15,7 @@ * */ -use super::{r2::Positioned, se2::MaybeOriented, timed, Interpolation}; +use super::{Interpolation, r2::Positioned, se2::MaybeOriented, timed}; use arrayvec::ArrayVec; pub trait Waypoint: diff --git a/mapf/src/negotiation/mod.rs b/mapf/src/negotiation/mod.rs index 3f85c4e..044a988 100644 --- a/mapf/src/negotiation/mod.rs +++ b/mapf/src/negotiation/mod.rs @@ -20,21 +20,21 @@ pub use scenario::*; use crate::{ algorithm::{ - path::{DecisionRange, MetaTrajectory}, AStarConnect, QueueLength, SearchStatus, + path::{DecisionRange, MetaTrajectory}, }, domain::{ClosedStatus, Configurable, Cost}, error::ThisError, - graph::{occupancy::*, SharedGraph}, + graph::{SharedGraph, occupancy::*}, motion::{ + BoundingBox, CcbsConstraint, CcbsEnvironment, CircularProfile, Duration, + DynamicCircularObstacle, DynamicEnvironment, Motion, TimePoint, Timed, TravelEffortCost, have_conflict, r2::{Positioned, WaypointR2}, se2::{DifferentialDriveLineFollow, WaypointSE2}, trajectory::TrajectoryIter, - BoundingBox, CcbsConstraint, CcbsEnvironment, CircularProfile, Duration, - DynamicCircularObstacle, DynamicEnvironment, Motion, TimePoint, Timed, TravelEffortCost, }, - planner::{halt::QueueLengthLimit, Planner}, + planner::{Planner, halt::QueueLengthLimit}, premade::{SippSE2, StateSippSE2}, util::triangular_for, }; @@ -151,7 +151,7 @@ pub fn negotiate( Err(err) => { return Err(NegotiationError::PlanningImpossible( format!("{err:?}").to_owned(), - )) + )); } } .solve() @@ -683,8 +683,10 @@ impl Scenario { let mut footprints = Vec::new(); for (i, (_name, agent)) in self.agents.iter().enumerate() { - footprints.push(std::sync::Arc::new(crate::post::shape::Ball::new(agent.radius)) - as std::sync::Arc); + footprints.push( + std::sync::Arc::new(crate::post::shape::Ball::new(agent.radius)) + as std::sync::Arc, + ); let mut poses = Vec::new(); if let Some(proposal) = solution.proposals.get(&i) { @@ -709,8 +711,10 @@ impl Scenario { } for obstacle in &self.obstacles { - footprints.push(std::sync::Arc::new(crate::post::shape::Ball::new(obstacle.radius)) - as std::sync::Arc); + footprints.push( + std::sync::Arc::new(crate::post::shape::Ball::new(obstacle.radius)) + as std::sync::Arc, + ); let mut poses = Vec::new(); let steps = (max_finish_time.as_secs_f64() / timestep).ceil() as usize; diff --git a/mapf/src/negotiation/scenario.rs b/mapf/src/negotiation/scenario.rs index 904e81b..565006a 100644 --- a/mapf/src/negotiation/scenario.rs +++ b/mapf/src/negotiation/scenario.rs @@ -18,8 +18,8 @@ use crate::{ graph::occupancy::Cell, motion::{ - se2::{GoalSE2, Orientation, Position, StartSE2, WaypointSE2}, TimePoint, Trajectory, + se2::{GoalSE2, Orientation, Position, StartSE2, WaypointSE2}, }, }; use nalgebra::{Isometry2, Vector2}; @@ -233,38 +233,35 @@ pub fn is_false(b: &bool) -> bool { #[cfg(test)] mod tests { use super::*; + use crate::domain::Cost; use crate::negotiation::NegotiationNode; use std::collections::HashMap; - use crate::domain::Cost; #[test] fn test_derive_mapf_result_with_obstacles() { let scenario = Scenario { agents: BTreeMap::new(), - obstacles: vec![ - Obstacle { - trajectory: vec![ - (0.0, 0, 0), - (1.0, 1, 0), - ], - radius: 0.5, - indefinite_start: false, - indefinite_finish: false, - } - ], + obstacles: vec![Obstacle { + trajectory: vec![(0.0, 0, 0), (1.0, 1, 0)], + radius: 0.5, + indefinite_start: false, + indefinite_finish: false, + }], occupancy: HashMap::new(), cell_size: 1.0, camera_bounds: None, id_to_name: HashMap::new(), }; - // We need a mock solution node. + // We need a mock solution node. // Proposals can be empty if there are no agents. let solution = NegotiationNode { negotiation: crate::negotiation::Negotiation::default(), proposals: HashMap::new(), environment: crate::motion::CcbsEnvironment::new(std::sync::Arc::new( - crate::motion::DynamicEnvironment::new(crate::motion::CircularProfile::new(0.0, 0.0, 0.0).unwrap()) + crate::motion::DynamicEnvironment::new( + crate::motion::CircularProfile::new(0.0, 0.0, 0.0).unwrap(), + ), )), keys: std::collections::HashSet::new(), conceded: None, @@ -281,18 +278,18 @@ mod tests { // One obstacle should result in one trajectory assert_eq!(mapf_result.trajectories.len(), 1); assert_eq!(mapf_result.footprints.len(), 1); - + // Obstacle trajectory from t=0 to t=1 with timestep 0.5 should have 3 poses (0.0, 0.5, 1.0) assert_eq!(mapf_result.trajectories[0].poses.len(), 3); - + // Check first and last poses let p0 = mapf_result.trajectories[0].poses[0].translation.vector; let p2 = mapf_result.trajectories[0].poses[2].translation.vector; - + // Cell (0,0) center is (0.5, 0.5) assert!((p0.x - 0.5).abs() < 1e-6); assert!((p0.y - 0.5).abs() < 1e-6); - + // Cell (1,0) center is (1.5, 0.5) assert!((p2.x - 1.5).abs() < 1e-6); assert!((p2.y - 0.5).abs() < 1e-6); diff --git a/mapf/src/post/mod.rs b/mapf/src/post/mod.rs index a5e361a..b722f10 100644 --- a/mapf/src/post/mod.rs +++ b/mapf/src/post/mod.rs @@ -10,7 +10,6 @@ use parry2d::shape::Shape; pub use parry2d::na; pub use parry2d::shape; use petgraph::algo::toposort; -use petgraph::data::Build; use petgraph::graph::DiGraph; use petgraph::visit::EdgeRef; @@ -184,13 +183,13 @@ fn test_waypoint_follower() { assert!((next_wp.translation.x - example_trajectory.poses[2].translation.x).abs() < 0.01); let semantic_pose = follower.get_semantic_waypoint(); assert_eq!(semantic_pose.trajectory_index, 2); - } +} - #[cfg(test)] - #[test] - fn test_trajectory_conversion() { - use crate::motion::se2::WaypointSE2; +#[cfg(test)] +#[test] +fn test_trajectory_conversion() { use crate::motion::Trajectory as MapfTrajectory; + use crate::motion::se2::WaypointSE2; use time_point::TimePoint; let w1 = WaypointSE2::new(TimePoint::from_secs_f64(0.0), 0.0, 0.0, 0.0); @@ -201,9 +200,9 @@ fn test_waypoint_follower() { assert_eq!(post_traj.len(), 2); assert!((post_traj.poses[0].translation.x - 0.0).abs() < 1e-6); assert!((post_traj.poses[1].translation.x - 1.0).abs() < 1e-6); - } +} - /// Semantic waypoint: A point on the trajectory that needs to be passed. +/// Semantic waypoint: A point on the trajectory that needs to be passed. #[derive(Clone, Copy, Debug, Hash, PartialEq, Eq)] pub struct SemanticWaypoint { @@ -730,24 +729,24 @@ impl SemanticPlan { // Extract prime leader let ts = toposort(&sub_graph, None); let ts: Vec<_> = ts.unwrap().iter().map(|v| sub_graph[*v]).collect(); - if let Some(&leader) = ts.first() - && ts.len() > 1 - { - cluster_to_leader.insert(cluster_id, leader); - leader_to_cluster.insert(leader, cluster_id); - allocation_strategy.insert( - ts[0], - (AllocationStrategy::Leader(0.0, 0.0), 0, *cluster_id), - ); - for i in 1..ts.len() { + if let Some(&leader) = ts.first() { + if ts.len() > 1 { + cluster_to_leader.insert(cluster_id, leader); + leader_to_cluster.insert(leader, cluster_id); allocation_strategy.insert( - ts[i], - ( - AllocationStrategy::Follower(ts[i - 1].agent), - i, - *cluster_id, - ), + ts[0], + (AllocationStrategy::Leader(0.0, 0.0), 0, *cluster_id), ); + for i in 1..ts.len() { + allocation_strategy.insert( + ts[i], + ( + AllocationStrategy::Follower(ts[i - 1].agent), + i, + *cluster_id, + ), + ); + } } } } @@ -767,12 +766,12 @@ impl SemanticPlan { for leader in leaders { let mut hypothetical_leader = leader; hypothetical_leader.trajectory_index += 1; - if let Some(leader_segment) = leader_to_leader_segments.get(&hypothetical_leader) - && let Some(leaders) = leader_segment_to_leader.get_mut(leader_segment) - { - leaders.insert(leader); - leader_to_leader_segments.insert(leader, *leader_segment); - continue; + if let Some(leader_segment) = leader_to_leader_segments.get(&hypothetical_leader) { + if let Some(leaders) = leader_segment_to_leader.get_mut(leader_segment) { + leaders.insert(leader); + leader_to_leader_segments.insert(leader, *leader_segment); + continue; + } } leader_segment_to_leader.insert( last_lead_segment, @@ -904,10 +903,10 @@ impl SemanticPlan { continue; } - if let Some(other_agent) = agent_to_pos.get(&dep_wp.agent) - && other_agent.trajectory_index <= dep_wp.trajectory_index - { - return true; + if let Some(other_agent) = agent_to_pos.get(&dep_wp.agent) { + if other_agent.trajectory_index <= dep_wp.trajectory_index { + return true; + } } } } diff --git a/mapf/src/post/spatial_allocation/mod.rs b/mapf/src/post/spatial_allocation/mod.rs index fd6333d..405b2d9 100644 --- a/mapf/src/post/spatial_allocation/mod.rs +++ b/mapf/src/post/spatial_allocation/mod.rs @@ -55,7 +55,8 @@ impl Grid2D { positions: &[CurrentPosition], ) -> AllocationField { let semantic_plan = mapf_post(trajectories); - let semantic_positions: Vec = positions.iter().map(|p| p.semantic_position).collect(); + let semantic_positions: Vec = + positions.iter().map(|p| p.semantic_position).collect(); let assignment = semantic_plan.get_claim_dict(&semantic_positions); let mut allocation_field = AllocationField::create(semantic_plan, 1000, 1000); @@ -222,10 +223,11 @@ impl AllocationField { } fn update_spot(&mut self, alloc: &TrajectoryAllocation, x: usize, y: usize) { - if let Some(prev_alloc) = &self.grid_space[x][y] - && let Some(p) = self.cell_by_agent.get_mut(&prev_alloc.agent) { + if let Some(prev_alloc) = &self.grid_space[x][y] { + if let Some(p) = self.cell_by_agent.get_mut(&prev_alloc.agent) { p.remove(&(x, y)); } + } self.grid_space[x][y] = Some(alloc.clone()); if let Some(allocated_list) = self.cell_by_agent.get_mut(&alloc.agent) { allocated_list.insert((x, y)); @@ -271,9 +273,11 @@ impl AllocationField { .leader_follower .allocation_strategy .get(&previous_alloc.to_wp()) - && let Some((_mode2, priority2, cluster_id2)) = + { + if let Some((_mode2, priority2, cluster_id2)) = self.leader_follower.allocation_strategy.get(&alloc.to_wp()) - && cluster_id == cluster_id2 { + { + if cluster_id == cluster_id2 { if priority <= priority2 { self.grid_space[x][y] = Some(previous_alloc.clone()); return; @@ -282,6 +286,8 @@ impl AllocationField { return; } } + } + } // Logic for intersections if let Some(intersection_type) = self @@ -293,11 +299,12 @@ impl AllocationField { self.grid_space[x][y] = Some(alloc.clone()); return; } - } else if let IntersectionType::Next(leaders) = intersection_type - && leaders.contains(&alloc.to_wp()) { + } else if let IntersectionType::Next(leaders) = intersection_type { + if leaders.contains(&alloc.to_wp()) { self.update_spot(&previous_alloc, x, y); return; } + } } // Vicinity rule. if previous_alloc.dist_from_center < alloc.dist_from_center { @@ -316,11 +323,11 @@ impl AllocationField { } let x = x as usize; let y = y as usize; - + if x >= self.width() || y >= self.height() { return None; } - + let Some(p) = self.grid_space[x][y].clone() else { return None; }; @@ -339,11 +346,11 @@ impl AllocationField { } let x = x as usize; let y = y as usize; - + if x >= self.width() || y >= self.height() { return None; } - + let Some(p) = self.grid_space[x][y].clone() else { return None; }; diff --git a/mapf/src/premade/search_r2.rs b/mapf/src/premade/search_r2.rs index 3e79269..b66bb18 100644 --- a/mapf/src/premade/search_r2.rs +++ b/mapf/src/premade/search_r2.rs @@ -18,7 +18,7 @@ use crate::{ domain::{Key, KeyedCloser, Reversible}, graph::{Graph, SharedGraph}, - motion::{r2::*, SpeedLimiter, TravelTimeCost}, + motion::{SpeedLimiter, TravelTimeCost, r2::*}, templates::{GraphMotion, InformedSearch}, }; @@ -67,7 +67,7 @@ where mod tests { use super::*; use crate::{ - algorithm::AStar, domain::AsTimeVariant, graph::SimpleGraph, motion::SpeedLimit, Planner, + Planner, algorithm::AStar, domain::AsTimeVariant, graph::SimpleGraph, motion::SpeedLimit, }; use std::sync::Arc; diff --git a/mapf/src/premade/search_se2.rs b/mapf/src/premade/search_se2.rs index 913f95f..22aa2ab 100644 --- a/mapf/src/premade/search_se2.rs +++ b/mapf/src/premade/search_se2.rs @@ -20,9 +20,9 @@ use crate::{ error::Anyhow, graph::{Graph, SharedGraph}, motion::{ + SpeedLimiter, TravelTimeCost, r2::{DirectTravelHeuristic, DiscreteSpaceTimeR2, Positioned, StateR2}, se2::*, - SpeedLimiter, TravelTimeCost, }, templates::{GraphMotion, InformedSearch}, }; @@ -90,7 +90,7 @@ where #[cfg(test)] mod tests { use super::*; - use crate::{algorithm::AStarConnect, graph::SimpleGraph, motion::SpeedLimit, Planner}; + use crate::{Planner, algorithm::AStarConnect, graph::SimpleGraph, motion::SpeedLimit}; use std::sync::Arc; #[test] diff --git a/mapf/src/premade/sipp_se2.rs b/mapf/src/premade/sipp_se2.rs index ff57ea7..d9c29f5 100644 --- a/mapf/src/premade/sipp_se2.rs +++ b/mapf/src/premade/sipp_se2.rs @@ -20,9 +20,9 @@ use crate::{ error::{Anyhow, StdError}, graph::{Graph, SharedGraph}, motion::{ + CcbsEnvironment, SafeIntervalCache, SafeIntervalCloser, SpeedLimiter, TravelEffortCost, r2::Positioned, se2::{quickest_path::QuickestPathSearch, *}, - CcbsEnvironment, SafeIntervalCache, SafeIntervalCloser, SpeedLimiter, TravelEffortCost, }, templates::{ConflictAvoidance, GraphMotion, InformedSearch, LazyGraphMotion}, }; @@ -393,15 +393,15 @@ where mod tests { use super::*; use crate::{ + Planner, algorithm::AStarConnect, graph::{ - occupancy::{Cell, NeighborhoodGraph, SparseGrid, Visibility, VisibilityGraph}, SimpleGraph, + occupancy::{Cell, NeighborhoodGraph, SparseGrid, Visibility, VisibilityGraph}, }, motion::{ CircularProfile, DynamicCircularObstacle, DynamicEnvironment, TimePoint, Trajectory, }, - Planner, }; use approx::assert_relative_eq; use std::sync::Arc; From 6a192f6e0483530a2bc80349148ee57bafeb4350 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 19 Mar 2026 04:31:47 +0000 Subject: [PATCH 05/11] Fix formatting Signed-off-by: Arjo Chakravarty --- mapf/src/algorithm/a_star.rs | 2 +- mapf/src/algorithm/dijkstra/backward.rs | 8 +++--- mapf/src/algorithm/dijkstra/forward.rs | 4 +-- mapf/src/domain/closable/keyed_closed_set.rs | 2 +- .../closable/partial_keyed_closed_set.rs | 26 +++++++------------ .../closable/time_variant_keyed_closed_set.rs | 2 +- .../time_variant_partial_keyed_closed_set.rs | 4 +-- mapf/src/domain/state_map.rs | 2 +- .../graph/occupancy/accessibility_graph.rs | 4 +-- mapf/src/graph/occupancy/mod.rs | 6 ++--- mapf/src/graph/occupancy/sparse_grid.rs | 2 +- mapf/src/graph/occupancy/visibility_graph.rs | 4 +-- mapf/src/motion/conflict.rs | 4 +-- mapf/src/motion/environment.rs | 4 +-- mapf/src/motion/r2/line_follow.rs | 14 +++++----- mapf/src/motion/r2/space.rs | 2 +- mapf/src/motion/r2/timed_position.rs | 3 ++- mapf/src/motion/safe_interval.rs | 5 ++-- .../se2/differential_drive_line_follow.rs | 15 ++++++----- mapf/src/motion/se2/quickest_path.rs | 8 +++--- mapf/src/motion/se2/space.rs | 4 +-- mapf/src/motion/se2/timed_position.rs | 3 ++- mapf/src/motion/trajectory.rs | 2 +- mapf/src/motion/waypoint.rs | 2 +- mapf/src/negotiation/mod.rs | 10 +++---- mapf/src/negotiation/scenario.rs | 2 +- mapf/src/post/mod.rs | 2 +- mapf/src/post/spatial_allocation/mod.rs | 2 +- mapf/src/premade/search_r2.rs | 4 +-- mapf/src/premade/search_se2.rs | 4 +-- mapf/src/premade/sipp_se2.rs | 6 ++--- 31 files changed, 80 insertions(+), 82 deletions(-) diff --git a/mapf/src/algorithm/a_star.rs b/mapf/src/algorithm/a_star.rs index 79512e9..db9c98b 100644 --- a/mapf/src/algorithm/a_star.rs +++ b/mapf/src/algorithm/a_star.rs @@ -17,7 +17,7 @@ use crate::{ algorithm::{ - Algorithm, Coherent, MinimumCostBound, Path, QueueLength, SearchStatus, Solvable, tree::*, + tree::*, Algorithm, Coherent, MinimumCostBound, Path, QueueLength, SearchStatus, Solvable, }, domain::{ Activity, Closable, CloseResult, ClosedSet, Configurable, Connectable, Domain, Informed, diff --git a/mapf/src/algorithm/dijkstra/backward.rs b/mapf/src/algorithm/dijkstra/backward.rs index 13ff59a..cc8b04d 100644 --- a/mapf/src/algorithm/dijkstra/backward.rs +++ b/mapf/src/algorithm/dijkstra/backward.rs @@ -17,11 +17,11 @@ use crate::{ algorithm::{ - Algorithm, Coherent, Path, SearchStatus, Solvable, dijkstra::{ - Dijkstra, forward::{DijkstraSearchError, Memory}, + Dijkstra, }, + Algorithm, Coherent, Path, SearchStatus, Solvable, }, domain::{ Activity, ArrivalKeyring, Backtrack, Closable, ClosedStatusForKey, Configurable, @@ -175,11 +175,11 @@ pub enum BackwardConfigurationError { mod tests { use super::*; use crate::{ - Planner, domain::KeyedCloser, graph::{SharedGraph, SimpleGraph}, - motion::{TravelTimeCost, se2::*}, + motion::{se2::*, TravelTimeCost}, templates::{GraphMotion, LazyGraphMotion, UninformedSearch}, + Planner, }; use std::sync::Arc; diff --git a/mapf/src/algorithm/dijkstra/forward.rs b/mapf/src/algorithm/dijkstra/forward.rs index af427a7..53e49d2 100644 --- a/mapf/src/algorithm/dijkstra/forward.rs +++ b/mapf/src/algorithm/dijkstra/forward.rs @@ -16,7 +16,7 @@ */ use crate::{ - algorithm::{Algorithm, Coherent, Path, SearchStatus, Solvable, tree::*}, + algorithm::{tree::*, Algorithm, Coherent, Path, SearchStatus, Solvable}, domain::{ Activity, ArrivalKeyring, Closable, CloseResult, ClosedSet, ClosedStatus, ClosedStatusForKey, Configurable, Connectable, Domain, Initializable, Keyed, Keyring, @@ -26,7 +26,7 @@ use crate::{ }; use std::{ borrow::Borrow, - collections::{HashMap, hash_map::Entry}, + collections::{hash_map::Entry, HashMap}, ops::Add, sync::{Arc, Mutex, RwLock}, }; diff --git a/mapf/src/domain/closable/keyed_closed_set.rs b/mapf/src/domain/closable/keyed_closed_set.rs index cf8fe64..32eb28c 100644 --- a/mapf/src/domain/closable/keyed_closed_set.rs +++ b/mapf/src/domain/closable/keyed_closed_set.rs @@ -25,7 +25,7 @@ use crate::{ }; use std::{ borrow::Borrow, - collections::{HashMap, hash_map::Entry}, + collections::{hash_map::Entry, HashMap}, }; /// [`KeyedCloser`] implements the [`Closable`] trait by providing a diff --git a/mapf/src/domain/closable/partial_keyed_closed_set.rs b/mapf/src/domain/closable/partial_keyed_closed_set.rs index fa3b538..de74851 100644 --- a/mapf/src/domain/closable/partial_keyed_closed_set.rs +++ b/mapf/src/domain/closable/partial_keyed_closed_set.rs @@ -25,7 +25,7 @@ use crate::{ }; use std::{ borrow::Borrow, - collections::{HashMap, hash_map::Entry}, + collections::{hash_map::Entry, HashMap}, }; /// Factory for [`PartialKeyedClosedSet`]. Provide this to your domain, e.g. @@ -197,21 +197,15 @@ mod tests { #[test] fn test_partial_keyed_closed_set() { let mut closed_set = PartialKeyedClosedSet::new(SelfPartialKeyring::::new()); - assert!( - closed_set - .close(&TestState::new(Some(1), 0.24), 0) - .accepted() - ); - assert!( - closed_set - .status(&TestState::new(Some(1), 0.55)) - .is_closed() - ); - assert!( - closed_set - .close(&TestState::new(Some(1), 0.1), 32) - .rejected() - ); + assert!(closed_set + .close(&TestState::new(Some(1), 0.24), 0) + .accepted()); + assert!(closed_set + .status(&TestState::new(Some(1), 0.55)) + .is_closed()); + assert!(closed_set + .close(&TestState::new(Some(1), 0.1), 32) + .rejected()); assert!(closed_set.close(&TestState::new(None, 0.4), 0).accepted()); assert!(closed_set.status(&TestState::new(None, 123.4)).is_open()); 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 cfc6cab..77e9593 100644 --- a/mapf/src/domain/closable/time_variant_keyed_closed_set.rs +++ b/mapf/src/domain/closable/time_variant_keyed_closed_set.rs @@ -26,7 +26,7 @@ use crate::{ }; use std::{ borrow::Borrow, - collections::{HashMap, hash_map::Entry}, + collections::{hash_map::Entry, HashMap}, }; pub const DEFAULT_TIME_THRESH: i64 = 100_000_000; 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 64cee34..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 @@ -17,7 +17,7 @@ use super::{ AsTimeInvariant, AsTimeVariant, Closable, CloseResult, ClosedSet, ClosedStatus, - ClosedStatusForKey, DEFAULT_TIME_THRESH, PartialKeyedCloser, + ClosedStatusForKey, PartialKeyedCloser, DEFAULT_TIME_THRESH, }; use crate::{ domain::{Keyed, Keyring, PartialKeyed, Reversible}, @@ -26,7 +26,7 @@ use crate::{ }; use std::{ borrow::Borrow, - collections::{HashMap, hash_map::Entry}, + collections::{hash_map::Entry, HashMap}, }; /// Factory for [`TimeVariantPartialKeyedClosedSet`]. Provide this to your domain, diff --git a/mapf/src/domain/state_map.rs b/mapf/src/domain/state_map.rs index 4f2a139..6eb68d7 100644 --- a/mapf/src/domain/state_map.rs +++ b/mapf/src/domain/state_map.rs @@ -27,7 +27,7 @@ pub trait ProjectState: StateSubspace { /// Project a state down to the target state space. fn project(&self, state: &State) - -> Result, Self::ProjectionError>; + -> Result, Self::ProjectionError>; } pub trait LiftState: StateSubspace { diff --git a/mapf/src/graph/occupancy/accessibility_graph.rs b/mapf/src/graph/occupancy/accessibility_graph.rs index e9e0289..e0aeac2 100644 --- a/mapf/src/graph/occupancy/accessibility_graph.rs +++ b/mapf/src/graph/occupancy/accessibility_graph.rs @@ -19,12 +19,12 @@ use crate::{ domain::Reversible, error::NoError, graph::{ - Graph, occupancy::{Cell, Grid}, + Graph, }, motion::r2::Point, }; -use bitfield::{Bit, bitfield}; +use bitfield::{bitfield, Bit}; use std::{ collections::{HashMap, HashSet}, sync::Arc, diff --git a/mapf/src/graph/occupancy/mod.rs b/mapf/src/graph/occupancy/mod.rs index 026ff93..23ca977 100644 --- a/mapf/src/graph/occupancy/mod.rs +++ b/mapf/src/graph/occupancy/mod.rs @@ -19,11 +19,11 @@ use crate::{ motion::{MaybeTimed, TimePoint}, util::triangular_for, }; -use bitfield::{Bit, BitMut, bitfield}; +use bitfield::{bitfield, Bit, BitMut}; use std::{ collections::{ - HashMap, HashSet, hash_map::{Entry, Iter as HashMapIter}, + HashMap, HashSet, }, ops::Sub, }; @@ -274,7 +274,7 @@ pub trait Grid: std::fmt::Debug { /// Change the occupancy value of a set of cells. Get back any changes that /// have occurred to the corners of the occupancy. fn change_cells(&mut self, changes: &HashMap) - -> (ConfirmedChanges, ChangedCorners); + -> (ConfirmedChanges, ChangedCorners); /// Get the size (width and height) of a cell. fn cell_size(&self) -> f64; diff --git a/mapf/src/graph/occupancy/sparse_grid.rs b/mapf/src/graph/occupancy/sparse_grid.rs index 4cf2b04..07a1f80 100644 --- a/mapf/src/graph/occupancy/sparse_grid.rs +++ b/mapf/src/graph/occupancy/sparse_grid.rs @@ -17,7 +17,7 @@ use super::util::{LineSegment, SearchF64}; use super::{Cell, ChangedCorners, ConfirmedChanges, Corner, CornerStatus, Grid, Point, Vector}; -use std::collections::{BTreeMap, BTreeSet, HashMap, HashSet, btree_map, hash_map, hash_set}; +use std::collections::{btree_map, hash_map, hash_set, BTreeMap, BTreeSet, HashMap, HashSet}; #[derive(Debug, Clone)] pub struct SparseGrid { diff --git a/mapf/src/graph/occupancy/visibility_graph.rs b/mapf/src/graph/occupancy/visibility_graph.rs index 398039a..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::{ - Edge, Graph, occupancy::{Cell, Grid, NeighborsIter, Point, Visibility, VisibleCells}, + Edge, Graph, }, util::triangular_for, }; use std::{ - collections::{HashMap, HashSet, hash_set::Iter as HashSetIter}, + collections::{hash_set::Iter as HashSetIter, HashMap, HashSet}, sync::Arc, }; diff --git a/mapf/src/motion/conflict.rs b/mapf/src/motion/conflict.rs index d51e9f1..1876684 100644 --- a/mapf/src/motion/conflict.rs +++ b/mapf/src/motion/conflict.rs @@ -18,11 +18,11 @@ use crate::{ error::ThisError, motion::{ + r2::{Point, Positioned, WaypointR2}, + se2::MaybeOriented, Arclength, BoundingBox, CircularProfile, Duration, DynamicCircularObstacle, Environment, IntegrateWaypoints, Interpolation, Measurable, Motion, TimePoint, Timed, Trajectory, Waypoint, - r2::{Point, Positioned, WaypointR2}, - se2::MaybeOriented, }, }; use arrayvec::ArrayVec; diff --git a/mapf/src/motion/environment.rs b/mapf/src/motion/environment.rs index d292bf7..1f15c57 100644 --- a/mapf/src/motion/environment.rs +++ b/mapf/src/motion/environment.rs @@ -18,12 +18,12 @@ use crate::{ domain::Key, motion::{ - Trajectory, Waypoint, r2::{Point, WaypointR2}, + Trajectory, Waypoint, }, }; use std::{ - collections::{HashMap, hash_map::Entry}, + collections::{hash_map::Entry, HashMap}, iter::Enumerate, slice::Iter as SliceIter, sync::Arc, diff --git a/mapf/src/motion/r2/line_follow.rs b/mapf/src/motion/r2/line_follow.rs index 468eab7..a2f070a 100644 --- a/mapf/src/motion/r2/line_follow.rs +++ b/mapf/src/motion/r2/line_follow.rs @@ -18,22 +18,22 @@ use super::{Position, Positioned, WaypointR2}; use crate::{ domain::{ - Backtrack, ConflictAvoider, ExtrapolationProgress, Extrapolator, IncrementalExtrapolator, - Key, Reversible, backtrack_times, flip_endpoint_times, + backtrack_times, flip_endpoint_times, Backtrack, ConflictAvoider, ExtrapolationProgress, + Extrapolator, IncrementalExtrapolator, Key, Reversible, }, error::{NoError, ThisError}, graph::Graph, motion::{ - self, Duration, SafeArrivalTimes, SafeIntervalCache, SafeIntervalMotionError, SpeedLimiter, + self, conflict::{ - SafeAction, WaitForObstacle, compute_safe_arrival_path, - compute_safe_linear_path_wait_hints, + compute_safe_arrival_path, compute_safe_linear_path_wait_hints, SafeAction, + WaitForObstacle, }, - se2, + se2, Duration, SafeArrivalTimes, SafeIntervalCache, SafeIntervalMotionError, SpeedLimiter, }, }; use arrayvec::ArrayVec; -use smallvec::{SmallVec, smallvec}; +use smallvec::{smallvec, SmallVec}; #[derive(Debug, Clone, Copy, PartialEq)] pub struct LineFollow { diff --git a/mapf/src/motion/r2/space.rs b/mapf/src/motion/r2/space.rs index af643b0..dbc6967 100644 --- a/mapf/src/motion/r2/space.rs +++ b/mapf/src/motion/r2/space.rs @@ -20,9 +20,9 @@ use crate::{ error::{NoError, ThisError}, graph::Graph, motion::{ - IntegrateWaypoints, MaybeTimed, TimePoint, Timed, r2::*, se2::{MaybeOriented, Orientation, StateSE2}, + IntegrateWaypoints, MaybeTimed, TimePoint, Timed, }, }; use std::borrow::Borrow; diff --git a/mapf/src/motion/r2/timed_position.rs b/mapf/src/motion/r2/timed_position.rs index 9feec37..7d400d9 100644 --- a/mapf/src/motion/r2/timed_position.rs +++ b/mapf/src/motion/r2/timed_position.rs @@ -19,8 +19,9 @@ use super::{MaybePositioned, Position, Positioned, Velocity}; use crate::{ error::NoError, motion::{ - self, IntegrateWaypoints, InterpError, Interpolation, MaybeTimed, TimePoint, Timed, + self, se2::{MaybeOriented, WaypointSE2}, + IntegrateWaypoints, InterpError, Interpolation, MaybeTimed, TimePoint, Timed, }, }; use arrayvec::ArrayVec; diff --git a/mapf/src/motion/safe_interval.rs b/mapf/src/motion/safe_interval.rs index 8f1aab3..e0c4630 100644 --- a/mapf/src/motion/safe_interval.rs +++ b/mapf/src/motion/safe_interval.rs @@ -20,16 +20,17 @@ use crate::{ error::ThisError, graph::Graph, motion::{ - CcbsEnvironment, SafeArrivalTimes, TimePoint, Timed, compute_safe_arrival_times, + compute_safe_arrival_times, r2::{Positioned, WaypointR2}, se2::WaypointSE2, + CcbsEnvironment, SafeArrivalTimes, TimePoint, Timed, }, util::Minimum, }; use smallvec::SmallVec; use std::{ borrow::Borrow, - collections::{HashMap, hash_map::Entry}, + collections::{hash_map::Entry, HashMap}, sync::{Arc, RwLock}, }; diff --git a/mapf/src/motion/se2/differential_drive_line_follow.rs b/mapf/src/motion/se2/differential_drive_line_follow.rs index 5122751..1d2524a 100644 --- a/mapf/src/motion/se2/differential_drive_line_follow.rs +++ b/mapf/src/motion/se2/differential_drive_line_follow.rs @@ -17,24 +17,25 @@ use crate::{ domain::{ - Backtrack, ConflictAvoider, Connectable, ExtrapolationProgress, Extrapolator, - IncrementalExtrapolator, Key, Reversible, backtrack_times, flip_endpoint_times, + backtrack_times, flip_endpoint_times, Backtrack, ConflictAvoider, Connectable, + ExtrapolationProgress, Extrapolator, IncrementalExtrapolator, Key, Reversible, }, error::{NoError, ThisError}, graph::Graph, motion::{ - self, CcbsEnvironment, Duration, MaybeTimed, SafeArrivalTimes, SafeIntervalCache, - SafeIntervalMotionError, SpeedLimiter, Timed, + self, conflict::{ - SafeAction, WaitForObstacle, compute_safe_arrival_path, - compute_safe_linear_path_wait_hints, is_safe_segment, + compute_safe_arrival_path, compute_safe_linear_path_wait_hints, is_safe_segment, + SafeAction, WaitForObstacle, }, r2::{MaybePositioned, Positioned, WaypointR2}, se2::{MaybeOriented, Orientation, Point, Position, StateSE2, WaypointSE2}, + CcbsEnvironment, Duration, MaybeTimed, SafeArrivalTimes, SafeIntervalCache, + SafeIntervalMotionError, SpeedLimiter, Timed, }, }; use arrayvec::ArrayVec; -use smallvec::{SmallVec, smallvec}; +use smallvec::{smallvec, SmallVec}; use std::{borrow::Borrow, sync::Arc}; use time_point::TimePoint; diff --git a/mapf/src/motion/se2/quickest_path.rs b/mapf/src/motion/se2/quickest_path.rs index eba7035..ebf1331 100644 --- a/mapf/src/motion/se2/quickest_path.rs +++ b/mapf/src/motion/se2/quickest_path.rs @@ -16,12 +16,10 @@ */ use crate::{ - Graph, Planner, algorithm::{BackwardDijkstra, Path}, domain::{Connectable, Extrapolator, Informed, Key, KeyedCloser, Reversible, Weighted}, error::{Anyhow, ThisError}, motion::{ - MaybeTimed, SpeedLimiter, TimePoint, Timed, r2::{ DiscreteSpaceTimeR2, InitializeR2, LineFollow, MaybePositioned, Positioned, StateR2, WaypointR2, @@ -30,9 +28,11 @@ use crate::{ DifferentialDriveLineFollow, DifferentialDriveLineFollowError, DifferentialDriveLineFollowMotion, KeySE2, MaybeOriented, MergeIntoGoal, StateSE2, }, + MaybeTimed, SpeedLimiter, TimePoint, Timed, }, planner::halt::StepLimit, templates::{GraphMotion, LazyGraphMotion, UninformedSearch}, + Graph, Planner, }; use arrayvec::ArrayVec; use std::{ @@ -317,12 +317,12 @@ mod tests { use super::*; use crate::{ graph::{ - SharedGraph, SimpleGraph, occupancy::{Cell, SparseGrid, Visibility, VisibilityGraph}, + SharedGraph, SimpleGraph, }, motion::{ - CircularProfile, TimePoint, TravelTimeCost, se2::{GoalSE2, Point, WaypointSE2}, + CircularProfile, TimePoint, TravelTimeCost, }, }; use approx::assert_relative_eq; diff --git a/mapf/src/motion/se2/space.rs b/mapf/src/motion/se2/space.rs index 18d26bd..115140c 100644 --- a/mapf/src/motion/se2/space.rs +++ b/mapf/src/motion/se2/space.rs @@ -23,11 +23,11 @@ use crate::{ error::{NoError, ThisError}, graph::{Edge, Graph}, motion::{ - DEFAULT_ROTATIONAL_THRESHOLD, IntegrateWaypoints, MaybeTimed, TimePoint, Timed, r2::{MaybePositioned, Point, Positioned}, se2::*, + IntegrateWaypoints, MaybeTimed, TimePoint, Timed, DEFAULT_ROTATIONAL_THRESHOLD, }, - util::{ForkIter, IterError, wrap_to_pi}, + util::{wrap_to_pi, ForkIter, IterError}, }; use smallvec::SmallVec; use std::borrow::Borrow; diff --git a/mapf/src/motion/se2/timed_position.rs b/mapf/src/motion/se2/timed_position.rs index b52c178..0a19c9d 100644 --- a/mapf/src/motion/se2/timed_position.rs +++ b/mapf/src/motion/se2/timed_position.rs @@ -19,9 +19,10 @@ use super::{Position, Vector, Velocity}; use crate::{ error::NoError, motion::{ - self, IntegrateWaypoints, InterpError, Interpolation, MaybeTimed, TimePoint, Timed, + self, r2::{MaybePositioned, Positioned}, se2::{MaybeOriented, Orientation, Oriented}, + IntegrateWaypoints, InterpError, Interpolation, MaybeTimed, TimePoint, Timed, }, }; use arrayvec::ArrayVec; diff --git a/mapf/src/motion/trajectory.rs b/mapf/src/motion/trajectory.rs index 6c67825..363e14f 100644 --- a/mapf/src/motion/trajectory.rs +++ b/mapf/src/motion/trajectory.rs @@ -15,7 +15,7 @@ * */ -use super::{Duration, InterpError, Motion, TimePoint, Waypoint, timed::TimeCmp}; +use super::{timed::TimeCmp, Duration, InterpError, Motion, TimePoint, Waypoint}; use cached::{Cached, UnboundCache}; use sorted_vec::{FindOrInsert, SortedSet}; use std::cell::RefCell; diff --git a/mapf/src/motion/waypoint.rs b/mapf/src/motion/waypoint.rs index 4f69ac8..69d712b 100644 --- a/mapf/src/motion/waypoint.rs +++ b/mapf/src/motion/waypoint.rs @@ -15,7 +15,7 @@ * */ -use super::{Interpolation, r2::Positioned, se2::MaybeOriented, timed}; +use super::{r2::Positioned, se2::MaybeOriented, timed, Interpolation}; use arrayvec::ArrayVec; pub trait Waypoint: diff --git a/mapf/src/negotiation/mod.rs b/mapf/src/negotiation/mod.rs index 044a988..0d4e1cd 100644 --- a/mapf/src/negotiation/mod.rs +++ b/mapf/src/negotiation/mod.rs @@ -20,21 +20,21 @@ pub use scenario::*; use crate::{ algorithm::{ - AStarConnect, QueueLength, SearchStatus, path::{DecisionRange, MetaTrajectory}, + AStarConnect, QueueLength, SearchStatus, }, domain::{ClosedStatus, Configurable, Cost}, error::ThisError, - graph::{SharedGraph, occupancy::*}, + graph::{occupancy::*, SharedGraph}, motion::{ - BoundingBox, CcbsConstraint, CcbsEnvironment, CircularProfile, Duration, - DynamicCircularObstacle, DynamicEnvironment, Motion, TimePoint, Timed, TravelEffortCost, have_conflict, r2::{Positioned, WaypointR2}, se2::{DifferentialDriveLineFollow, WaypointSE2}, trajectory::TrajectoryIter, + BoundingBox, CcbsConstraint, CcbsEnvironment, CircularProfile, Duration, + DynamicCircularObstacle, DynamicEnvironment, Motion, TimePoint, Timed, TravelEffortCost, }, - planner::{Planner, halt::QueueLengthLimit}, + planner::{halt::QueueLengthLimit, Planner}, premade::{SippSE2, StateSippSE2}, util::triangular_for, }; diff --git a/mapf/src/negotiation/scenario.rs b/mapf/src/negotiation/scenario.rs index 565006a..3dc7882 100644 --- a/mapf/src/negotiation/scenario.rs +++ b/mapf/src/negotiation/scenario.rs @@ -18,8 +18,8 @@ use crate::{ graph::occupancy::Cell, motion::{ - TimePoint, Trajectory, se2::{GoalSE2, Orientation, Position, StartSE2, WaypointSE2}, + TimePoint, Trajectory, }, }; use nalgebra::{Isometry2, Vector2}; diff --git a/mapf/src/post/mod.rs b/mapf/src/post/mod.rs index b722f10..f332915 100644 --- a/mapf/src/post/mod.rs +++ b/mapf/src/post/mod.rs @@ -188,8 +188,8 @@ fn test_waypoint_follower() { #[cfg(test)] #[test] fn test_trajectory_conversion() { - use crate::motion::Trajectory as MapfTrajectory; use crate::motion::se2::WaypointSE2; + use crate::motion::Trajectory as MapfTrajectory; use time_point::TimePoint; let w1 = WaypointSE2::new(TimePoint::from_secs_f64(0.0), 0.0, 0.0, 0.0); diff --git a/mapf/src/post/spatial_allocation/mod.rs b/mapf/src/post/spatial_allocation/mod.rs index 405b2d9..e153e24 100644 --- a/mapf/src/post/spatial_allocation/mod.rs +++ b/mapf/src/post/spatial_allocation/mod.rs @@ -3,7 +3,7 @@ use std::collections::{HashMap, HashSet}; use bresenham::Bresenham; use super::{ - IntersectionType, LeaderFollowerZones, MapfResult, SemanticPlan, SemanticWaypoint, mapf_post, + mapf_post, IntersectionType, LeaderFollowerZones, MapfResult, SemanticPlan, SemanticWaypoint, }; #[derive(Clone)] diff --git a/mapf/src/premade/search_r2.rs b/mapf/src/premade/search_r2.rs index b66bb18..3e79269 100644 --- a/mapf/src/premade/search_r2.rs +++ b/mapf/src/premade/search_r2.rs @@ -18,7 +18,7 @@ use crate::{ domain::{Key, KeyedCloser, Reversible}, graph::{Graph, SharedGraph}, - motion::{SpeedLimiter, TravelTimeCost, r2::*}, + motion::{r2::*, SpeedLimiter, TravelTimeCost}, templates::{GraphMotion, InformedSearch}, }; @@ -67,7 +67,7 @@ where mod tests { use super::*; use crate::{ - Planner, algorithm::AStar, domain::AsTimeVariant, graph::SimpleGraph, motion::SpeedLimit, + algorithm::AStar, domain::AsTimeVariant, graph::SimpleGraph, motion::SpeedLimit, Planner, }; use std::sync::Arc; diff --git a/mapf/src/premade/search_se2.rs b/mapf/src/premade/search_se2.rs index 22aa2ab..913f95f 100644 --- a/mapf/src/premade/search_se2.rs +++ b/mapf/src/premade/search_se2.rs @@ -20,9 +20,9 @@ use crate::{ error::Anyhow, graph::{Graph, SharedGraph}, motion::{ - SpeedLimiter, TravelTimeCost, r2::{DirectTravelHeuristic, DiscreteSpaceTimeR2, Positioned, StateR2}, se2::*, + SpeedLimiter, TravelTimeCost, }, templates::{GraphMotion, InformedSearch}, }; @@ -90,7 +90,7 @@ where #[cfg(test)] mod tests { use super::*; - use crate::{Planner, algorithm::AStarConnect, graph::SimpleGraph, motion::SpeedLimit}; + use crate::{algorithm::AStarConnect, graph::SimpleGraph, motion::SpeedLimit, Planner}; use std::sync::Arc; #[test] diff --git a/mapf/src/premade/sipp_se2.rs b/mapf/src/premade/sipp_se2.rs index d9c29f5..ff57ea7 100644 --- a/mapf/src/premade/sipp_se2.rs +++ b/mapf/src/premade/sipp_se2.rs @@ -20,9 +20,9 @@ use crate::{ error::{Anyhow, StdError}, graph::{Graph, SharedGraph}, motion::{ - CcbsEnvironment, SafeIntervalCache, SafeIntervalCloser, SpeedLimiter, TravelEffortCost, r2::Positioned, se2::{quickest_path::QuickestPathSearch, *}, + CcbsEnvironment, SafeIntervalCache, SafeIntervalCloser, SpeedLimiter, TravelEffortCost, }, templates::{ConflictAvoidance, GraphMotion, InformedSearch, LazyGraphMotion}, }; @@ -393,15 +393,15 @@ where mod tests { use super::*; use crate::{ - Planner, algorithm::AStarConnect, graph::{ - SimpleGraph, occupancy::{Cell, NeighborhoodGraph, SparseGrid, Visibility, VisibilityGraph}, + SimpleGraph, }, motion::{ CircularProfile, DynamicCircularObstacle, DynamicEnvironment, TimePoint, Trajectory, }, + Planner, }; use approx::assert_relative_eq; use std::sync::Arc; From ecb1afc5e29426dc0c89fe424155170aca2137a0 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 19 Mar 2026 04:58:16 +0000 Subject: [PATCH 06/11] undo unessecary changes Signed-off-by: Arjo Chakravarty --- mapf/src/algorithm/dijkstra/forward.rs | 2 +- mapf/src/motion/conflict.rs | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/mapf/src/algorithm/dijkstra/forward.rs b/mapf/src/algorithm/dijkstra/forward.rs index 53e49d2..ce6aeff 100644 --- a/mapf/src/algorithm/dijkstra/forward.rs +++ b/mapf/src/algorithm/dijkstra/forward.rs @@ -127,7 +127,7 @@ where Err(_) => { return Err(Self::algo_err( DijkstraImplError::PoisonedMutex, - )); + )) } }; diff --git a/mapf/src/motion/conflict.rs b/mapf/src/motion/conflict.rs index 1876684..66822ef 100644 --- a/mapf/src/motion/conflict.rs +++ b/mapf/src/motion/conflict.rs @@ -95,7 +95,7 @@ where None => { return SmallVec::from_iter([Err( SafeActionIntegrateWaypointError::MissingInitialWaypoint, - )]); + )]) } }; From e4af8fba4fa301f3875c11f5c7617d55668e164f Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 20 Mar 2026 08:43:31 +0000 Subject: [PATCH 07/11] Remove dead slop code Signed-off-by: Arjo Chakravarty --- mapf/src/negotiation/scenario.rs | 23 ----------------------- 1 file changed, 23 deletions(-) diff --git a/mapf/src/negotiation/scenario.rs b/mapf/src/negotiation/scenario.rs index 3dc7882..cca527a 100644 --- a/mapf/src/negotiation/scenario.rs +++ b/mapf/src/negotiation/scenario.rs @@ -183,29 +183,6 @@ pub struct Scenario { pub id_to_name: HashMap, } -impl Scenario { - pub fn add_mobile_objects_as_agents(&mut self) { - let mut last_id = 0; - for key in self.agents.keys() { - if let Ok(id) = key.parse::() { - last_id = last_id.max(id); - } - } - - if !self.agents.is_empty() { - last_id += 1; - } - - let obstacles = std::mem::take(&mut self.obstacles); - for obstacle in obstacles { - if let Some(agent) = obstacle.to_agent() { - self.agents.insert(last_id.to_string(), agent); - last_id += 1; - } - } - } -} - pub fn default_radius() -> f64 { 0.45 } From c473b144ef94e651fd9313e5f2049d47fb08bd85 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 20 Mar 2026 08:55:42 +0000 Subject: [PATCH 08/11] Refactor agent ID handling and remove unnecessary mutability - Move agent ID assignment from negotiate to derive_mapf_result - Remove id_to_name field from Scenario struct - Change negotiate and Scenario::solve to take shared references - Add agent_name_to_id to MapfResult and SemanticPlan - Add get_agent_id helper to SemanticPlan Generated-by: Gemini CLI Signed-off-by: Arjo Chakravarty --- mapf/src/negotiation/mod.rs | 10 ++++++---- mapf/src/negotiation/scenario.rs | 3 --- mapf/src/post/mod.rs | 21 ++++++++++++++++++--- 3 files changed, 24 insertions(+), 10 deletions(-) diff --git a/mapf/src/negotiation/mod.rs b/mapf/src/negotiation/mod.rs index 0d4e1cd..14de0e3 100644 --- a/mapf/src/negotiation/mod.rs +++ b/mapf/src/negotiation/mod.rs @@ -55,7 +55,7 @@ pub enum NegotiationError { } pub fn negotiate( - scenario: &mut Scenario, + scenario: &Scenario, queue_length_limit: Option, ) -> Result< ( @@ -98,7 +98,6 @@ pub fn negotiate( agents.push(agent.clone()); } - scenario.id_to_name = name_map.clone(); (name_map, agents) }; @@ -656,7 +655,7 @@ impl NegotiationNode { impl Scenario { pub fn solve( - &mut self, + &self, queue_length_limit: Option, ) -> Result { let (solution, _, _) = negotiate(self, queue_length_limit)?; @@ -681,8 +680,10 @@ impl Scenario { let mut trajectories = Vec::new(); let mut footprints = Vec::new(); + let mut agent_name_to_id = std::collections::HashMap::new(); - for (i, (_name, agent)) in self.agents.iter().enumerate() { + for (i, (name, agent)) in self.agents.iter().enumerate() { + agent_name_to_id.insert(name.clone(), i); footprints.push( std::sync::Arc::new(crate::post::shape::Ball::new(agent.radius)) as std::sync::Arc, @@ -729,6 +730,7 @@ impl Scenario { trajectories, footprints, discretization_timestep: timestep, + agent_name_to_id, } } diff --git a/mapf/src/negotiation/scenario.rs b/mapf/src/negotiation/scenario.rs index cca527a..f6e70f4 100644 --- a/mapf/src/negotiation/scenario.rs +++ b/mapf/src/negotiation/scenario.rs @@ -179,8 +179,6 @@ pub struct Scenario { pub cell_size: f64, #[serde(skip_serializing_if = "Option::is_none", default)] pub camera_bounds: Option<[[f32; 2]; 2]>, - #[serde(skip_serializing)] - pub id_to_name: HashMap, } pub fn default_radius() -> f64 { @@ -227,7 +225,6 @@ mod tests { occupancy: HashMap::new(), cell_size: 1.0, camera_bounds: None, - id_to_name: HashMap::new(), }; // We need a mock solution node. diff --git a/mapf/src/post/mod.rs b/mapf/src/post/mod.rs index f332915..0a4f949 100644 --- a/mapf/src/post/mod.rs +++ b/mapf/src/post/mod.rs @@ -237,9 +237,16 @@ pub struct SemanticPlan { depends_on_all_of: HashMap>, /// Next states. Key is the state potential_successors: HashMap>, + /// Mapping from agent name to ID + pub agent_name_to_id: HashMap, } impl SemanticPlan { + /// Get the agent ID from its name + pub fn get_agent_id(&self, name: &str) -> Option { + self.agent_name_to_id.get(name).copied() + } + fn add_waypoint(&mut self, waypoint: &SemanticWaypoint) { self.agent_time_to_wp_id .insert(*waypoint, self.waypoints.len()); @@ -1010,6 +1017,8 @@ pub struct MapfResult { pub footprints: Vec>, /// The time discretization of the trajectories pub discretization_timestep: f64, + /// Mapping from agent name to ID + pub agent_name_to_id: HashMap, } impl std::fmt::Debug for MapfResult { @@ -1084,7 +1093,10 @@ fn collides( /// /// Based on https://whoenig.github.io/publications/2019_RA-L_Hoenig.pdf pub fn mapf_post(mapf_result: &MapfResult) -> SemanticPlan { - let mut semantic_plan = SemanticPlan::default(); + let mut semantic_plan = SemanticPlan { + agent_name_to_id: mapf_result.agent_name_to_id.clone(), + ..SemanticPlan::default() + }; semantic_plan.num_agents = mapf_result.trajectories.len(); // Type 1 edges for agent in 0..mapf_result.trajectories.len() { @@ -1335,7 +1347,8 @@ mod tests { Arc::new(parry2d::shape::Ball::new(0.49)), // Footprint for agent1 Arc::new(parry2d::shape::Ball::new(0.49)), // Footprint for agent2 ], - discretization_timestep: 1.0, // Example timestep + discretization_timestep: 1.0, + agent_name_to_id: HashMap::default(), }; // Generate the semantic plan @@ -1394,7 +1407,8 @@ mod tests { Arc::new(parry2d::shape::Ball::new(0.49)), // Footprint for agent1 Arc::new(parry2d::shape::Ball::new(0.49)), // Footprint for agent2 ], - discretization_timestep: 1.0, // Example timestep + discretization_timestep: 1.0, + agent_name_to_id: HashMap::default(), }; let semantic_plan = mapf_post(&mapf_result); @@ -1461,6 +1475,7 @@ mod tests { Arc::new(parry2d::shape::Ball::new(0.49)), ], discretization_timestep: 1.0, + agent_name_to_id: HashMap::default(), }; let semantic_plan = mapf_post(&mapf_result); From 71cfa001d6efc3160ffb2e81613769891ea03625 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 20 Mar 2026 09:00:03 +0000 Subject: [PATCH 09/11] Make MapfResult private to prvent confusion Signed-off-by: Arjo Chakravarty --- mapf/src/negotiation/mod.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mapf/src/negotiation/mod.rs b/mapf/src/negotiation/mod.rs index 14de0e3..88c1bbb 100644 --- a/mapf/src/negotiation/mod.rs +++ b/mapf/src/negotiation/mod.rs @@ -662,7 +662,7 @@ impl Scenario { Ok(solution) } - pub fn derive_mapf_result( + fn derive_mapf_result( &self, solution: &NegotiationNode, timestep: f64, From a001f0ef0f4c5552131885bb6fa716124fdccdba Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 20 Mar 2026 09:09:34 +0000 Subject: [PATCH 10/11] Implement Grid2D constructor from Scenario - Add Grid2D::from_scenario and From<&Scenario> for Grid2D - Correctly calculate grid dimensions and occupancy from Scenario data - Update allocate_trajectory to use dynamic grid dimensions - Add unit test for from_scenario constructor Generated-by: Gemini CLI Signed-off-by: Gemini CLI Signed-off-by: Arjo Chakravarty --- mapf/src/post/spatial_allocation/mod.rs | 94 ++++++++++++++++++++++++- 1 file changed, 93 insertions(+), 1 deletion(-) diff --git a/mapf/src/post/spatial_allocation/mod.rs b/mapf/src/post/spatial_allocation/mod.rs index e153e24..de8f3fb 100644 --- a/mapf/src/post/spatial_allocation/mod.rs +++ b/mapf/src/post/spatial_allocation/mod.rs @@ -6,6 +6,8 @@ use super::{ mapf_post, IntersectionType, LeaderFollowerZones, MapfResult, SemanticPlan, SemanticWaypoint, }; +use crate::negotiation::Scenario; + #[derive(Clone)] pub struct CurrentPosition { pub semantic_position: SemanticWaypoint, @@ -20,6 +22,49 @@ pub struct Grid2D { } impl Grid2D { + pub fn from_scenario(scenario: &Scenario) -> Self { + let mut max_x = 0i64; + let mut max_y = 0i64; + + for (y, row) in &scenario.occupancy { + max_y = max_y.max(*y); + for x in row { + max_x = max_x.max(*x); + } + } + + for agent in scenario.agents.values() { + max_x = max_x.max(agent.start[0]).max(agent.goal[0]); + max_y = max_y.max(agent.start[1]).max(agent.goal[1]); + } + + for obstacle in &scenario.obstacles { + for (_, x, y) in &obstacle.trajectory { + max_x = max_x.max(*x); + max_y = max_y.max(*y); + } + } + + if let Some(bounds) = scenario.camera_bounds { + max_x = max_x.max((bounds[1][0] / scenario.cell_size as f32) as i64); + max_y = max_y.max((bounds[1][1] / scenario.cell_size as f32) as i64); + } + + let width = (max_x + 1) as usize; + let height = (max_y + 1) as usize; + + let mut static_obstacles = vec![vec![0i8; height]; width]; + for (y, row) in &scenario.occupancy { + for x in row { + if *x >= 0 && *y >= 0 { + static_obstacles[*x as usize][height - 1 - *y as usize] = 100; + } + } + } + + Self::new(static_obstacles, scenario.cell_size) + } + pub fn new(static_obstacles: Vec>, cell_size: f64) -> Self { let width = static_obstacles.len(); let height = if width > 0 { @@ -59,7 +104,7 @@ impl Grid2D { positions.iter().map(|p| p.semantic_position).collect(); let assignment = semantic_plan.get_claim_dict(&semantic_positions); - let mut allocation_field = AllocationField::create(semantic_plan, 1000, 1000); + let mut allocation_field = AllocationField::create(semantic_plan, self.width, self.height); for (agent, traj) in trajectories.trajectories.iter().enumerate() { let Some(region) = assignment.get(&agent) else { @@ -357,3 +402,50 @@ impl AllocationField { Some(p.priority) } } + +impl From<&Scenario> for Grid2D { + fn from(scenario: &Scenario) -> Self { + Self::from_scenario(scenario) + } +} + +#[cfg(test)] +mod tests { + use super::*; + use std::collections::BTreeMap; + + #[test] + fn test_grid2d_from_scenario() { + let mut occupancy = HashMap::new(); + occupancy.insert(0, vec![0, 1]); // y=0, x=0,1 + occupancy.insert(1, vec![0]); // y=1, x=0 + + let scenario = Scenario { + agents: BTreeMap::new(), + obstacles: vec![], + occupancy, + cell_size: 1.0, + camera_bounds: None, + }; + + let grid = Grid2D::from_scenario(&scenario); + + // max_x = 1, max_y = 1 + // width = 2, height = 2 + assert_eq!(grid.width, 2); + assert_eq!(grid.height, 2); + + // In Grid2D, y_grid = height - 1 - y_scenario + // y_scenario=0 -> y_grid=1 + // y_scenario=1 -> y_grid=0 + + // (0,0) scenario -> (0,1) grid + assert_eq!(grid.static_obstacles[0][1], 100); + // (1,0) scenario -> (1,1) grid + assert_eq!(grid.static_obstacles[1][1], 100); + // (0,1) scenario -> (0,0) grid + assert_eq!(grid.static_obstacles[0][0], 100); + // (1,1) scenario -> (1,0) grid (not occupied) + assert_eq!(grid.static_obstacles[1][0], 0); + } +} From ced6539b179fa1ced3fe4781032dcfaef91a8e75 Mon Sep 17 00:00:00 2001 From: Uday Kalyan S <65308101+uday-kalyan-s@users.noreply.github.com> Date: Wed, 19 Aug 2026 12:41:45 +0530 Subject: [PATCH 11/11] Transfer collision detection optimizations from arjo129/mapf_post (#44) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit ### Implemented feature Adds an AABB sweep-and-prune broad phase to `mapf::post::mapf_post`'s Type‑2 (cross-agent collision) dependency detection, replacing the previous brute-force O(N²T²) nested-loop check. Also adds Criterion benchmarks so the improvement (and any future regression) is measurable rather than just asserted. ### Implementation description The broad phase mirrors the approach already used in the sibling `mapf_post` crate: for every trajectory segment, compute a merged AABB spanning its start and end pose, pick whichever axis (X or Y) has the larger spread across all segments, sort segments by AABB min on that axis, then sweep with an early-exit break once a later segment's AABB min passes the current segment's AABB max on that axis. Only AABB-overlapping pairs go through the exact continuous collision check (`collides`, via `parry2d::query::cast_shapes_nonlinear`) that used to run on every pair unconditionally. Changes: - `mapf/mapf/src/post/mod.rs`: replaced the brute-force Type‑2 edge loop in `mapf_post` with the sweep-and-prune broad phase. - `mapf/mapf/src/post/mod.rs` (`sweep_line_tests` module): added tests comparing the sweep-line output against a brute-force reference implementation across several scenarios (crossing, following, head-on swap, diagonal, far-apart, unequal-length trajectories), plus a sort-axis-invariance test (X-sort vs. Y-sort scenes must agree). Complexity is unchanged in the worst case (still O((N·T)²) when a scene is AABB-dense on both axes), but substantially better in the common case of spatially separated trajectories, since the sweep prunes the AABB comparisons that no longer need the expensive narrow-phase check. Signed-off-by: uday-kalyan-s * fixed styling Signed-off-by: uday-kalyan-s * code cleanup and better benchmarking Signed-off-by: uday-kalyan-s * moved analysis to cargo bench Signed-off-by: uday-kalyan-s --------- Signed-off-by: uday-kalyan-s --- mapf/Cargo.toml | 4 + mapf/benches/grid_scene_timings.rs | 89 ++++++ mapf/src/post/mod.rs | 457 +++++++++++++++++++++++++++-- 3 files changed, 530 insertions(+), 20 deletions(-) create mode 100644 mapf/benches/grid_scene_timings.rs diff --git a/mapf/Cargo.toml b/mapf/Cargo.toml index 04e3cdb..8e6bf87 100644 --- a/mapf/Cargo.toml +++ b/mapf/Cargo.toml @@ -35,3 +35,7 @@ csv = "1.1" [dev-dependencies] approx = "0.5" + +[[bench]] +name = "grid_scene_timings" +harness = false diff --git a/mapf/benches/grid_scene_timings.rs b/mapf/benches/grid_scene_timings.rs new file mode 100644 index 0000000..24f6ac2 --- /dev/null +++ b/mapf/benches/grid_scene_timings.rs @@ -0,0 +1,89 @@ +//! Plain `Instant` timing, no benchmarking crate. See `post::timing::grid_scene` +//! in `src/post/mod.rs` for why this scene is adversarial to the sweep. + +use mapf::post::{ + mapf_post, mapf_post_sweep, + na::{Isometry2, Vector2}, + shape::{Ball, Shape}, + MapfResult, SemanticPlan, Trajectory, +}; +use std::collections::HashMap; +use std::sync::Arc; +use std::time::{Duration, Instant}; + +fn grid_scene(n: usize, num_waypoints: usize) -> MapfResult { + let side = (n as f64).sqrt().ceil() as usize; + let cell_spacing = 3.0; + let wiggle = 0.4; + let denom = (num_waypoints.max(2) - 1) as f64; + + let mut trajectories = Vec::with_capacity(n); + let mut footprints = Vec::with_capacity(n); + for idx in 0..n { + let col = (idx % side) as f64; + let row = (idx / side) as f64; + let cx = col * cell_spacing; + let cy = row * cell_spacing; + let poses = (0..num_waypoints) + .map(|wp_idx| { + let t = wp_idx as f64 / denom * std::f64::consts::TAU; + Isometry2::new( + Vector2::new(cx + t.sin() * wiggle, cy + t.cos() * wiggle), + 0.0, + ) + }) + .collect(); + trajectories.push(Trajectory { poses }); + footprints.push(Arc::new(Ball::new(0.49)) as Arc); + } + MapfResult { + trajectories, + footprints, + discretization_timestep: 1.0, + agent_name_to_id: HashMap::default(), + } +} + +fn time(mut f: F, iters: u32) -> Duration { + let start = Instant::now(); + for _ in 0..iters { + f(); + } + start.elapsed() / iters +} + +fn main() { + let agent_counts = [ + 1usize, 10, 50, 100, 500, 1000, 2000, 4000, 8000, 16000, + ]; + let traj_lengths = [1usize, 10, 50]; + + for (strategy_name, strategy) in [ + ("sweep", mapf_post_sweep as fn(&MapfResult) -> SemanticPlan), + ("aabb_tree", mapf_post as fn(&MapfResult) -> SemanticPlan), + ] { + println!("\n== {strategy_name}: agents (rows) x trajectory length (columns) =="); + print!("{:>10}", "agents"); + for &tl in &traj_lengths { + print!(" {:>14}", format!("len={tl}")); + } + println!(); + + for &n in &agent_counts { + print!("{n:>10}"); + for &tl in &traj_lengths { + let scene = grid_scene(n, tl); + let segments = n * tl.saturating_sub(1); + let iters = if segments > 5_000 { 1 } else { 3 }; + let elapsed = time( + || { + std::hint::black_box(strategy(&scene)); + }, + iters, + ); + print!(" {elapsed:>14?}"); + } + println!(); + } + } +} diff --git a/mapf/src/post/mod.rs b/mapf/src/post/mod.rs index 0a4f949..546bff8 100644 --- a/mapf/src/post/mod.rs +++ b/mapf/src/post/mod.rs @@ -2,7 +2,9 @@ use core::panic; use std::collections::{HashMap, HashSet}; use std::sync::Arc; +use parry2d::bounding_volume::{Aabb, BoundingVolume}; use parry2d::na::{Isometry2, Point2}; +use parry2d::partitioning::Qbvh; use parry2d::query::cast_shapes_nonlinear; use parry2d::query::{NonlinearRigidMotion, ShapeCastStatus}; use parry2d::shape::Shape; @@ -1087,18 +1089,13 @@ fn collides( } } -/// Simple implementation of mapf_post: A MapF -> semantic plan -/// Method. -/// TODO(arjoc): This can be improved. -/// -/// Based on https://whoenig.github.io/publications/2019_RA-L_Hoenig.pdf -pub fn mapf_post(mapf_result: &MapfResult) -> SemanticPlan { +/// Agent bookkeeping + Type 1 edges, shared by every `mapf_post*` variant. +fn init_semantic_plan_with_type1_edges(mapf_result: &MapfResult) -> SemanticPlan { let mut semantic_plan = SemanticPlan { agent_name_to_id: mapf_result.agent_name_to_id.clone(), ..SemanticPlan::default() }; semantic_plan.num_agents = mapf_result.trajectories.len(); - // Type 1 edges for agent in 0..mapf_result.trajectories.len() { for trajectory_index in 0..mapf_result.trajectories[agent].len() { semantic_plan.add_waypoint(&SemanticWaypoint { @@ -1122,34 +1119,36 @@ pub fn mapf_post(mapf_result: &MapfResult) -> SemanticPlan { ); } } + semantic_plan +} - // Type 2 edges - for agent1 in 0..mapf_result.trajectories.len() { - for trajectory_index1 in 1..mapf_result.trajectories[agent1].len() { - for agent2 in 0..mapf_result.trajectories.len() { +/// Type 2 edges, brute-force O((N*T)^2), no pruning. Correctness reference. +fn add_type2_edges_bruteforce(semantic_plan: &mut SemanticPlan, mapf_result: &MapfResult) { + let trajectories = &mapf_result.trajectories; + for agent1 in 0..trajectories.len() { + for ti1 in 1..trajectories[agent1].len() { + for agent2 in 0..trajectories.len() { if agent1 == agent2 { continue; } - for trajectory_index2 in - trajectory_index1 + 1..mapf_result.trajectories[agent2].len() - { + for ti2 in ti1 + 1..trajectories[agent2].len() { if collides( - &mapf_result.trajectories[agent1].poses[trajectory_index1 - 1], - &mapf_result.trajectories[agent1].poses[trajectory_index1], + &trajectories[agent1].poses[ti1 - 1], + &trajectories[agent1].poses[ti1], &*mapf_result.footprints[agent1], - &mapf_result.trajectories[agent2].poses[trajectory_index2 - 1], - &mapf_result.trajectories[agent2].poses[trajectory_index2], + &trajectories[agent2].poses[ti2 - 1], + &trajectories[agent2].poses[ti2], &*mapf_result.footprints[agent2], mapf_result.discretization_timestep, ) { semantic_plan.requires_comes_after( &SemanticWaypoint { agent: agent1, - trajectory_index: trajectory_index1 - 1, + trajectory_index: ti1 - 1, }, &SemanticWaypoint { agent: agent2, - trajectory_index: trajectory_index2 - 1, + trajectory_index: ti2 - 1, }, ); } @@ -1157,6 +1156,186 @@ pub fn mapf_post(mapf_result: &MapfResult) -> SemanticPlan { } } } +} + +struct SegmentAabb { + agent: usize, + index: usize, // index in trajectory (represents motion from index-1 to index) + aabb: Aabb, +} + +fn segment_aabbs(mapf_result: &MapfResult) -> Vec { + let mut all_segments = Vec::new(); + for (agent_idx, trajectory) in mapf_result.trajectories.iter().enumerate() { + let footprint = &*mapf_result.footprints[agent_idx]; + for i in 1..trajectory.len() { + let aabb1 = footprint.compute_aabb(&trajectory.poses[i - 1]); + let aabb2 = footprint.compute_aabb(&trajectory.poses[i]); + all_segments.push(SegmentAabb { + agent: agent_idx, + index: i, + aabb: aabb1.merged(&aabb2), + }); + } + } + all_segments +} + +/// Exact `collides` check on a normalized (earlier, later) pair; records +/// the dependency edge if they actually collide. +fn check_and_record_collision( + semantic_plan: &mut SemanticPlan, + mapf_result: &MapfResult, + earlier: &SegmentAabb, + later: &SegmentAabb, +) { + if collides( + &mapf_result.trajectories[earlier.agent].poses[earlier.index - 1], + &mapf_result.trajectories[earlier.agent].poses[earlier.index], + &*mapf_result.footprints[earlier.agent], + &mapf_result.trajectories[later.agent].poses[later.index - 1], + &mapf_result.trajectories[later.agent].poses[later.index], + &*mapf_result.footprints[later.agent], + mapf_result.discretization_timestep, + ) { + semantic_plan.requires_comes_after( + &SemanticWaypoint { + agent: earlier.agent, + trajectory_index: earlier.index - 1, + }, + &SemanticWaypoint { + agent: later.agent, + trajectory_index: later.index - 1, + }, + ); + } +} + +/// Orders a pair by trajectory index. `None` if same agent or same index. +fn normalize_pair<'a>( + seg1: &'a SegmentAabb, + seg2: &'a SegmentAabb, +) -> Option<(&'a SegmentAabb, &'a SegmentAabb)> { + if seg1.agent == seg2.agent { + return None; + } + if seg1.index < seg2.index { + Some((seg1, seg2)) + } else if seg2.index < seg1.index { + Some((seg2, seg1)) + } else { + None + } +} + +/// Type 2 edges via sort-and-sweep on whichever axis has the larger spread. +fn add_type2_edges_sweep(semantic_plan: &mut SemanticPlan, mapf_result: &MapfResult) { + let mut all_segments = segment_aabbs(mapf_result); + if all_segments.is_empty() { + return; + } + + let mut min_bound = Point2::new(f64::MAX, f64::MAX); + let mut max_bound = Point2::new(f64::MIN, f64::MIN); + for seg in &all_segments { + min_bound.x = min_bound.x.min(seg.aabb.mins.x); + min_bound.y = min_bound.y.min(seg.aabb.mins.y); + max_bound.x = max_bound.x.max(seg.aabb.maxs.x); + max_bound.y = max_bound.y.max(seg.aabb.maxs.y); + } + let sort_by_x = (max_bound.x - min_bound.x) >= (max_bound.y - min_bound.y); + + if sort_by_x { + all_segments.sort_by(|a, b| a.aabb.mins.x.partial_cmp(&b.aabb.mins.x).unwrap()); + } else { + all_segments.sort_by(|a, b| a.aabb.mins.y.partial_cmp(&b.aabb.mins.y).unwrap()); + } + + for i in 0..all_segments.len() { + let seg1 = &all_segments[i]; + for j in i + 1..all_segments.len() { + let seg2 = &all_segments[j]; + + if sort_by_x { + if seg2.aabb.mins.x > seg1.aabb.maxs.x { + break; + } + } else if seg2.aabb.mins.y > seg1.aabb.maxs.y { + break; + } + + let Some((earlier, later)) = normalize_pair(seg1, seg2) else { + continue; + }; + if earlier.aabb.intersects(&later.aabb) { + check_and_record_collision(semantic_plan, mapf_result, earlier, later); + } + } + } +} + +/// Type 2 edges via `parry2d::partitioning::Qbvh` (already a dependency). +fn add_type2_edges_aabb_tree(semantic_plan: &mut SemanticPlan, mapf_result: &MapfResult) { + let all_segments = segment_aabbs(mapf_result); + if all_segments.is_empty() { + return; + } + + let entries: Vec<(usize, Aabb)> = all_segments + .iter() + .enumerate() + .map(|(i, seg)| (i, seg.aabb)) + .collect(); + let mut tree: Qbvh = Qbvh::new(); + tree.clear_and_rebuild(entries.into_iter(), 0.0); + + let mut candidates = Vec::new(); + for i in 0..all_segments.len() { + let seg1 = &all_segments[i]; + + candidates.clear(); + tree.intersect_aabb(&seg1.aabb, &mut candidates); + + for &j in &candidates { + // dedup: each pair is found from both sides, and a segment finds itself + if j <= i { + continue; + } + + let seg2 = &all_segments[j]; + let Some((earlier, later)) = normalize_pair(seg1, seg2) else { + continue; + }; + check_and_record_collision(semantic_plan, mapf_result, earlier, later); + } + } +} + +/// Simple implementation of mapf_post: A MapF -> semantic plan +/// Method. +/// TODO(arjoc): This can be improved. +/// +/// Based on https://whoenig.github.io/publications/2019_RA-L_Hoenig.pdf +/// +/// Uses the `Qbvh` broad phase. See `mapf_post_sweep` and +/// `mapf_post_bruteforce` for the alternatives this is benchmarked against. +pub fn mapf_post(mapf_result: &MapfResult) -> SemanticPlan { + let mut semantic_plan = init_semantic_plan_with_type1_edges(mapf_result); + add_type2_edges_aabb_tree(&mut semantic_plan, mapf_result); + semantic_plan +} + +/// Same as `mapf_post`, but with the sort-and-sweep broad phase. +pub fn mapf_post_sweep(mapf_result: &MapfResult) -> SemanticPlan { + let mut semantic_plan = init_semantic_plan_with_type1_edges(mapf_result); + add_type2_edges_sweep(&mut semantic_plan, mapf_result); + semantic_plan +} + +/// Same as `mapf_post`, but brute-force with no pruning. +pub fn mapf_post_bruteforce(mapf_result: &MapfResult) -> SemanticPlan { + let mut semantic_plan = init_semantic_plan_with_type1_edges(mapf_result); + add_type2_edges_bruteforce(&mut semantic_plan, mapf_result); semantic_plan } @@ -1520,3 +1699,241 @@ mod tests { assert!(!semantic_plan.check_for_violation(&safe_state_2)); } } + +/// Cross-checks all three `mapf_post*` strategies against each other. +#[cfg(test)] +mod aabb_tree_tests { + use super::*; + use parry2d::na::Vector2; + use parry2d::shape::Ball; + use std::collections::HashSet; + + fn mapf_result_from(paths: &[Vec<(f64, f64)>], r: f64) -> MapfResult { + MapfResult { + trajectories: paths + .iter() + .map(|path| Trajectory { + poses: path + .iter() + .map(|&(x, y)| Isometry2::new(Vector2::new(x, y), 0.0)) + .collect(), + }) + .collect(), + footprints: paths + .iter() + .map(|_| Arc::new(Ball::new(r)) as Arc) + .collect(), + discretization_timestep: 1.0, + agent_name_to_id: HashMap::default(), + } + } + + fn type2_edges_of(plan: &SemanticPlan) -> HashSet<(SemanticWaypoint, SemanticWaypoint)> { + let mut edges = HashSet::new(); + for successor in &plan.waypoints { + let Some(predecessor_ids) = plan.comes_before(successor) else { + continue; + }; + for &pred_id in predecessor_ids { + let predecessor = plan.waypoints[pred_id]; + if predecessor.agent != successor.agent { + edges.insert((predecessor, *successor)); + } + } + } + edges + } + + /// All three strategies must produce the same dependency graph. + #[test] + fn test_all_strategies_agree() { + let scenarios: Vec<(&str, Vec>)> = vec![ + ( + "horizontal_cross", + vec![ + vec![(0.0, 0.0), (1.0, 0.0), (2.0, 0.0), (2.0, 0.0)], + vec![(1.0, -1.0), (1.0, -1.0), (1.0, 0.0), (1.0, 1.0)], + ], + ), + ( + "horizontal_follow", + vec![ + vec![(0.0, 0.0), (1.0, 0.0), (2.0, 0.0), (3.0, 0.0)], + vec![(1.0, 0.0), (2.0, 0.0), (3.0, 0.0), (4.0, 0.0)], + ], + ), + ( + "vertical_follow", + vec![ + vec![(0.0, 0.0), (0.0, 1.0), (0.0, 2.0), (0.0, 3.0)], + vec![(0.0, 1.0), (0.0, 2.0), (0.0, 3.0), (0.0, 4.0)], + ], + ), + ( + "head_on_swap", + vec![ + vec![(0.0, 0.0), (1.0, 0.0), (2.0, 0.0)], + vec![(2.0, 0.0), (1.0, 0.0), (0.0, 0.0)], + ], + ), + ( + "diagonal_cross", + vec![ + vec![(0.0, 0.0), (1.0, 1.0), (2.0, 2.0)], + vec![(0.0, 2.0), (1.0, 1.0), (2.0, 0.0)], + ], + ), + ( + "parallel_far_apart", + vec![ + vec![(0.0, 0.0), (1.0, 0.0), (2.0, 0.0)], + vec![(0.0, 50.0), (1.0, 50.0), (2.0, 50.0)], + ], + ), + ( + "three_agents", + vec![ + vec![(0.0, 0.0), (1.0, 0.0), (2.0, 0.0), (3.0, 0.0)], + vec![(1.5, -1.5), (1.5, 0.0), (1.5, 1.5), (1.5, 1.5)], + vec![(3.0, 0.0), (2.0, 0.0), (1.0, 0.0), (0.0, 0.0)], + ], + ), + ( + "unequal_lengths_crossing", + vec![ + vec![(0.0, 0.0), (1.0, 0.0), (2.0, 0.0)], + vec![(1.0, -2.0), (1.0, -1.0), (1.0, 0.0), (1.0, 1.0), (1.0, 2.0)], + ], + ), + ]; + + for (name, paths) in scenarios { + let mapf_result = mapf_result_from(&paths, 0.49); + let expected = type2_edges_of(&mapf_post_bruteforce(&mapf_result)); + let sweep = type2_edges_of(&mapf_post_sweep(&mapf_result)); + let tree = type2_edges_of(&mapf_post(&mapf_result)); + assert_eq!( + sweep, expected, + "sweep and brute-force disagree on scenario '{name}'" + ); + assert_eq!( + tree, expected, + "AABB tree and brute-force disagree on scenario '{name}'" + ); + } + } + + /// A scene and its 90-degree rotation must yield identical edges. + #[test] + fn test_rotation_invariance() { + let horizontal = mapf_result_from( + &[ + vec![(0.0, 0.0), (1.0, 0.0), (2.0, 0.0), (3.0, 0.0)], + vec![(1.0, 0.0), (2.0, 0.0), (3.0, 0.0), (4.0, 0.0)], + ], + 0.49, + ); + let vertical = mapf_result_from( + &[ + vec![(0.0, 0.0), (0.0, 1.0), (0.0, 2.0), (0.0, 3.0)], + vec![(0.0, 1.0), (0.0, 2.0), (0.0, 3.0), (0.0, 4.0)], + ], + 0.49, + ); + + let horizontal_edges = type2_edges_of(&mapf_post(&horizontal)); + let vertical_edges = type2_edges_of(&mapf_post(&vertical)); + + assert!( + !horizontal_edges.is_empty(), + "overlapping follow paths must produce collision dependencies" + ); + assert_eq!( + horizontal_edges, vertical_edges, + "X-split and Y-split root partitions must produce identical edges" + ); + } +} + +/// See `benches/grid_scene_timings.rs` for timings. +#[cfg(test)] +mod timing { + use super::*; + use parry2d::na::Vector2; + use parry2d::shape::Ball; + + /// `n` agents on a `ceil(sqrt(n))`-per-side grid, each wiggling in place + /// in its own cell (nothing collides). Adversarial to a single sweep + /// axis: every column shares near-identical X extent, so the sweep's + /// early-break never fires within a column (O(n^1.5) total), while + /// `Qbvh` re-picks its split axis per node and isn't stuck. + fn grid_scene(n: usize, num_waypoints: usize) -> MapfResult { + let side = (n as f64).sqrt().ceil() as usize; + let cell_spacing = 3.0; + let wiggle = 0.4; + // num_waypoints == 1 has no segment, so denom just avoids div-by-zero + let denom = (num_waypoints.max(2) - 1) as f64; + + let mut trajectories = Vec::with_capacity(n); + let mut footprints = Vec::with_capacity(n); + for idx in 0..n { + let col = (idx % side) as f64; + let row = (idx / side) as f64; + let cx = col * cell_spacing; + let cy = row * cell_spacing; + let poses = (0..num_waypoints) + .map(|wp_idx| { + let t = wp_idx as f64 / denom * std::f64::consts::TAU; + Isometry2::new( + Vector2::new(cx + t.sin() * wiggle, cy + t.cos() * wiggle), + 0.0, + ) + }) + .collect(); + trajectories.push(Trajectory { poses }); + footprints.push(Arc::new(Ball::new(0.49)) as Arc); + } + MapfResult { + trajectories, + footprints, + discretization_timestep: 1.0, + agent_name_to_id: HashMap::default(), + } + } + + fn type2_edge_count(plan: &SemanticPlan) -> usize { + let mut count = 0; + for wp in &plan.waypoints { + if let Some(deps) = plan.comes_before(wp) { + count += deps + .iter() + .filter(|&&d| plan.waypoints[d].agent != wp.agent) + .count(); + } + } + count + } + + /// `grid_scene` never puts agents close enough to collide. + #[test] + fn test_grid_scene_has_no_collisions() { + for &num_waypoints in &[1, 10, 50] { + let scene = grid_scene(4, num_waypoints); // small n keeps brute-force fast + let bruteforce = type2_edge_count(&mapf_post_bruteforce(&scene)); + let sweep = type2_edge_count(&mapf_post_sweep(&scene)); + let aabb_tree = type2_edge_count(&mapf_post(&scene)); + assert_eq!( + bruteforce, 0, + "grid scene (num_waypoints={num_waypoints}) should have zero collisions by construction" + ); + assert_eq!( + sweep, bruteforce, + "sweep disagrees with brute-force on grid scene (num_waypoints={num_waypoints})" + ); + assert_eq!( + aabb_tree, bruteforce, + "aabb tree disagrees with brute-force on grid scene (num_waypoints={num_waypoints})" + ); + } + } +}