diff --git a/mapf/Cargo.toml b/mapf/Cargo.toml index 6920707..64586c0 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.33" time-point = "0.1" sorted-vec = "0.8.2" cached = "0.40" @@ -27,6 +27,11 @@ smallvec = "1.10" serde = { version="1.0", features = ["derive"] } serde_yaml = "0.9" slotmap = "1.0" +parry2d = { version = "0.21", package = "parry2d-f64" } +petgraph = "0.6" +bresenham = "0.1.1" +rand = "0.8" +csv = "1.1" [dev-dependencies] approx = "0.5" 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/negotiation/mod.rs b/mapf/src/negotiation/mod.rs index 49f7f78..3f85c4e 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}, @@ -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) }; @@ -653,6 +654,90 @@ impl NegotiationNode { } } +impl Scenario { + pub fn solve( + &mut 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()); + } + + 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(); + + 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 }); + } + + 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, + 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, 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); + } +} 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) + } +}