From 917c3c3ed5db5c1e101c9cb6c1670a4a5f99466d Mon Sep 17 00:00:00 2001 From: Dustin Carlino Date: Mon, 20 Aug 2018 08:49:59 -0700 Subject: [PATCH] WIP storing per-car properties like accel somewhere --- docs/design.md | 8 +++++ sim/src/driving.rs | 56 +++++++++++++++++++++++++++-------- sim/src/kinematics.rs | 22 +++++++++++--- sim/src/parametric_driving.rs | 47 ++++++++++++++++++++++------- sim/src/sim.rs | 48 ++++++++++++++++++++++-------- sim/src/spawn.rs | 25 ++++++++++------ 6 files changed, 157 insertions(+), 49 deletions(-) diff --git a/docs/design.md b/docs/design.md index 85e5c651e1..6310a88959 100644 --- a/docs/design.md +++ b/docs/design.md @@ -548,3 +548,11 @@ Some first tests to write: - a line of cars moving through a stop sign looks jittery right now. correct or not? Unclear how to nicely let the test inspect stuff every tick. + +## Per-car properties + +Need to associate car length between driving and parking sims. + +---> could store this in master Sim; after all, there will be some more permanentish stuff like agent/building/trip/owned car soon + - but soon need to bundle things together and pass less params everywhere +- or stash in parking sim, transfer to driving, and back later diff --git a/sim/src/driving.rs b/sim/src/driving.rs index 52cd20486a..11b2c0167e 100644 --- a/sim/src/driving.rs +++ b/sim/src/driving.rs @@ -85,6 +85,7 @@ impl Car { sim: &DrivingSimState, parking_sim: &ParkingSimState, intersections: &IntersectionSimState, + properties: &BTreeMap, ) -> Action { if self.parking.is_some() { // TODO right place for this check? @@ -108,7 +109,7 @@ impl Car { } } - let vehicle = Vehicle::typical_car(); + let vehicle = &properties[&self.id]; // TODO could wrap this state up let mut current_speed_limit = self.on.speed_limit(map); @@ -153,7 +154,7 @@ impl Car { let accel = vehicle.accel_to_follow( self.speed, current_speed_limit, - &vehicle, + &properties[&other.id], dist_behind_other, other.speed, ); @@ -380,10 +381,16 @@ impl SimQueue { // TODO this starts cars with their front aligned with the end of the lane, sticking their back // into the intersection. :( - fn get_draw_cars(&self, sim: &DrivingSimState, map: &Map, time: Tick) -> Vec { + fn get_draw_cars( + &self, + sim: &DrivingSimState, + map: &Map, + time: Tick, + properties: &BTreeMap, + ) -> Vec { let mut results = Vec::new(); for id in &self.cars_queue { - results.push(sim.get_draw_car(*id, time, map).unwrap()) + results.push(sim.get_draw_car(*id, time, map, properties).unwrap()) } results } @@ -542,11 +549,15 @@ impl DrivingSimState { parking_sim: &ParkingSimState, intersections: &mut IntersectionSimState, rng: &mut R, + properties: &BTreeMap, ) -> Result, InvariantViolated> { // Could be concurrent, since this is deterministic. let mut requested_moves: Vec<(CarID, Action)> = Vec::new(); for c in self.cars.values() { - requested_moves.push((c.id, c.react(map, time, self, parking_sim, intersections))); + requested_moves.push(( + c.id, + c.react(map, time, self, parking_sim, intersections, properties), + )); } // In AORTA, there was a split here -- react vs step phase. We're still following the same @@ -648,6 +659,7 @@ impl DrivingSimState { parking: CarParking, mut path: VecDeque, map: &Map, + properties: &BTreeMap, ) -> bool { let start = path.pop_front().unwrap(); let dist_along = parking.spot.dist_along; @@ -670,11 +682,11 @@ impl DrivingSimState { return false; } - let vehicle = Vehicle::typical_car(); + let vehicle = &properties[&car]; let accel_for_other_to_stop = vehicle.accel_to_follow( self.cars[&other].speed, map.get_parent(start).get_speed_limit(), - &vehicle, + &properties[&other], dist_along - other_dist, 0.0 * si::MPS, ); @@ -713,10 +725,16 @@ impl DrivingSimState { true } - pub fn get_draw_car(&self, id: CarID, time: Tick, map: &Map) -> Option { + pub fn get_draw_car( + &self, + id: CarID, + time: Tick, + map: &Map, + properties: &BTreeMap, + ) -> Option { let c = self.cars.get(&id)?; let (base_pos, angle) = c.on.dist_along(c.dist_along, map); - let stopping_dist = Vehicle::typical_car().stopping_distance(c.speed); + let stopping_dist = properties[&id].stopping_distance(c.speed); // TODO arguably, this math might belong in DrawCar. let pos = if let Some(ref parking) = c.parking { @@ -744,13 +762,25 @@ impl DrivingSimState { )) } - pub fn get_draw_cars_on_lane(&self, lane: LaneID, time: Tick, map: &Map) -> Vec { - self.lanes[lane.0].get_draw_cars(self, map, time) + pub fn get_draw_cars_on_lane( + &self, + lane: LaneID, + time: Tick, + map: &Map, + properties: &BTreeMap, + ) -> Vec { + self.lanes[lane.0].get_draw_cars(self, map, time, properties) } - pub fn get_draw_cars_on_turn(&self, turn: TurnID, time: Tick, map: &Map) -> Vec { + pub fn get_draw_cars_on_turn( + &self, + turn: TurnID, + time: Tick, + map: &Map, + properties: &BTreeMap, + ) -> Vec { if let Some(queue) = self.turns.get(&turn) { - return queue.get_draw_cars(self, map, time); + return queue.get_draw_cars(self, map, time, properties); } return Vec::new(); } diff --git a/sim/src/kinematics.rs b/sim/src/kinematics.rs index c91e463b43..22b95b9459 100644 --- a/sim/src/kinematics.rs +++ b/sim/src/kinematics.rs @@ -1,8 +1,9 @@ use dimensioned::si; use geom::EPSILON_DIST; use models::FOLLOWING_DISTANCE; +use rand::Rng; use std; -use {Acceleration, Distance, Speed, Time, TIMESTEP}; +use {Acceleration, CarID, Distance, Speed, Time, TIMESTEP}; pub const EPSILON_SPEED: Speed = si::MeterPerSecond { value_unsafe: 0.00000001, @@ -12,18 +13,31 @@ pub const EPSILON_SPEED: Speed = si::MeterPerSecond { // TODO unit test all of this // TODO handle floating point issues uniformly here +#[derive(Serialize, Deserialize, Debug)] pub struct Vehicle { + pub id: CarID, + // > 0 max_accel: Acceleration, // < 0 pub max_deaccel: Acceleration, } +// TODO this is used for verifying sim state determinism, so it should actually check everything. +// the f64 prevents this from being derived. +impl PartialEq for Vehicle { + fn eq(&self, other: &Vehicle) -> bool { + self.id == other.id + } +} +impl Eq for Vehicle {} + impl Vehicle { - pub fn typical_car() -> Vehicle { + pub fn generate_typical_car(id: CarID, rng: &mut R) -> Vehicle { Vehicle { - max_accel: 2.7 * si::MPS2, - max_deaccel: -2.7 * si::MPS2, + id, + max_accel: rng.gen_range(2.4, 2.8) * si::MPS2, + max_deaccel: rng.gen_range(-2.8, -2.4) * si::MPS2, } } diff --git a/sim/src/parametric_driving.rs b/sim/src/parametric_driving.rs index 610d809103..437f0f2dae 100644 --- a/sim/src/parametric_driving.rs +++ b/sim/src/parametric_driving.rs @@ -194,13 +194,20 @@ impl SimQueue { // TODO this starts cars with their front aligned with the end of the lane, sticking their back // into the intersection. :( - fn get_draw_cars(&self, time: Tick, sim: &DrivingSimState, map: &Map) -> Vec { + fn get_draw_cars( + &self, + time: Tick, + sim: &DrivingSimState, + map: &Map, + properties: &BTreeMap, + ) -> Vec { if self.cars_queue.is_empty() { return Vec::new(); } - // TODO base this on actual speed ;) - let stopping_dist = Vehicle::typical_car().stopping_distance(self.id.speed_limit(map)); + // TODO base this on actual speed and each vehicle ;) + let stopping_dist = + properties[&self.cars_queue[0]].stopping_distance(self.id.speed_limit(map)); let mut results = Vec::new(); let (pos1, angle1, dist_along1) = @@ -372,6 +379,7 @@ impl DrivingSimState { _parking_sim: &ParkingSimState, intersections: &mut IntersectionSimState, _rng: &mut R, + _properties: &BTreeMap, ) -> Result, InvariantViolated> { // Could be concurrent, since this is deterministic. let mut requested_moves: Vec<(CarID, Action)> = Vec::new(); @@ -467,6 +475,7 @@ impl DrivingSimState { _parking: CarParking, mut path: VecDeque, map: &Map, + _properties: &BTreeMap, ) -> bool { let start = path.pop_front().unwrap(); @@ -491,21 +500,39 @@ impl DrivingSimState { true } - pub fn get_draw_car(&self, id: CarID, time: Tick, map: &Map) -> Option { + pub fn get_draw_car( + &self, + id: CarID, + time: Tick, + map: &Map, + properties: &BTreeMap, + ) -> Option { let all = match self.cars.get(&id)?.on { - On::Lane(l) => self.get_draw_cars_on_lane(l, time, map), - On::Turn(t) => self.get_draw_cars_on_turn(t, time, map), + On::Lane(l) => self.get_draw_cars_on_lane(l, time, map, properties), + On::Turn(t) => self.get_draw_cars_on_turn(t, time, map, properties), }; all.into_iter().find(|c| c.id == id) } - pub fn get_draw_cars_on_lane(&self, lane: LaneID, time: Tick, map: &Map) -> Vec { - self.lanes[lane.0].get_draw_cars(time, self, map) + pub fn get_draw_cars_on_lane( + &self, + lane: LaneID, + time: Tick, + map: &Map, + properties: &BTreeMap, + ) -> Vec { + self.lanes[lane.0].get_draw_cars(time, self, map, properties) } - pub fn get_draw_cars_on_turn(&self, turn: TurnID, time: Tick, map: &Map) -> Vec { + pub fn get_draw_cars_on_turn( + &self, + turn: TurnID, + time: Tick, + map: &Map, + properties: &BTreeMap, + ) -> Vec { if let Some(queue) = self.turns.get(&turn) { - return queue.get_draw_cars(time, self, map); + return queue.get_draw_cars(time, self, map, properties); } return Vec::new(); } diff --git a/sim/src/sim.rs b/sim/src/sim.rs index 18a93fc6bf..62377583c5 100644 --- a/sim/src/sim.rs +++ b/sim/src/sim.rs @@ -6,12 +6,13 @@ use draw_car::DrawCar; use draw_ped::DrawPedestrian; use driving; use intersections::{AgentInfo, IntersectionSimState}; +use kinematics::Vehicle; use map_model::{IntersectionID, LaneID, LaneType, Map, Turn, TurnID}; use parametric_driving; use parking::ParkingSimState; use rand::{FromEntropy, Rng, SeedableRng, XorShiftRng}; use spawn::Spawner; -use std::collections::{HashMap, HashSet, VecDeque}; +use std::collections::{BTreeMap, HashMap, HashSet, VecDeque}; use std::f64; use std::time::{Duration, Instant}; use walking::WalkingSimState; @@ -106,18 +107,19 @@ impl DrivingModel { delegate!(fn edit_add_lane(&mut self, id: LaneID)); delegate!(fn edit_remove_turn(&mut self, id: TurnID)); delegate!(fn edit_add_turn(&mut self, id: TurnID, map: &Map)); - delegate!(fn step(&mut self, time: Tick, map: &Map, parking: &ParkingSimState, intersections: &mut IntersectionSimState, rng: &mut R) -> Result, InvariantViolated>); + delegate!(fn step(&mut self, time: Tick, map: &Map, parking: &ParkingSimState, intersections: &mut IntersectionSimState, rng: &mut R, properties: &BTreeMap) -> Result, InvariantViolated>); delegate!(pub fn start_car_on_lane( &mut self, time: Tick, car: CarID, parking: CarParking, path: VecDeque, - map: &Map + map: &Map, + properties: &BTreeMap ) -> bool); - delegate!(fn get_draw_car(&self, id: CarID, time: Tick, map: &Map) -> Option); - delegate!(fn get_draw_cars_on_lane(&self, lane: LaneID, time: Tick, map: &Map) -> Vec); - delegate!(fn get_draw_cars_on_turn(&self, turn: TurnID, time: Tick, map: &Map) -> Vec); + delegate!(fn get_draw_car(&self, id: CarID, time: Tick, map: &Map, properties: &BTreeMap) -> Option); + delegate!(fn get_draw_cars_on_lane(&self, lane: LaneID, time: Tick, map: &Map, properties: &BTreeMap) -> Vec); + delegate!(fn get_draw_cars_on_turn(&self, turn: TurnID, time: Tick, map: &Map, properties: &BTreeMap) -> Vec); } #[derive(Serialize, Deserialize, Derivative)] @@ -134,6 +136,8 @@ pub struct Sim { driving_state: DrivingModel, parking_state: ParkingSimState, walking_state: WalkingSimState, + + car_properties: BTreeMap, } impl Sim { @@ -157,6 +161,7 @@ impl Sim { parking_state: ParkingSimState::new(map), walking_state: WalkingSimState::new(), time: Tick::zero(), + car_properties: BTreeMap::new(), } } @@ -193,13 +198,24 @@ impl Sim { } pub fn seed_parked_cars(&mut self, percent: f64) { - self.spawner - .seed_parked_cars(percent, &mut self.parking_state, &mut self.rng); + for v in self.spawner + .seed_parked_cars(percent, &mut self.parking_state, &mut self.rng) + .into_iter() + { + self.car_properties.insert(v.id, v); + } } pub fn seed_specific_parked_cars(&mut self, lane: LaneID, spots: Vec) -> Vec { - self.spawner - .seed_specific_parked_cars(lane, spots, &mut self.parking_state) + let mut ids = Vec::new(); + for v in self.spawner + .seed_specific_parked_cars(lane, spots, &mut self.parking_state, &mut self.rng) + .into_iter() + { + ids.push(v.id); + self.car_properties.insert(v.id, v); + } + ids } pub fn start_many_parked_cars(&mut self, map: &Map, num_cars: usize) { @@ -252,6 +268,7 @@ impl Sim { &mut self.parking_state, &mut self.walking_state, &mut self.driving_state, + &self.car_properties, ); let mut cars_parked_this_step: Vec = Vec::new(); @@ -261,6 +278,7 @@ impl Sim { &self.parking_state, &mut self.intersection_state, &mut self.rng, + &self.car_properties, ) { Ok(parked_cars) => for p in parked_cars { cars_parked_this_step.push(p.clone()); @@ -298,7 +316,7 @@ impl Sim { pub fn get_draw_car(&self, id: CarID, map: &Map) -> Option { self.driving_state - .get_draw_car(id, self.time, map) + .get_draw_car(id, self.time, map, &self.car_properties) .or_else(|| self.parking_state.get_draw_car(id, map)) } @@ -309,7 +327,10 @@ impl Sim { // TODO maybe just DrawAgent instead? should caller care? pub fn get_draw_cars_on_lane(&self, l: LaneID, map: &Map) -> Vec { match map.get_l(l).lane_type { - LaneType::Driving => self.driving_state.get_draw_cars_on_lane(l, self.time, map), + LaneType::Driving => { + self.driving_state + .get_draw_cars_on_lane(l, self.time, map, &self.car_properties) + } LaneType::Parking => self.parking_state.get_draw_cars(l, map), LaneType::Sidewalk => Vec::new(), LaneType::Biking => Vec::new(), @@ -317,7 +338,8 @@ impl Sim { } pub fn get_draw_cars_on_turn(&self, t: TurnID, map: &Map) -> Vec { - self.driving_state.get_draw_cars_on_turn(t, self.time, map) + self.driving_state + .get_draw_cars_on_turn(t, self.time, map, &self.car_properties) } pub fn get_draw_peds_on_lane(&self, l: LaneID, map: &Map) -> Vec { diff --git a/sim/src/spawn.rs b/sim/src/spawn.rs index 729874cca4..62769faea7 100644 --- a/sim/src/spawn.rs +++ b/sim/src/spawn.rs @@ -1,9 +1,10 @@ +use kinematics::Vehicle; use map_model; use map_model::{LaneID, Map}; use parking::ParkingSimState; use rand::Rng; use sim::{CarParking, DrivingModel}; -use std::collections::VecDeque; +use std::collections::{BTreeMap, VecDeque}; use std::time::Instant; use walking::WalkingSimState; use {AgentID, CarID, PedestrianID, Tick}; @@ -47,6 +48,7 @@ impl Spawner { parking_sim: &mut ParkingSimState, walking_sim: &mut WalkingSimState, driving_sim: &mut DrivingModel, + properties: &BTreeMap, ) { for p in self.spawn_parked_cars.drain(0..) { parking_sim.add_parked_car(p); @@ -95,6 +97,7 @@ impl Spawner { CarParking::new(*car, spot), VecDeque::from(path), map, + properties, ) { parking_sim.remove_parked_car(parking_lane, *car); spawned_agents += 1; @@ -134,36 +137,40 @@ impl Spawner { percent_capacity_to_fill: f64, parking_sim: &mut ParkingSimState, rng: &mut R, - ) { + ) -> Vec { assert!(percent_capacity_to_fill >= 0.0 && percent_capacity_to_fill <= 1.0); assert!(self.spawn_parked_cars.is_empty()); let mut total_capacity = 0; - let mut new_cars = 0; + let mut new_cars: Vec = Vec::new(); for spot in parking_sim.get_all_free_spots() { total_capacity += 1; if rng.gen_bool(percent_capacity_to_fill) { - new_cars += 1; + let id = CarID(self.car_id_counter); // TODO since spawning applies during the next step, lots of stuff breaks without // this :( - parking_sim.add_parked_car(CarParking::new(CarID(self.car_id_counter), spot)); + parking_sim.add_parked_car(CarParking::new(id, spot)); //self.spawn_parked_cars.push(CarParking::new(CarID(self.car_id_counter), spot)); + new_cars.push(Vehicle::generate_typical_car(id, rng)); self.car_id_counter += 1; } } println!( "Seeded {} of {} parking spots with cars", - new_cars, total_capacity + new_cars.len(), + total_capacity ); + new_cars } - pub fn seed_specific_parked_cars( + pub fn seed_specific_parked_cars( &mut self, lane: LaneID, spot_indices: Vec, parking_sim: &mut ParkingSimState, - ) -> Vec { + rng: &mut R, + ) -> Vec { assert!(self.spawn_parked_cars.is_empty()); let spots = parking_sim.get_all_spots(lane); spot_indices @@ -173,7 +180,7 @@ impl Spawner { parking_sim.add_parked_car(CarParking::new(id, spots[idx].clone())); // TODO push onto spawn_parked_cars? self.car_id_counter += 1; - id + Vehicle::generate_typical_car(id, rng) }) .collect() }