From c685a80a982a9cde615988bcdc9656e6e96b55fc Mon Sep 17 00:00:00 2001 From: Dustin Carlino Date: Thu, 30 Aug 2018 12:12:33 -0700 Subject: [PATCH] moving state to transit model, making router get directives from it --- docs/design.md | 21 +++++- sim/src/driving.rs | 1 + sim/src/init.rs | 7 +- sim/src/lib.rs | 9 +++ sim/src/router.rs | 115 ++++++++--------------------- sim/src/sim.rs | 10 +-- sim/src/spawn.rs | 64 ++++++++-------- sim/src/transit.rs | 172 ++++++++++++++++++++++++++++++++++++++++--- sim/tests/transit.rs | 13 ++-- 9 files changed, 272 insertions(+), 140 deletions(-) diff --git a/docs/design.md b/docs/design.md index 9762939628..11a705dba9 100644 --- a/docs/design.md +++ b/docs/design.md @@ -652,13 +652,32 @@ for now, since pathfinding ignores live traffic, probably fine to ignore this. - this actually belongs to the map layer! associated with a sidewalk I guess. - render the bus in a special color, and also, make it really long (adjust following dist, but not parking spot len) - how to unit test that a bus has reached a stop and is waiting? how do we even know that a bus is at a stop for peds to soon board it? I think a transit state will happen soon... + - step 2: make some peds pick a SINGLE bus to use for their route, if it helps + - step 3: make peds load on the bus and get off at the correct stop. make buses usually wait a fixed time at each stop, but wait a littl extra if loading passengers takes a while. - - will need to store transit state of what peds are on what bus somewhere... right? or can trips somehow do it? but will want to jump to a ped and spot the bus + - should walking state own peds waiting for a bus? + - yes: easier drawing, later need to know how crowded a sidewalk is, it's just weird to keep indicating we're at a place. router for cars does this, and the transit sim holds the higher-level state. do the same for now. + - no: transit sim can also contribute DrawPeds. the walking layer has nothing left to do with them... right? + - step N: load in GTFS for seattle to get real routes and stops later: multiple transfers, dedicated bus lanes, light rail... +Actually, jump to step 3 and just hardcode a ped to use a route, for now. what should the setup be? hardcode what stop to go to, what route to use, what stop to get off at? trip plan is a sequence... + +- walk to a sidewalk POI (bldg, parking spot, bus stop) +- drive somewhere and park +- ride bus route until some stop + +for now, these trip sequences can be hardcoded, and planned later. + +## Everything as FSMs + +Driving and walking layer are both kind of broken, since they know about +parking spots and bus stops. Feels like they need to be dumb, mechanical layers +that're guided by higher-level behaviors, which understand trips and such. + ## Routers, lookahead, cars as FSMs Hmm, hard to figure out the interactions for the router. when should it get a diff --git a/sim/src/driving.rs b/sim/src/driving.rs index e572dd91c7..1972a80393 100644 --- a/sim/src/driving.rs +++ b/sim/src/driving.rs @@ -175,6 +175,7 @@ impl Car { vehicle, map, parking_sim, + transit_sim, ); let dist_to_maybe_stop_at = maybe_stop_early.unwrap_or(current_on.length(map)); let dist_from_stop = dist_to_maybe_stop_at - current_dist_along; diff --git a/sim/src/init.rs b/sim/src/init.rs index e79753b313..9267c63ae1 100644 --- a/sim/src/init.rs +++ b/sim/src/init.rs @@ -36,13 +36,16 @@ pub fn small_spawn(sim: &mut Sim, map: &Map) { sim.seed_walking_trips(&map, 100); sim.seed_driving_trips(&map, 100); - sim.seed_bus( + if sim.seed_bus_route( vec![ map.get_l(LaneID(309)).bus_stops[0].clone(), map.get_l(LaneID(840)).bus_stops[0].clone(), ], map, - ).expect("Bus didn't fit"); + ).len() != 2 + { + panic!("Two buses didn't fit"); + } } pub fn big_spawn(sim: &mut Sim, map: &Map) { diff --git a/sim/src/lib.rs b/sim/src/lib.rs index 3161f98aab..ce5df303e8 100644 --- a/sim/src/lib.rs +++ b/sim/src/lib.rs @@ -62,6 +62,15 @@ impl fmt::Display for PedestrianID { } } +#[derive(Clone, Copy, Debug, Eq, Hash, PartialEq, PartialOrd, Ord, Serialize, Deserialize)] +pub struct RouteID(pub usize); + +impl fmt::Display for RouteID { + fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { + write!(f, "RouteID({0})", self.0) + } +} + #[derive(Serialize, Deserialize, PartialEq, Eq, PartialOrd, Ord, Clone, Copy, Debug, Hash)] pub enum AgentID { Car(CarID), diff --git a/sim/src/router.rs b/sim/src/router.rs index 479817fcb7..e07444a8cb 100644 --- a/sim/src/router.rs +++ b/sim/src/router.rs @@ -2,8 +2,7 @@ use dimensioned::si; use driving::{Action, CarView}; use kinematics; use kinematics::Vehicle; -use map_model; -use map_model::{BuildingID, BusStop, LaneID, Map, TurnID}; +use map_model::{BuildingID, LaneID, Map, TurnID}; use parking::ParkingSimState; use rand::Rng; use std::collections::VecDeque; @@ -13,11 +12,7 @@ use {Distance, Event, On, ParkingSpot, Tick}; #[derive(Clone, PartialEq, Eq, Serialize, Deserialize)] enum Goal { ParkNearBuilding(BuildingID), - // This is stateful -- first stop is the one we're aiming for now, and we might also be waiting - // at that stop for a little while. - // Buses really have a few states... GotoNextStop(list of stops), WaitAtStop(list of stops, - // tick) - CycleThroughStops(Vec, Option), + FollowBusRoute, } // Gives higher-level instructions to a car. @@ -37,11 +32,10 @@ impl Router { } } - pub fn make_router_for_bus(first_path: VecDeque, stops: Vec) -> Router { - assert_eq!(*first_path.back().unwrap(), stops[1].driving_lane); + pub fn make_router_for_bus(first_path: VecDeque) -> Router { Router { path: first_path, - goal: Goal::CycleThroughStops(rotate_stops(&stops), None), + goal: Goal::FollowBusRoute, } } @@ -64,73 +58,32 @@ impl Router { transit_sim: &mut TransitSimState, rng: &mut R, ) -> Option { - if self.path.is_empty() && view.speed <= kinematics::EPSILON_SPEED { - match self.goal { - Goal::ParkNearBuilding(_) => { - let last_lane = view.on.as_lane(); - if let Some(spot) = - find_parking_spot(last_lane, view.dist_along, map, parking_sim) - { - if parking_sim.dist_along_for_car(spot, vehicle) == view.dist_along { - return Some(Action::StartParking(spot)); - } - // Being stopped before the parking spot is normal if the final road is - // clogged with other drivers. - } else { - return self.look_for_parking(last_lane, view, map, rng); + if !self.path.is_empty() || view.speed > kinematics::EPSILON_SPEED { + return None; + } + + match self.goal { + Goal::ParkNearBuilding(_) => { + let last_lane = view.on.as_lane(); + if let Some(spot) = find_parking_spot(last_lane, view.dist_along, map, parking_sim) + { + if parking_sim.dist_along_for_car(spot, vehicle) == view.dist_along { + return Some(Action::StartParking(spot)); } + // Being stopped before the parking spot is normal if the final road is + // clogged with other drivers. + } else { + return self.look_for_parking(last_lane, view, map, rng); } - Goal::CycleThroughStops(ref mut stops, ref mut wait_until) => { - if view.dist_along == stops[0].dist_along { - if let Some(wait) = wait_until.clone() { - if time == wait { - // TODO is this the right place to actually do the state - // transition, or should we just indicate an event and let - // something else handle it? - events.push(Event::BusDepartedFromStop(view.id, stops[0].clone())); - - if view.debug || true { - println!( - "{} finished waiting at bus stop, going to next stop {:?}", - view.id, stops[1] - ); - } - - let mut new_path = VecDeque::from( - map_model::pathfind( - map, - stops[0].driving_lane, - stops[1].driving_lane, - ).expect(&format!( - "No route between bus stops {:?} and {:?}", - stops[0], stops[1] - )), - ); - new_path.pop_front(); - self.path = new_path; - - let new_stops = rotate_stops(stops); - stops.clear(); - stops.extend(new_stops); - *wait_until = None; - - transit_sim.bus_is_driving(view.id); - } - } else { - assert_eq!(view.on.as_lane(), stops[0].driving_lane); - events.push(Event::BusArrivedAtStop(view.id, stops[0].clone())); - // TODO const - *wait_until = Some(time + 10.0 * si::S); - if view.debug || true { - println!( - "{} reached {:?}, now waiting until {:?}", - view.id, stops[0], wait_until - ); - } - transit_sim.bus_is_at_stop(view.id, stops[0].clone()); - return Some(Action::Continue(0.0 * si::MPS2, Vec::new())); - } - } + } + Goal::FollowBusRoute => { + let (should_idle, new_path) = + transit_sim.get_action_when_stopped_at_end(events, view, time, map); + if let Some(p) = new_path { + self.path = p; + } + if should_idle { + return Some(Action::Continue(0.0 * si::MPS2, Vec::new())); } } } @@ -146,6 +99,7 @@ impl Router { vehicle: &Vehicle, map: &Map, parking_sim: &ParkingSimState, + transit_sim: &TransitSimState, ) -> Option { if self.path.is_empty() { match self.goal { @@ -160,8 +114,8 @@ impl Router { return Some(on.length(map)); } } - Goal::CycleThroughStops(ref stops, _) => { - return Some(stops[0].dist_along); + Goal::FollowBusRoute => { + return Some(transit_sim.get_dist_to_stop_at(vehicle.id, on.as_lane())); } } } @@ -221,10 +175,3 @@ fn find_parking_spot( .find_parking_lane(driving_lane) .and_then(|l| parking_sim.get_first_free_spot(l, dist_along)) } - -// TODO double-ended list can do this natively -fn rotate_stops(stops: &Vec) -> Vec { - let mut cycled_stops: Vec = stops[1..].to_vec(); - cycled_stops.push(stops[0].clone()); - cycled_stops -} diff --git a/sim/src/sim.rs b/sim/src/sim.rs index 385eb5a262..db23dd0e60 100644 --- a/sim/src/sim.rs +++ b/sim/src/sim.rs @@ -115,10 +115,11 @@ impl Sim { } } - pub fn seed_bus(&mut self, stops: Vec, map: &Map) -> Option { + pub fn seed_bus_route(&mut self, stops: Vec, map: &Map) -> Vec { // TODO throw away the events? :( let mut events: Vec = Vec::new(); - if let Some(v) = self.spawner.seed_bus( + let mut result: Vec = Vec::new(); + for v in self.spawner.seed_bus_route( &mut events, stops, &mut self.rng, @@ -130,10 +131,9 @@ impl Sim { ) { let id = v.id; self.car_properties.insert(v.id, v); - Some(id) - } else { - None + result.push(id); } + result } pub fn seed_specific_parked_cars(&mut self, lane: LaneID, spots: Vec) -> Vec { diff --git a/sim/src/spawn.rs b/sim/src/spawn.rs index 393641b24c..4aa2247885 100644 --- a/sim/src/spawn.rs +++ b/sim/src/spawn.rs @@ -168,7 +168,7 @@ impl Spawner { } // This happens immediately; it isn't scheduled. - pub fn seed_bus( + pub fn seed_bus_route( &mut self, events: &mut Vec, stops: Vec, @@ -178,38 +178,40 @@ impl Spawner { transit_sim: &mut TransitSimState, now: Tick, properties: &BTreeMap, - ) -> Option { - assert!(stops.len() > 1); - let id = CarID(self.car_id_counter); - self.car_id_counter += 1; - let vehicle = Vehicle::generate_bus(id, rng); + ) -> Vec { + let route = transit_sim.create_empty_route(stops); + let mut vehicles: Vec = Vec::new(); + // Try to spawn a bus at each stop + for (next_stop_idx, start_dist_along, mut path) in + transit_sim.get_route_starts(route, map).into_iter() + { + let id = CarID(self.car_id_counter); + self.car_id_counter += 1; + let vehicle = Vehicle::generate_bus(id, rng); - let mut first_path = VecDeque::from( - map_model::pathfind(map, stops[0].driving_lane, stops[1].driving_lane).expect( - &format!( - "No route between bus stops {:?} and {:?}", - stops[0], stops[1] - ), - ), - ); - let start = first_path.pop_front().unwrap(); - if driving_sim.start_car_on_lane( - events, - now, - id, - None, - stops[0].dist_along, - start, - Router::make_router_for_bus(first_path, stops), - map, - properties, - ) { - transit_sim.bus_is_driving(id); - println!("Spawned bus {}", id); - return Some(vehicle); + let start = path.pop_front().unwrap(); + if driving_sim.start_car_on_lane( + events, + now, + id, + None, + start_dist_along, + start, + Router::make_router_for_bus(path), + map, + properties, + ) { + transit_sim.bus_created(id, route, next_stop_idx); + println!("Spawned bus {} for route {}", id, route); + vehicles.push(vehicle); + } else { + println!( + "No room for a bus headed towards stop {} of {}, giving up", + next_stop_idx, route + ); + } } - println!("No room for bus, giving up"); - None + vehicles } // This happens immediately; it isn't scheduled. diff --git a/sim/src/transit.rs b/sim/src/transit.rs index 5d1d1a86c8..735461a885 100644 --- a/sim/src/transit.rs +++ b/sim/src/transit.rs @@ -1,31 +1,181 @@ -use map_model::BusStop; -use std::collections::BTreeMap; -use CarID; +use dimensioned::si; +use driving::CarView; +use events::Event; +use map_model; +use map_model::{BusStop, LaneID, Map}; +use std::collections::{BTreeMap, VecDeque}; +use {CarID, Distance, PedestrianID, RouteID, Tick}; + +type StopIdx = usize; + +#[derive(Serialize, Deserialize, PartialEq, Eq)] +struct Route { + id: RouteID, + buses: Vec, + stops: Vec, + // TODO info on schedules +} + +impl Route { + fn next_stop(&self, idx: StopIdx) -> StopIdx { + if idx + 1 == self.stops.len() { + 0 + } else { + idx + 1 + } + } +} + +#[derive(Serialize, Deserialize, PartialEq, Eq)] +struct Bus { + car: CarID, + route: RouteID, + passengers: Vec, + state: BusState, +} #[derive(Serialize, Deserialize, PartialEq, Eq)] enum BusState { - Driving, - AtStop(BusStop), + DrivingToStop(StopIdx), + // When do we leave? + AtStop(StopIdx, Tick), } #[derive(Serialize, Deserialize, PartialEq, Eq)] pub struct TransitSimState { - buses: BTreeMap, + buses: BTreeMap, + routes: BTreeMap, } impl TransitSimState { pub fn new() -> TransitSimState { TransitSimState { buses: BTreeMap::new(), + routes: BTreeMap::new(), } } - // Transitions - pub fn bus_is_driving(&mut self, bus: CarID) { - self.buses.insert(bus, BusState::Driving); + pub fn create_empty_route(&mut self, stops: Vec) -> RouteID { + assert!(stops.len() > 1); + let id = RouteID(self.routes.len()); + self.routes.insert( + id, + Route { + id, + buses: Vec::new(), + stops: stops.clone(), + }, + ); + id } - pub fn bus_is_at_stop(&mut self, bus: CarID, stop: BusStop) { - self.buses.insert(bus, BusState::AtStop(stop)); + // (next stop, start distance, first path) + pub fn get_route_starts( + &self, + id: RouteID, + map: &Map, + ) -> Vec<(StopIdx, Distance, VecDeque)> { + let route = &self.routes[&id]; + route + .stops + .iter() + .enumerate() + .map(|(idx, stop1)| { + let next_stop = route.next_stop(idx); + let stop2 = &route.stops[next_stop]; + let path = VecDeque::from( + map_model::pathfind(map, stop1.driving_lane, stop2.driving_lane).expect( + &format!("No route between bus stops {:?} and {:?}", stop1, stop2), + ), + ); + (next_stop, stop1.dist_along, path) + }) + .collect() + } + + pub fn bus_created(&mut self, bus: CarID, route: RouteID, next_stop_idx: StopIdx) { + self.routes.get_mut(&route).unwrap().buses.push(bus); + self.buses.insert( + bus, + Bus { + car: bus, + route, + passengers: Vec::new(), + state: BusState::DrivingToStop(next_stop_idx), + }, + ); + } + + // Returns (should idle, new path) + pub fn get_action_when_stopped_at_end( + &mut self, + events: &mut Vec, + view: &CarView, + time: Tick, + map: &Map, + ) -> (bool, Option>) { + let route = &self.routes[&self.buses[&view.id].route]; + match self.buses[&view.id].state { + BusState::DrivingToStop(stop_idx) => { + let stop = &route.stops[stop_idx]; + assert_eq!(stop.driving_lane, view.on.as_lane()); + if stop.dist_along == view.dist_along { + // TODO constant for stop time + self.buses.get_mut(&view.id).unwrap().state = + BusState::AtStop(stop_idx, time + 10.0 * si::S); + events.push(Event::BusArrivedAtStop(view.id, stop.clone())); + if view.debug { + println!("{} arrived at stop {:?}, now waiting", view.id, stop); + } + return (true, None); + } + // No, keep creeping forwards + (false, None) + } + BusState::AtStop(stop_idx, wait_until) => { + let stop = &route.stops[stop_idx]; + assert_eq!(stop.driving_lane, view.on.as_lane()); + assert_eq!(stop.dist_along, view.dist_along); + + if time == wait_until { + let next_stop = route.next_stop(stop_idx); + self.buses.get_mut(&view.id).unwrap().state = + BusState::DrivingToStop(next_stop); + events.push(Event::BusDepartedFromStop(view.id, stop.clone())); + if view.debug { + println!("{} departing from stop {:?}", view.id, stop); + } + + let mut new_path = VecDeque::from( + map_model::pathfind( + map, + stop.driving_lane, + route.stops[next_stop].driving_lane, + ).expect(&format!( + "No route between bus stops {:?} and {:?}", + stop, route.stops[next_stop] + )), + ); + new_path.pop_front(); + + return (true, Some(new_path)); + } + + (true, None) + } + } + } + + pub fn get_dist_to_stop_at(&self, bus: CarID, driving_lane: LaneID) -> Distance { + match self.buses[&bus].state { + BusState::DrivingToStop(stop_idx) => { + let stop = &self.routes[&self.buses[&bus].route].stops[stop_idx]; + assert_eq!(stop.driving_lane, driving_lane); + stop.dist_along + } + BusState::AtStop(_, _) => { + panic!("Shouldn't ask where to stop if the bus is already at a stop") + } + } } } diff --git a/sim/tests/transit.rs b/sim/tests/transit.rs index 425e105629..456c01d20b 100644 --- a/sim/tests/transit.rs +++ b/sim/tests/transit.rs @@ -14,18 +14,19 @@ fn bus_reaches_stops() { let stop1 = map.get_l(map_model::LaneID(309)).bus_stops[0].clone(); let stop2 = map.get_l(map_model::LaneID(840)).bus_stops[0].clone(); - let bus = sim.seed_bus(vec![stop1.clone(), stop2.clone()], &map) - .unwrap(); + let buses = sim.seed_bus_route(vec![stop1.clone(), stop2.clone()], &map); + let (bus1, bus2) = (buses[0], buses[1]); sim::init::run_until_expectations_met( &mut sim, &map, &control_map, + // TODO assert stuff about bus2 as well, although the timing is a little unclear vec![ - sim::Event::BusArrivedAtStop(bus, stop2.clone()), - sim::Event::BusDepartedFromStop(bus, stop2), - sim::Event::BusArrivedAtStop(bus, stop1.clone()), - sim::Event::BusDepartedFromStop(bus, stop1), + sim::Event::BusArrivedAtStop(bus1, stop2.clone()), + sim::Event::BusDepartedFromStop(bus1, stop2), + sim::Event::BusArrivedAtStop(bus1, stop1.clone()), + sim::Event::BusDepartedFromStop(bus1, stop1), ], sim::Tick::from_minutes(10), );