From 081cac761695e08ded4468e7dfd3de1a6accd8cb Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 6 Mar 2026 09:04:26 +0000 Subject: [PATCH 01/10] 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/10] 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/10] 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/10] 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/10] 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/10] 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/10] 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/10] 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/10] 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/10] 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); + } +}