diff --git a/examples/discrete_vs_nodiscrete.rs b/examples/discrete_vs_nodiscrete.rs index dda68a8..bd09f03 100644 --- a/examples/discrete_vs_nodiscrete.rs +++ b/examples/discrete_vs_nodiscrete.rs @@ -108,7 +108,11 @@ fn main() { FixedTimeSATSolver::without_optimality_check(soln.clone()); let optimality_proof_dur = timer.elapsed(); - let sat_flexible_time_problem = sat_flexible_time_model::Problem { requests, dependencies: vec![] , one_of_dependencies: vec![]}; + let sat_flexible_time_problem = sat_flexible_time_model::Problem { + requests, + dependencies: vec![], + one_of_dependencies: vec![], + }; let stop = Arc::new(AtomicBool::new(false)); diff --git a/examples/end_to_end_planner.rs b/examples/end_to_end_planner.rs index 6fda55a..1cefaae 100644 --- a/examples/end_to_end_planner.rs +++ b/examples/end_to_end_planner.rs @@ -1,17 +1,21 @@ -use std::{ops::Index, sync::atomic::AtomicBool}; use chrono::Duration; +use std::{ops::Index, sync::atomic::AtomicBool}; use futures::pending; /// This example show cases how to use a* for accounting for optimal parking spot allocation /// The core idea is we have a set of robots that need to traverse an area. The goal is to pick up an item /// and then have it delivered to a certain location. -/// +/// /// Along the way we may place a door which serves as a bottle neck. use pathfinding::prelude::astar; -use rmf_reservations::{algorithms::sat_flexible_time_model::{Problem, SATFlexibleTimeModel}, cost_function::static_cost, database::DefaultUtcClock, ReservationParameters, ReservationRequestAlternative, StartTimeRange}; - -fn test_scenario() -> String -{ +use rmf_reservations::{ + algorithms::sat_flexible_time_model::{Problem, SATFlexibleTimeModel}, + cost_function::static_cost, + database::DefaultUtcClock, + ReservationParameters, ReservationRequestAlternative, StartTimeRange, +}; + +fn test_scenario() -> String { // Goal is for robot to transport i to I and j to J, with parking spots // "p" using robots 1 and 2. @@ -28,57 +32,68 @@ fn test_scenario() -> String room.to_string() } -fn find_character_coordinate(map: &Vec, character: &String) -> Option<(usize, usize)> -{ - map.iter().enumerate() - .map(|(idx, f)| {(f.find(character), idx)}) - .fold(None, |num, res| if res.0 == None { num } else { - if let Some(row) = res.0 { - Some((res.1, row)) - } - else { - num - } - }) +fn find_character_coordinate(map: &Vec, character: &String) -> Option<(usize, usize)> { + map.iter() + .enumerate() + .map(|(idx, f)| (f.find(character), idx)) + .fold(None, |num, res| { + if res.0 == None { + num + } else { + if let Some(row) = res.0 { + Some((res.1, row)) + } else { + num + } + } + }) } -fn transform_to_map(map: String) -> Vec -{ - map.split("\n").filter(|p| p.len() > 0).map(|f| f.replace(" ", "").to_string()).collect() +fn transform_to_map(map: String) -> Vec { + map.split("\n") + .filter(|p| p.len() > 0) + .map(|f| f.replace(" ", "").to_string()) + .collect() } - -fn display_path(path: &Vec<(usize, usize)>, m: &Vec) -{ +fn display_path(path: &Vec<(usize, usize)>, m: &Vec) { let mut m = m.clone(); for p in path { - m[p.0].replace_range(p.1..p.1+1, "*"); + m[p.0].replace_range(p.1..p.1 + 1, "*"); } - for line in m{ + for line in m { println!("{:?}", line); } } -fn astar_cost_estimate(start_goal: (usize, usize), end_goal: (usize, usize), m: &Vec) -> Option<(Vec<(usize, usize)>, usize)> -{ - let opts = vec![(0,1), (1,0), (0,-1), (-1,0)]; - astar(&start_goal, |node| { - let v:Vec<_> = opts.iter() - .map(|(dx, dy)| (node.0 as i32 + dx.clone(), node.1 as i32 + dy.clone())) - .filter(|(x,y)| *x >= 0 && *y >= 0 && *x < m.len() as i32 && *y < m[0].len() as i32) - .filter(|(x,y)| m[*x as usize].bytes().nth(*y as usize).unwrap() != b'x') - .collect(); - v.into_iter().map(|(x,y)| ((x as usize, y as usize), 1)) }, - |p| p.0.abs_diff(end_goal.0) + p.1.abs_diff(end_goal.1), - |f| *f == end_goal) +fn astar_cost_estimate( + start_goal: (usize, usize), + end_goal: (usize, usize), + m: &Vec, +) -> Option<(Vec<(usize, usize)>, usize)> { + let opts = vec![(0, 1), (1, 0), (0, -1), (-1, 0)]; + astar( + &start_goal, + |node| { + let v: Vec<_> = opts + .iter() + .map(|(dx, dy)| (node.0 as i32 + dx.clone(), node.1 as i32 + dy.clone())) + .filter(|(x, y)| { + *x >= 0 && *y >= 0 && *x < m.len() as i32 && *y < m[0].len() as i32 + }) + .filter(|(x, y)| m[*x as usize].bytes().nth(*y as usize).unwrap() != b'x') + .collect(); + v.into_iter().map(|(x, y)| ((x as usize, y as usize), 1)) + }, + |p| p.0.abs_diff(end_goal.0) + p.1.abs_diff(end_goal.1), + |f| *f == end_goal, + ) } -fn ototot() -{ +fn ototot() { use std::sync::Arc; - let current_time = chrono::Utc::now(); let n = 60usize; @@ -89,7 +104,7 @@ fn ototot() parameters: ReservationParameters { resource_name: "Resource1".to_string(), duration: Some(task_dur), - start_time:StartTimeRange { + start_time: StartTimeRange { earliest_start: Some(current_time), latest_start: Some(current_time + task_dur * (n as i32 + 1)), }, @@ -98,7 +113,11 @@ fn ototot() }]); } - let problem = Problem { requests, one_of_dependencies: vec![], dependencies: vec![] }; + let problem = Problem { + requests, + one_of_dependencies: vec![], + dependencies: vec![], + }; let stop = Arc::new(AtomicBool::new(false)); /*let model = SATFlexibleTimeModel { @@ -106,17 +125,15 @@ fn ototot() } .time_optimality_solver(&problem, stop); let result = model.unwrap();*/ - } -fn main() -{ +fn main() { let m = transform_to_map(test_scenario()); let Some(end_goal) = find_character_coordinate(&m, &"i".to_string()) else { panic!("couldnt find start"); }; - let Some(start_goal) = find_character_coordinate(&m, &"I".to_string()) else { + let Some(start_goal) = find_character_coordinate(&m, &"I".to_string()) else { panic!("could not find end"); }; @@ -126,4 +143,4 @@ fn main() //let mut problem = Problem {} //display_path(&res.unwrap().0, &m); -} \ No newline at end of file +} diff --git a/src/algorithms/sat_flexible_time_model.rs b/src/algorithms/sat_flexible_time_model.rs index 1f4d10e..c65fc31 100644 --- a/src/algorithms/sat_flexible_time_model.rs +++ b/src/algorithms/sat_flexible_time_model.rs @@ -27,7 +27,7 @@ pub struct Problem { pub one_of_dependencies: Vec>, /// Dependency of the form (a, b) where a is the request id and b is the alternative id. - pub dependencies: Vec<((usize, usize), (usize, usize))> + pub dependencies: Vec<((usize, usize), (usize, usize))>, } impl Problem { @@ -38,13 +38,17 @@ impl Problem { return self.requests.len() - 1; } - pub fn implies(&mut self, a: &(usize, usize), b: &(usize, usize)) -> Result<(),String> { - if a.0 >= self.requests.len() || a.1 >= self.requests[a.0].len() || b.0 >= self.requests.len() || b.1 >= self.requests[b.0].len() { - return Err("Request and alternative was not found.".to_string()) + pub fn implies(&mut self, a: &(usize, usize), b: &(usize, usize)) -> Result<(), String> { + if a.0 >= self.requests.len() + || a.1 >= self.requests[a.0].len() + || b.0 >= self.requests.len() + || b.1 >= self.requests[b.0].len() + { + return Err("Request and alternative was not found.".to_string()); } - self.dependencies.push((*a,*b)); + self.dependencies.push((*a, *b)); Ok(()) - } + } } /// Snapshot of a solution. A solved schedule contains a list of assingments for each resource @@ -244,7 +248,7 @@ impl SATFlexibl } } - for dep in problem.dependencies.iter() { + for dep in problem.dependencies.iter() { let (x1, x2) = dep; let Some(x_ij) = var_list.get(x1) else { panic!("Could not find variable"); @@ -364,7 +368,7 @@ impl SATFlexibl final_schedule.clear(); - // Shrink the time window. Recalculate + // Shrink the time window. Recalculate if let Some(time_window) = time_window { println!("Attempting shrink"); let mut formula = varisat::CnfFormula::new(); @@ -403,8 +407,10 @@ impl SATFlexibl let X_ijkm = list_ij.get(&alt_km).expect(""); let Some(list_km) = comes_after_vars.get(&alt_km) else { - panic!("For some reason unable to get comes after vars - "); + panic!( + "For some reason unable to get comes after vars + " + ); }; let X_kmij = list_km.get(&alt_ij).expect(""); @@ -519,12 +525,11 @@ impl SATFlexibl let alternative = &problem.requests[sched[i].0][sched[i].1]; let Some(duration) = alternative.parameters.duration else { - if i + 1 == sched.len() { + if i + 1 < sched.len() { if let Some(latest) = alternative.parameters.start_time.latest_start { if last_reservation_end > latest { - // Add a banning of this specific ordering [Exponential bomb if ordering is too long] - for (j,k) in (last_gap..i).tuple_windows() { + for (j, k) in (last_gap..i).tuple_windows() { let j_id = sched[j]; let Some(vars) = comes_after_vars.get(&j_id) else { continue; @@ -532,11 +537,15 @@ impl SATFlexibl let mut transitive_pairs = vec![]; - for (id, j_var )in vars.iter() { - if *id == sched[j] || *id ==sched[k] { + for (id, j_var) in vars.iter() { + if *id == sched[j] || *id == sched[k] { continue; } - if problem.requests[id.0][id.1].parameters.resource_name != problem.requests[j_id.0][j_id.1].parameters.resource_name { + if problem.requests[id.0][id.1].parameters.resource_name + != problem.requests[j_id.0][j_id.1] + .parameters + .resource_name + { continue; } let Some(other) = comes_after_vars.get(&id) else { @@ -561,20 +570,20 @@ impl SATFlexibl for y in 0..transitive_pairs.len() { if (1 << y) & x != 0 { clause.push(transitive_pairs[y].0); - } - else { + } else { clause.push(transitive_pairs[y].1); } } - let mut formula = vec![Lit::from_var(*not_allowed_next, false)]; + let mut formula = + vec![Lit::from_var(*not_allowed_next, false)]; for i in clause { formula.push(Lit::from_var(*i, true)); } learned_clauses.push(formula); } } - + /*let mut formula = vec![]; for j in last_gap..i { let v = var_list @@ -583,6 +592,10 @@ impl SATFlexibl formula.push(Lit::from_var(*v, false)); }*/ //learned_clauses.push(formula); + } else { + if last_reservation_end != latest { + last_gap = i; + } } } continue; @@ -632,7 +645,9 @@ impl SATFlexibl println!("learned_clauses {:?}", learned_clauses.len()); if learned_clauses.len() == 0 { - sender.send(AlgorithmState::FeasibleScheduleSolution(final_schedule.clone())); + sender.send(AlgorithmState::FeasibleScheduleSolution( + final_schedule.clone(), + )); } for clause in learned_clauses { @@ -650,11 +665,13 @@ impl SATFlexibl }) .max(); // We also don't want the same solution - let banned_assignment: Vec<_> = final_schedule.iter() - .map(|(_,assignment)| assignment.iter()).flatten() - .map(|p| Lit::from_var(var_list[&p.id], false)).collect(); + let banned_assignment: Vec<_> = final_schedule + .iter() + .map(|(_, assignment)| assignment.iter()) + .flatten() + .map(|p| Lit::from_var(var_list[&p.id], false)) + .collect(); solver.add_clause(&banned_assignment); - } else { println!("Could not solve"); ok = false; @@ -1079,9 +1096,7 @@ fn test_multi_item_sat_solver() { for t in rx.iter() { println!("{:?}", t) } -} - - +} #[cfg(test)] #[test] @@ -1109,7 +1124,7 @@ fn test_flexible_one_item_sat_solver() { let problem = Problem { requests: vec![req1], one_of_dependencies: vec![], - dependencies: vec![] + dependencies: vec![], }; let stop = Arc::new(AtomicBool::new(false)); @@ -1174,7 +1189,7 @@ fn test_flexible_two_items_sat_solver() { let problem = Problem { requests: vec![req1, req2], one_of_dependencies: vec![], - dependencies: vec![] + dependencies: vec![], }; let stop = Arc::new(AtomicBool::new(false)); @@ -1220,7 +1235,11 @@ fn test_flexible_n_items_sat_solver() { }]); } - let problem = Problem { requests, dependencies: vec![], one_of_dependencies: vec![] }; + let problem = Problem { + requests, + dependencies: vec![], + one_of_dependencies: vec![], + }; let stop = Arc::new(AtomicBool::new(false)); let model = SATFlexibleTimeModel { @@ -1265,7 +1284,11 @@ fn test_flexible_no_soln_sat_solver() { }]); } - let problem = Problem { requests, one_of_dependencies: vec![], dependencies: vec![] }; + let problem = Problem { + requests, + one_of_dependencies: vec![], + dependencies: vec![], + }; let stop = Arc::new(AtomicBool::new(false)); let model = SATFlexibleTimeModel { diff --git a/src/database/mod.rs b/src/database/mod.rs index 905454a..f7070c2 100644 --- a/src/database/mod.rs +++ b/src/database/mod.rs @@ -374,10 +374,11 @@ impl