From c033f51da22f6c898810ee933879786f96d014fd Mon Sep 17 00:00:00 2001 From: Dustin Carlino Date: Sun, 8 Jul 2018 14:52:47 -0700 Subject: [PATCH] make cars pathfind to their destination --- docs/TODO_phase3.md | 3 +- editor/src/ui.rs | 2 +- map_model/src/pathfind.rs | 6 ++-- sim/src/straw_model.rs | 70 +++++++++++++++++++++++++++++---------- 4 files changed, 56 insertions(+), 25 deletions(-) diff --git a/docs/TODO_phase3.md b/docs/TODO_phase3.md index 5d5debee65..c3ae60726f 100644 --- a/docs/TODO_phase3.md +++ b/docs/TODO_phase3.md @@ -2,11 +2,10 @@ ## cars -- make cars pathfind to their destination - - model cars parking - maybe render numbers on the cars to distinguish them - document the FSM (on lane driving, waiting, turning, parking, etc) + - populate a bunch of parked cars initially - try to simplify straw_model step (less phases?) diff --git a/editor/src/ui.rs b/editor/src/ui.rs index d50dee0e75..267aa2177d 100644 --- a/editor/src/ui.rs +++ b/editor/src/ui.rs @@ -436,7 +436,7 @@ impl gui::GUI for UI { if self.map.get_r(id).lane_type == map_model::LaneType::Driving { if input.key_pressed(Key::A, "Press A to add a car starting from this road") { - if !self.sim_ctrl.sim.spawn_one_on_road(id) { + if !self.sim_ctrl.sim.spawn_one_on_road(&self.map, id) { println!("No room, sorry"); } return gui::EventLoopMode::InputOnly; diff --git a/map_model/src/pathfind.rs b/map_model/src/pathfind.rs index a012ab3fc7..8c1b48a570 100644 --- a/map_model/src/pathfind.rs +++ b/map_model/src/pathfind.rs @@ -24,15 +24,13 @@ pub fn pathfind(map: &Map, start: RoadID, end: RoadID) -> Option> { let mut lookup = current; loop { path.push(lookup); - if let Some(next) = backrefs.get(&lookup) { - lookup = *next; - } else { - assert!(lookup == start); + if lookup == start { path.reverse(); assert_eq!(path[0], start); assert_eq!(*path.last().unwrap(), end); return Some(path); } + lookup = backrefs[&lookup]; } } diff --git a/sim/src/straw_model.rs b/sim/src/straw_model.rs index 195dc66ae0..8dceb1f496 100644 --- a/sim/src/straw_model.rs +++ b/sim/src/straw_model.rs @@ -6,11 +6,12 @@ use ezgui::GfxCtx; use geom::{Angle, Pt2D}; use graphics; use graphics::math::Vec2d; +use map_model; use map_model::geometry; use map_model::{LaneType, Map, RoadID, TurnID}; use multimap::MultiMap; use rand::{FromEntropy, Rng, SeedableRng, XorShiftRng}; -use std::collections::{BTreeMap, HashSet}; +use std::collections::{BTreeMap, HashSet, VecDeque}; use std::f64; use std::time::{Duration, Instant}; use straw_intersections::{IntersectionPolicy, StopSign, TrafficSignal}; @@ -122,6 +123,8 @@ struct Car { // TODO ideally, something else would remember Goto was requested and not even call step() waiting_for: Option, debug: bool, + // Head is the next road + path: VecDeque, } enum Action { @@ -137,10 +140,11 @@ impl Car { format!("Car {:?}", self.id), format!("On {:?}, started at {:?}", self.on, self.started_at), format!("Committed to waiting for {:?}", self.waiting_for), + format!("{} roads left in path", self.path.len()), ] } - fn step(&self, map: &Map, time: Tick, rng: &mut XorShiftRng) -> Action { + fn step(&self, map: &Map, time: Tick) -> Action { if let Some(on) = self.waiting_for { return Action::Goto(on); } @@ -150,21 +154,29 @@ impl Car { return Action::Continue; } + // Done! + if self.path.is_empty() { + return Action::Vanish; + } + match self.on { - // For now, just kill off cars that're stuck on disconnected bits of the map - On::Road(id) if map.get_turns_from_road(id).is_empty() => Action::Vanish, // TODO cant try to go to next road unless we're the front car // if we dont do this here, we wont be able to see what turns people are waiting for // even if we wait till we're the front car, we might unravel the line of queued cars // too quickly - On::Road(id) => Action::Goto(On::Turn(self.choose_turn(id, map, rng))), + On::Road(id) => Action::Goto(On::Turn(self.choose_turn(id, map))), On::Turn(id) => Action::Goto(On::Road(map.get_t(id).dst)), } } - fn choose_turn(&self, from: RoadID, map: &Map, rng: &mut XorShiftRng) -> TurnID { + fn choose_turn(&self, from: RoadID, map: &Map) -> TurnID { assert!(self.waiting_for.is_none()); - rng.choose(&map.get_turns_from_road(from)).unwrap().id + for t in map.get_turns_from_road(from) { + if t.dst == self.path[0] { + return t.id; + } + } + panic!("No turn from {} to {}", from, self.path[0]); } // Returns the angle and the dist along the road/turn too @@ -312,6 +324,7 @@ impl Sim { intersections, cars: BTreeMap::new(), + // TODO only driving ones roads: map.all_roads() .iter() .map(|r| SimQueue::new(On::Road(r.id), map)) @@ -329,23 +342,39 @@ impl Sim { // TODO cars basically start in the intersection, with their front bumper right at the // beginning of the road. later, we want cars starting at arbitrary points in the middle of the // road (from a building), so just ignore this problem for now. - pub fn spawn_one_on_road(&mut self, road: RoadID) -> bool { - if !self.roads[road.0].room_at_end(self.time, &self.cars) { + pub fn spawn_one_on_road(&mut self, map: &Map, start: RoadID) -> bool { + if !self.roads[start.0].room_at_end(self.time, &self.cars) { return false; } let id = CarID(self.id_counter); self.id_counter += 1; + + let goal = self.rng.choose(map.all_roads()).unwrap(); + if goal.lane_type != LaneType::Driving || goal.id == start { + println!("Chose bad goal {}", goal.id); + return false; + } + let mut path = if let Some(steps) = map_model::pathfind(map, start, goal.id) { + VecDeque::from(steps) + } else { + println!("No path from {} to {}", start, goal.id); + return false; + }; + // path includes the start, but that's not the invariant Car enforces + path.pop_front(); + self.cars.insert( id, Car { id, + path, started_at: self.time, - on: On::Road(road), + on: On::Road(start), waiting_for: None, debug: false, }, ); - self.roads[road.0].cars_queue.push(id); + self.roads[start.0].cars_queue.push(id); true } @@ -366,19 +395,22 @@ impl Sim { } let n = num_cars.min(roads.len()); + let mut actual = 0; for i in 0..n { - assert!(self.spawn_one_on_road(roads[i])); + if self.spawn_one_on_road(map, roads[i]) { + actual += 1; + } } - println!("Spawned {}", n); + println!("Spawned {} of {}", actual, n); } pub fn step(&mut self, map: &Map, control_map: &ControlMap) { self.time.increment(); - // Could be concurrent. Ask all cars for their move, reinterpreting Goto to see if there's - // room now. It's important to query has_room_now here using the previous, fixed state of - // the world. If we did it in the next loop, then order of updates would matter for more - // than just conflict resolution. + // Could be concurrent, since this is deterministic. Note no RNG. Ask all cars for their + // move, reinterpreting Goto to see if there's room now. It's important to query + // has_room_now here using the previous, fixed state of the world. If we did it in the next + // loop, then order of updates would matter for more than just conflict resolution. // // Note that since this uses RNG right now, it's only deterministic if iteration order is! // So can't be concurrent and use RNG. Could have a RNG per car or something later if we @@ -387,7 +419,7 @@ impl Sim { for c in self.cars.values() { requested_moves.push(( c.id, - match c.step(map, self.time, &mut self.rng) { + match c.step(map, self.time) { Action::Goto(on) => { // This is a monotonic property in conjunction with // new_car_entered_this_step. The last car won't go backwards. @@ -444,6 +476,8 @@ impl Sim { let c = self.cars.get_mut(&id).unwrap(); if let On::Turn(t) = c.on { self.intersections[map.get_t(t).parent.0].on_exit(c.id); + assert_eq!(c.path[0], map.get_t(t).dst); + c.path.pop_front(); } c.waiting_for = None; c.on = on;