From fbcc2a90941101cdca99105a10cb198886fd7ce4 Mon Sep 17 00:00:00 2001 From: Dustin Carlino Date: Wed, 5 Sep 2018 12:37:57 -0700 Subject: [PATCH] unifying WorldView and AgentInfo --- docs/TODO_phase3.md | 6 +++ sim/src/driving.rs | 83 +++++++++------------------------------- sim/src/intersections.rs | 27 ++++++------- sim/src/lib.rs | 19 +++++++++ sim/src/router.rs | 9 +++-- sim/src/sim.rs | 23 +++++------ sim/src/transit.rs | 22 +++++------ sim/src/view.rs | 64 +++++++++++++++++++++++++++++++ sim/src/walking.rs | 22 +++++++---- 9 files changed, 158 insertions(+), 117 deletions(-) create mode 100644 sim/src/view.rs diff --git a/docs/TODO_phase3.md b/docs/TODO_phase3.md index 88e2c2e1a2..7917cf98c6 100644 --- a/docs/TODO_phase3.md +++ b/docs/TODO_phase3.md @@ -15,6 +15,12 @@ - reversible sim +- be careful + - could see if we ever have a lookahead constraint to deaccel more than + what we're capable of. it might mask problems. but since things like + accel_to_stop_in_dist don't have a careful notion of how much time will pass, + they recommend big rates sometimes. + ## bikes - model bikes as slow cars diff --git a/sim/src/driving.rs b/sim/src/driving.rs index db58473c0c..648990d77f 100644 --- a/sim/src/driving.rs +++ b/sim/src/driving.rs @@ -3,7 +3,7 @@ use abstutil::{deserialize_btreemap, serialize_btreemap}; use dimensioned::si; use draw_car::DrawCar; use geom::EPSILON_DIST; -use intersections::{AgentInfo, IntersectionSimState, Request}; +use intersections::{IntersectionSimState, Request}; use kinematics; use kinematics::Vehicle; use map_model::geometry::LANE_THICKNESS; @@ -14,8 +14,9 @@ use parking::ParkingSimState; use rand::Rng; use router::Router; use std; -use std::collections::{BTreeMap, HashMap}; +use std::collections::BTreeMap; use transit::TransitSimState; +use view::{AgentView, WorldView}; use { Acceleration, AgentID, CarID, CarState, Distance, Event, InvariantViolated, On, ParkedCar, ParkingSpot, Speed, Tick, Time, @@ -94,7 +95,7 @@ impl Car { if let Some(act) = orig_router.react_before_lookahead( events, - &view.cars[&self.id], + view.get_car(self.id), vehicle, time, map, @@ -146,9 +147,9 @@ impl Car { // Don't hit the vehicle in front of us if let Some(other) = view.next_car_in_front_of(current_on, current_dist_along) { - assert!(self.id != other.id); + assert!(self.id != other.id.as_car()); assert!(current_dist_along < other.dist_along); - let other_vehicle = &properties[&other.id]; + let other_vehicle = &properties[&other.id.as_car()]; let dist_behind_other = dist_scanned_ahead + (other.dist_along - current_dist_along); // If our lookahead doesn't even hit the lead vehicle (plus following distance!!!), then ignore them. @@ -321,7 +322,7 @@ impl Car { } #[derive(Serialize, Deserialize, Clone)] -struct SimQueue { +pub struct SimQueue { id: On, // First element is farthest along the queue; they have the greatest dist_along. // Caching the current dist_along vastly simplifies the API of SimQueue. @@ -407,7 +408,7 @@ impl SimQueue { } // TODO for these three, could use binary search - fn next_car_in_front_of(&self, dist: Distance) -> Option { + pub fn next_car_in_front_of(&self, dist: Distance) -> Option { self.cars_queue .iter() .rev() @@ -469,18 +470,6 @@ impl DrivingSimState { s } - // TODO remove this, just use WorldView - pub fn populate_info_for_intersections(&self, info: &mut AgentInfo, _map: &Map) { - let view = self.get_view(); - for c in view.cars.values() { - let id = AgentID::Car(c.id); - info.speeds.insert(id, c.speed); - if view.is_leader(c.id) { - info.leaders.insert(id); - } - } - } - pub fn get_car_state(&self, c: CarID) -> CarState { if let Some(driving) = self.cars.get(&c) { if driving.debug { @@ -560,8 +549,10 @@ impl DrivingSimState { self.turns.insert(id, SimQueue::new(On::Turn(id), map)); } + // Note that this populates the view BEFORE the step is applied pub fn step( &mut self, + view: &mut WorldView, events: &mut Vec, time: Tick, map: &Map, @@ -572,7 +563,7 @@ impl DrivingSimState { rng: &mut R, properties: &BTreeMap, ) -> Result, InvariantViolated> { - let view = self.get_view(); + self.populate_view(view); // Could be concurrent, since this is deterministic -- EXCEPT for the rng, used to // sometimes pick a next lane to try for parking. @@ -834,17 +825,15 @@ impl DrivingSimState { return Vec::new(); } - fn get_view(&self) -> WorldView { - let mut view = WorldView { - cars: HashMap::new(), - lanes: self.lanes.clone(), - turns: self.turns.clone(), - }; + fn populate_view(&self, view: &mut WorldView) { + view.lanes = self.lanes.clone(); + view.turns = self.turns.clone(); + for c in self.cars.values() { - view.cars.insert( - c.id, - CarView { - id: c.id, + view.agents.insert( + AgentID::Car(c.id), + AgentView { + id: AgentID::Car(c.id), debug: c.debug, on: c.on, dist_along: c.dist_along, @@ -852,43 +841,9 @@ impl DrivingSimState { }, ); } - view } pub fn get_current_route(&self, id: CarID) -> Option> { self.routers.get(&id).map(|r| r.get_current_route()) } } - -// The immutable view that cars see of other cars. -pub struct CarView { - pub id: CarID, - pub debug: bool, - pub on: On, - pub dist_along: Distance, - pub speed: Speed, -} - -// TODO unify with AgentInfo at least, and then come up with a better pattern for all of this -struct WorldView { - cars: HashMap, - // TODO I want to borrow the SimQueues, not clone, but then react() still doesnt work to - // mutably borrow router and immutably borrow the queues for the view. :( - lanes: Vec, - turns: BTreeMap, -} - -impl WorldView { - fn next_car_in_front_of(&self, on: On, dist: Distance) -> Option<&CarView> { - let maybe_id = match on { - On::Lane(id) => self.lanes[id.0].next_car_in_front_of(dist), - On::Turn(id) => self.turns[&id].next_car_in_front_of(dist), - }; - maybe_id.map(|id| &self.cars[&id]) - } - - fn is_leader(&self, id: CarID) -> bool { - let c = &self.cars[&id]; - self.next_car_in_front_of(c.on, c.dist_along).is_none() - } -} diff --git a/sim/src/intersections.rs b/sim/src/intersections.rs index 78f959d537..f965c7f4ab 100644 --- a/sim/src/intersections.rs +++ b/sim/src/intersections.rs @@ -7,8 +7,9 @@ use control::ControlMap; use dimensioned::si; use kinematics; use map_model::{IntersectionID, Map, TurnID}; -use std::collections::{BTreeMap, BTreeSet, HashMap, HashSet}; -use {AgentID, CarID, Event, InvariantViolated, PedestrianID, Speed, Tick, Time}; +use std::collections::{BTreeMap, BTreeSet}; +use view::WorldView; +use {AgentID, CarID, Event, InvariantViolated, PedestrianID, Tick, Time}; use std; const WAIT_AT_STOP_SIGN: Time = si::Second { @@ -105,15 +106,15 @@ impl IntersectionSimState { time: Tick, map: &Map, control_map: &ControlMap, - info: AgentInfo, + view: &WorldView, ) { for i in self.intersections.iter_mut() { match i { IntersectionPolicy::StopSignPolicy(ref mut p) => { - p.step(events, time, map, control_map, &info) + p.step(events, time, map, control_map, view) } IntersectionPolicy::TrafficSignalPolicy(ref mut p) => { - p.step(events, time, map, control_map, &info) + p.step(events, time, map, control_map, view) } } } @@ -255,7 +256,7 @@ impl StopSign { time: Tick, map: &Map, control_map: &ControlMap, - info: &AgentInfo, + view: &WorldView, ) { let ss = &control_map.stop_signs[&self.id]; @@ -264,14 +265,14 @@ impl StopSign { let mut newly_stopped: Vec = Vec::new(); for req in self.approaching_agents.iter() { // TODO or not blocked by somebody unaccepted - if !info.leaders.contains(&req.agent) { + if !view.is_leader(req.agent) { continue; } let should_promote = if ss.get_priority(req.turn) == TurnPriority::Stop { // TODO and the agent is at the end? maybe easier than looking at their speed // TODO with lane-changing, somebody could cut in front of them when they're stopped. - info.speeds[&req.agent] <= kinematics::EPSILON_SPEED + view.get_speed(req.agent) <= kinematics::EPSILON_SPEED } else { true }; @@ -346,7 +347,7 @@ impl TrafficSignal { time: Tick, map: &Map, control_map: &ControlMap, - info: &AgentInfo, + view: &WorldView, ) { let signal = &control_map.traffic_signals[&self.id]; let (cycle, _remaining_cycle_time) = @@ -373,7 +374,7 @@ impl TrafficSignal { assert_eq!(self.accepted.contains_key(&agent), false); // Don't accept cars unless they're in front. TODO or behind other accepted cars. - if !cycle.contains(turn.id) || !info.leaders.contains(&req.agent) { + if !cycle.contains(turn.id) || !view.is_leader(req.agent) { keep_requests.insert(req.clone()); continue; } @@ -393,9 +394,3 @@ impl TrafficSignal { self.requests = keep_requests; } } - -// TODO this is a kind of odd way to plumb info to intersections, but... -pub struct AgentInfo { - pub speeds: HashMap, - pub leaders: HashSet, -} diff --git a/sim/src/lib.rs b/sim/src/lib.rs index 379aed64ff..d20cf59ce8 100644 --- a/sim/src/lib.rs +++ b/sim/src/lib.rs @@ -35,6 +35,7 @@ mod sim; mod spawn; mod transit; mod trips; +mod view; mod walking; use dimensioned::si; @@ -79,6 +80,24 @@ pub enum AgentID { Pedestrian(PedestrianID), } +impl AgentID { + pub fn as_car(self) -> CarID { + match self { + AgentID::Car(id) => id, + _ => panic!("Not a CarID: {:?}", self), + } + } +} + +impl fmt::Display for AgentID { + fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { + match self { + AgentID::Car(id) => write!(f, "AgentID({})", id), + AgentID::Pedestrian(id) => write!(f, "AgentID({})", id), + } + } +} + pub const TIMESTEP: Time = si::Second { value_unsafe: 0.1, _marker: std::marker::PhantomData, diff --git a/sim/src/router.rs b/sim/src/router.rs index 4cf0d0917f..50343f94b9 100644 --- a/sim/src/router.rs +++ b/sim/src/router.rs @@ -1,5 +1,5 @@ use dimensioned::si; -use driving::{Action, CarView}; +use driving::Action; use kinematics; use kinematics::Vehicle; use map_model::{BuildingID, LaneID, Map, TurnID}; @@ -7,6 +7,7 @@ use parking::ParkingSimState; use rand::Rng; use std::collections::VecDeque; use transit::TransitSimState; +use view::AgentView; use {Distance, Event, On, ParkingSpot, Tick}; #[derive(Clone, PartialEq, Eq, Serialize, Deserialize)] @@ -49,7 +50,7 @@ impl Router { pub fn react_before_lookahead( &mut self, events: &mut Vec, - view: &CarView, + view: &AgentView, vehicle: &Vehicle, time: Tick, map: &Map, @@ -93,7 +94,7 @@ impl Router { // If we return None, then the caller will immediately ask what turn to do. pub fn stop_early_at_dist( &self, - // TODO urgh, we cant reuse CarView here, because lookahead doesn't advance the view :( + // TODO urgh, we cant reuse AgentView here, because lookahead doesn't advance the view :( on: On, dist_along: Distance, vehicle: &Vehicle, @@ -139,7 +140,7 @@ impl Router { fn look_for_parking( &mut self, last_lane: LaneID, - view: &CarView, + view: &AgentView, map: &Map, rng: &mut R, ) -> Option { diff --git a/sim/src/sim.rs b/sim/src/sim.rs index 14c3701c59..3e4a010891 100644 --- a/sim/src/sim.rs +++ b/sim/src/sim.rs @@ -6,18 +6,19 @@ use dimensioned::si; use draw_car::DrawCar; use draw_ped::DrawPedestrian; use driving::DrivingSimState; -use intersections::{AgentInfo, IntersectionSimState}; +use intersections::IntersectionSimState; use kinematics::Vehicle; use map_model::{IntersectionID, LaneID, LaneType, Map, Turn, TurnID}; use parking::ParkingSimState; use rand::{FromEntropy, SeedableRng, XorShiftRng}; use spawn::Spawner; use std; -use std::collections::{BTreeMap, HashMap, HashSet}; +use std::collections::BTreeMap; use std::f64; use std::time::{Duration, Instant}; use transit::TransitSimState; use trips::TripManager; +use view::WorldView; use walking::WalkingSimState; use {AgentID, CarID, CarState, Event, InvariantViolated, PedestrianID, Tick, TIMESTEP}; @@ -137,6 +138,7 @@ impl Sim { ) -> Result<(Vec), InvariantViolated> { self.time = self.time.next(); + let mut view = WorldView::new(); let mut events: Vec = Vec::new(); self.spawner.step( @@ -151,6 +153,7 @@ impl Sim { ); for p in self.driving_state.step( + &mut view, &mut events, self.time, map, @@ -171,6 +174,7 @@ impl Sim { ); } + self.walking_state.populate_view(&mut view); for (ped, spot) in self.walking_state .step(&mut events, TIMESTEP, map, &mut self.intersection_state)? @@ -194,19 +198,10 @@ impl Sim { map, ); - // TODO want to pass self as a lazy QueryCar trait, but intersection_state is mutably - // borrowed :( - let mut info = AgentInfo { - speeds: HashMap::new(), - leaders: HashSet::new(), - }; - self.driving_state - .populate_info_for_intersections(&mut info, map); - self.walking_state - .populate_info_for_intersections(&mut info); - + // Note that the intersection sees the WorldView BEFORE the updates that just happened this + // tick. self.intersection_state - .step(&mut events, self.time, map, control_map, info); + .step(&mut events, self.time, map, control_map, &view); // Savestate? if let Some(t) = self.savestate_every { diff --git a/sim/src/transit.rs b/sim/src/transit.rs index 48cbcaed8e..0e9599a676 100644 --- a/sim/src/transit.rs +++ b/sim/src/transit.rs @@ -1,12 +1,12 @@ use abstutil::{deserialize_btreemap, serialize_btreemap}; use dimensioned::si; -use driving::CarView; use events::Event; use map_model; use map_model::{BusStop, BusStopDetails, LaneID, Map}; use spawn::Spawner; use std::collections::{BTreeMap, VecDeque}; use trips::TripManager; +use view::AgentView; use walking::WalkingSimState; use {CarID, Distance, PedestrianID, RouteID, Tick}; @@ -122,22 +122,23 @@ impl TransitSimState { pub fn get_action_when_stopped_at_end( &mut self, events: &mut Vec, - view: &CarView, + view: &AgentView, time: Tick, map: &Map, ) -> (bool, Option>) { - let route = &self.routes[&self.buses[&view.id].route]; - match self.buses[&view.id].state { + let car = view.id.as_car(); + let route = &self.routes[&self.buses[&car].route]; + match self.buses[&car].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 = + self.buses.get_mut(&car).unwrap().state = BusState::AtStop(stop_idx, time + 10.0 * si::S); - events.push(Event::BusArrivedAtStop(view.id, stop.id)); + events.push(Event::BusArrivedAtStop(car, stop.id)); if view.debug { - println!("{} arrived at stop {:?}, now waiting", view.id, stop); + println!("{} arrived at stop {:?}, now waiting", car, stop); } return (true, None); } @@ -151,11 +152,10 @@ impl TransitSimState { 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.id)); + self.buses.get_mut(&car).unwrap().state = BusState::DrivingToStop(next_stop); + events.push(Event::BusDepartedFromStop(car, stop.id)); if view.debug { - println!("{} departing from stop {:?}", view.id, stop); + println!("{} departing from stop {:?}", car, stop); } let mut new_path = VecDeque::from( diff --git a/sim/src/view.rs b/sim/src/view.rs new file mode 100644 index 0000000000..9ff14aa445 --- /dev/null +++ b/sim/src/view.rs @@ -0,0 +1,64 @@ +use driving::SimQueue; +use map_model::TurnID; +use std::collections::{BTreeMap, HashMap}; +use {AgentID, CarID, Distance, On, Speed}; + +// An immutable view that agents and intersection controllers see of agents. +pub struct AgentView { + pub id: AgentID, + pub debug: bool, + pub on: On, + pub dist_along: Distance, + pub speed: Speed, +} + +pub struct WorldView { + pub agents: HashMap, + + // This is driving-specific state. Other ways of solving this: + // - having a {Driving,Walking}WorldView and using the enum delegation trick (don't even really + // need a macro; there's just three methods) + // - make WalkingSimState also use SimQueues; they're overpowered for the current use, but + // might be useful for understanding crowded sidewalks + + // TODO I want to borrow the SimQueues, not clone, but then react() still doesnt work to + // mutably borrow router and immutably borrow the queues for the view. :( + pub lanes: Vec, + pub turns: BTreeMap, +} + +impl WorldView { + pub fn new() -> WorldView { + WorldView { + agents: HashMap::new(), + lanes: Vec::new(), + turns: BTreeMap::new(), + } + } + + pub fn next_car_in_front_of(&self, on: On, dist: Distance) -> Option<&AgentView> { + let maybe_id = match on { + On::Lane(id) => self.lanes[id.0].next_car_in_front_of(dist), + On::Turn(id) => self.turns[&id].next_car_in_front_of(dist), + }; + maybe_id.map(|id| &self.agents[&AgentID::Car(id)]) + } + + pub fn is_leader(&self, id: AgentID) -> bool { + match id { + AgentID::Car(_) => { + let c = &self.agents[&id]; + self.next_car_in_front_of(c.on, c.dist_along).is_none() + } + AgentID::Pedestrian(_) => true, + } + } + + pub fn get_speed(&self, id: AgentID) -> Speed { + self.agents[&id].speed + } + + pub fn get_car(&self, id: CarID) -> &AgentView { + &self.agents[&AgentID::Car(id)] + } +} diff --git a/sim/src/walking.rs b/sim/src/walking.rs index 7bc0277890..c1309b44e7 100644 --- a/sim/src/walking.rs +++ b/sim/src/walking.rs @@ -3,12 +3,13 @@ use abstutil::{deserialize_multimap, serialize_multimap}; use dimensioned::si; use draw_ped::DrawPedestrian; use geom::Pt2D; -use intersections::{AgentInfo, IntersectionSimState, Request}; +use intersections::{IntersectionSimState, Request}; use map_model::{BuildingID, BusStop, Lane, LaneID, Map, Turn, TurnID}; use multimap::MultiMap; use parking::ParkingSimState; use std; use std::collections::{BTreeMap, VecDeque}; +use view::{AgentView, WorldView}; use { AgentID, Distance, Event, InvariantViolated, On, ParkingSpot, PedestrianID, Speed, Time, TIMESTEP, @@ -504,18 +505,23 @@ impl WalkingSimState { )); } - pub fn populate_info_for_intersections(&self, info: &mut AgentInfo) { + pub fn populate_view(&self, view: &mut WorldView) { for p in self.peds.values() { let id = AgentID::Pedestrian(p.id); - info.speeds.insert( + view.agents.insert( id, - if p.waiting_for.is_some() { - 0.0 * si::MPS - } else { - SPEED + AgentView { + id, + debug: false, + on: p.on, + dist_along: p.dist_along, + speed: if p.waiting_for.is_some() { + 0.0 * si::MPS + } else { + SPEED + }, }, ); - info.leaders.insert(id); } }