stop sign should only admit leader vehicles

editor/src/plugins/selection.rs

@@ -102,6 +102,7 @@ impl SelectionState {
                     true
                 } else if input.key_pressed(Key::D, "debug") {
                     map.get_i(*id).dump_debug();
+                    sim.debug_intersection(*id);
                     true
                 } else {
                     false

sim/src/driving.rs

@@ -2,14 +2,14 @@ use abstutil;
 use abstutil::{deserialize_btreemap, serialize_btreemap};
 use dimensioned::si;
 use draw_car::DrawCar;
-use intersections::{IntersectionSimState, Request};
+use intersections::{AgentInfo, IntersectionSimState, Request};
 use kinematics;
 use kinematics::Vehicle;
 use map_model::{LaneID, LaneType, Map, TurnID};
 use models::{choose_turn, FOLLOWING_DISTANCE};
 use multimap::MultiMap;
 use ordered_float::NotNaN;
-use std::collections::{BTreeMap, HashMap, VecDeque};
+use std::collections::{BTreeMap, VecDeque};
 use {
     Acceleration, AgentID, CarID, CarState, Distance, InvariantViolated, On, Speed, Tick,
     SPEED_LIMIT,
@@ -305,12 +305,14 @@ impl DrivingSimState {
         s
     }
 
-    pub fn get_all_speeds(&self) -> HashMap<AgentID, Speed> {
-        let mut m = HashMap::new();
+    pub fn populate_info_for_intersections(&self, info: &mut AgentInfo) {
         for c in self.cars.values() {
-            m.insert(AgentID::Car(c.id), c.speed);
+            let id = AgentID::Car(c.id);
+            info.speeds.insert(id, c.speed);
+            if !self.next_car_in_front_of(c.on, c.dist_along).is_some() {
+                info.leaders.insert(id);
+            }
         }
-        m
     }
 
     pub fn get_car_state(&self, c: CarID) -> CarState {

sim/src/intersections.rs

@@ -1,12 +1,13 @@
 // Copyright 2018 Google LLC, licensed under http://www.apache.org/licenses/LICENSE-2.0
 
+use abstutil;
 use abstutil::{deserialize_btreemap, serialize_btreemap};
 use control::stop_signs::{ControlStopSign, TurnPriority};
 use control::ControlMap;
 use dimensioned::si;
 use kinematics;
 use map_model::{IntersectionID, Map, TurnID};
-use std::collections::{BTreeMap, BTreeSet, HashMap};
+use std::collections::{BTreeMap, BTreeSet, HashMap, HashSet};
 use {AgentID, CarID, InvariantViolated, PedestrianID, Speed, Tick, Time};
 
 use std;
@@ -65,9 +66,9 @@ impl IntersectionSimState {
 
     // This is mutable, but MUST be idempotent, because it could be called in parallel/nondet
     // orders. It does NOT grant the request, just enqueues it for later consideration. The agent
-    // must be ready to enter the intersection (leader vehicle and at the end of the lane already).
-    // The request may have been previously granted, but the agent might not have been able to
-    // start the turn.
+    // MIGHT NOT be ready to enter the intersection (lookahead could send the request before the
+    // agent is the leader vehicle and at the end of the lane). The request may have been
+    // previously granted, but the agent might not have been able to start the turn.
     pub fn submit_request(&mut self, req: Request) -> Result<(), InvariantViolated> {
         let i = self.intersections.get_mut(req.turn.parent.0).unwrap();
         if let Some(t) = i.accepted().get(&req.agent) {
@@ -94,20 +95,14 @@ impl IntersectionSimState {
         Ok(())
     }
 
-    pub fn step(
-        &mut self,
-        time: Tick,
-        map: &Map,
-        control_map: &ControlMap,
-        speeds: HashMap<AgentID, Speed>,
-    ) {
+    pub fn step(&mut self, time: Tick, map: &Map, control_map: &ControlMap, info: AgentInfo) {
         for i in self.intersections.iter_mut() {
             match i {
                 IntersectionPolicy::StopSignPolicy(ref mut p) => {
-                    p.step(time, map, control_map, &speeds)
+                    p.step(time, map, control_map, &info)
                 }
                 IntersectionPolicy::TrafficSignalPolicy(ref mut p) => {
-                    p.step(time, map, control_map, &speeds)
+                    p.step(time, map, control_map, &info)
                 }
             }
         }
@@ -130,6 +125,10 @@ impl IntersectionSimState {
         assert!(i.accepted().contains_key(&req.agent));
         i.accepted_mut().remove(&req.agent);
     }
+
+    pub fn debug(&self, id: IntersectionID) {
+        println!("{}", abstutil::to_json(&self.intersections[id.0]));
+    }
 }
 
 // Use an enum instead of traits so that serialization works. I couldn't figure out erased_serde.
@@ -207,23 +206,19 @@ impl StopSign {
             .is_some()
     }
 
-    fn step(
-        &mut self,
-        time: Tick,
-        map: &Map,
-        control_map: &ControlMap,
-        speeds: &HashMap<AgentID, Speed>,
-    ) {
+    fn step(&mut self, time: Tick, map: &Map, control_map: &ControlMap, info: &AgentInfo) {
         // If anybody is stopped, promote them.
         // TODO retain() would rock
         let mut newly_stopped: Vec<Request> = Vec::new();
         for req in self.approaching_agents.iter() {
             // TODO tmpish debug
-            if !speeds.contains_key(&req.agent) {
+            if !info.speeds.contains_key(&req.agent) {
                 println!("no speed for {:?}", req);
             }
 
-            if speeds[&req.agent] <= kinematics::EPSILON_SPEED {
+            if info.leaders.contains(&req.agent)
+                && info.speeds[&req.agent] <= kinematics::EPSILON_SPEED
+            {
                 self.started_waiting_at.insert(req.clone(), time);
                 newly_stopped.push(req.clone());
             }
@@ -282,15 +277,10 @@ impl TrafficSignal {
 
     // TODO determine if agents are staying in the intersection past the cycle time.
 
-    fn step(
-        &mut self,
-        time: Tick,
-        map: &Map,
-        control_map: &ControlMap,
-        _speeds: &HashMap<AgentID, Speed>,
-    ) {
+    fn step(&mut self, time: Tick, map: &Map, control_map: &ControlMap, _info: &AgentInfo) {
         let signal = &control_map.traffic_signals[&self.id];
-        let (cycle, _remaining_cycle_time) = signal.current_cycle_and_remaining_time(time.as_time());
+        let (cycle, _remaining_cycle_time) =
+            signal.current_cycle_and_remaining_time(time.as_time());
 
         let mut keep_requests: BTreeSet<Request> = BTreeSet::new();
         for req in self.requests.iter() {
@@ -314,3 +304,9 @@ 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<AgentID, Speed>,
+    pub leaders: HashSet<AgentID>,
+}

sim/src/parametric_driving.rs

@@ -8,13 +8,13 @@ use abstutil::{deserialize_btreemap, serialize_btreemap};
 use dimensioned::si;
 use draw_car::DrawCar;
 use geom::{Angle, Pt2D};
-use intersections::{IntersectionSimState, Request};
+use intersections::{AgentInfo, IntersectionSimState, Request};
 use kinematics::Vehicle;
 use map_model::{LaneID, LaneType, Map, TurnID};
 use models::{choose_turn, Action, FOLLOWING_DISTANCE};
 use multimap::MultiMap;
-use std::collections::{BTreeMap, HashMap, HashSet, VecDeque};
-use {AgentID, CarID, CarState, Distance, InvariantViolated, On, Speed, Tick, SPEED_LIMIT};
+use std::collections::{BTreeMap, HashSet, VecDeque};
+use {AgentID, CarID, CarState, Distance, InvariantViolated, On, Tick, SPEED_LIMIT};
 
 // This represents an actively driving car, not a parked one
 #[derive(Clone, Serialize, Deserialize)]
@@ -277,19 +277,19 @@ impl DrivingSimState {
         s
     }
 
-    pub fn get_all_speeds(&self) -> HashMap<AgentID, Speed> {
-        let mut m = HashMap::new();
+    pub fn populate_info_for_intersections(&self, info: &mut AgentInfo) {
         for c in self.cars.values() {
-            m.insert(
-                AgentID::Car(c.id),
+            let id = AgentID::Car(c.id);
+            info.speeds.insert(
+                id,
                 if c.waiting_for.is_some() {
                     0.0 * si::MPS
                 } else {
                     SPEED_LIMIT
                 },
             );
+            info.leaders.insert(id);
         }
-        m
     }
 
     pub fn get_car_state(&self, c: CarID) -> CarState {

sim/src/sim.rs

@@ -5,17 +5,17 @@ use dimensioned::si;
 use draw_car::DrawCar;
 use draw_ped::DrawPedestrian;
 use driving;
-use intersections::IntersectionSimState;
+use intersections::{AgentInfo, IntersectionSimState};
 use map_model;
-use map_model::{LaneID, LaneType, Map, Turn, TurnID};
+use map_model::{IntersectionID, LaneID, LaneType, Map, Turn, TurnID};
 use parametric_driving;
 use parking::ParkingSimState;
 use rand::{FromEntropy, Rng, SeedableRng, XorShiftRng};
-use std::collections::{HashMap, VecDeque};
+use std::collections::{HashMap, HashSet, VecDeque};
 use std::f64;
 use std::time::{Duration, Instant};
 use walking::WalkingSimState;
-use {AgentID, CarID, CarState, InvariantViolated, PedestrianID, Speed, Tick, TIMESTEP};
+use {CarID, CarState, InvariantViolated, PedestrianID, Tick, TIMESTEP};
 
 #[derive(Serialize, Deserialize, Derivative, PartialEq, Eq)]
 enum DrivingModel {
@@ -44,6 +44,16 @@ macro_rules! delegate {
         }
     };
 
+    // Immutable, arguments, no return type
+    (fn $fxn_name:ident(&self, $($value:ident: $type:ty),* )) => {
+        fn $fxn_name(&self, $( $value: $type ),*) {
+            match self {
+                DrivingModel::V1(s) => s.$fxn_name($( $value ),*),
+                DrivingModel::V2(s) => s.$fxn_name($( $value ),*),
+            }
+        }
+    };
+
     // Mutable, arguments, return type
     (fn $fxn_name:ident(&mut self, $($value:ident: $type:ty),* ) -> $ret:ty) => {
         fn $fxn_name(&mut self, $( $value: $type ),*) -> $ret {
@@ -66,7 +76,7 @@ macro_rules! delegate {
 }
 
 impl DrivingModel {
-    delegate!(fn get_all_speeds(&self) -> HashMap<AgentID, Speed>);
+    delegate!(fn populate_info_for_intersections(&self, info: &mut AgentInfo));
     delegate!(fn get_car_state(&self, c: CarID) -> CarState);
     delegate!(fn get_active_and_waiting_count(&self) -> (usize, usize));
     delegate!(fn tooltip_lines(&self, id: CarID) -> Option<Vec<String>>);
@@ -328,11 +338,17 @@ impl Sim {
 
         // TODO want to pass self as a lazy QueryCar trait, but intersection_state is mutably
         // borrowed :(
-        let mut speeds = self.driving_state.get_all_speeds();
-        speeds.extend(self.walking_state.get_all_speeds());
+        let mut info = AgentInfo {
+            speeds: HashMap::new(),
+            leaders: HashSet::new(),
+        };
+        self.driving_state
+            .populate_info_for_intersections(&mut info);
+        self.walking_state
+            .populate_info_for_intersections(&mut info);
 
         self.intersection_state
-            .step(self.time, map, control_map, speeds);
+            .step(self.time, map, control_map, info);
     }
 
     pub fn get_car_state(&self, c: CarID) -> CarState {
@@ -416,6 +432,10 @@ impl Sim {
         b.last_sim_time = self.time;
         speed.value_unsafe
     }
+
+    pub fn debug_intersection(&self, id: IntersectionID) {
+        self.intersection_state.debug(id);
+    }
 }
 
 pub struct Benchmark {

sim/src/walking.rs

@@ -2,12 +2,12 @@ use abstutil;
 use abstutil::{deserialize_multimap, serialize_multimap};
 use dimensioned::si;
 use draw_ped::DrawPedestrian;
-use intersections::{IntersectionSimState, Request};
+use intersections::{AgentInfo, IntersectionSimState, Request};
 use map_model::{Lane, LaneID, Map, Turn, TurnID};
 use models::{choose_turn, Action};
 use multimap::MultiMap;
 use std;
-use std::collections::{BTreeMap, HashMap, VecDeque};
+use std::collections::{BTreeMap, VecDeque};
 use {AgentID, Distance, InvariantViolated, On, PedestrianID, Speed, Time};
 
 // TODO tune these!
@@ -297,19 +297,19 @@ impl WalkingSimState {
         self.peds_per_sidewalk.insert(start, id);
     }
 
-    pub fn get_all_speeds(&self) -> HashMap<AgentID, Speed> {
-        let mut m = HashMap::new();
+    pub fn populate_info_for_intersections(&self, info: &mut AgentInfo) {
         for p in self.peds.values() {
-            m.insert(
-                AgentID::Pedestrian(p.id),
+            let id = AgentID::Pedestrian(p.id);
+            info.speeds.insert(
+                id,
                 if p.waiting_for.is_some() {
                     0.0 * si::MPS
                 } else {
                     SPEED
                 },
             );
+            info.leaders.insert(id);
         }
-        m
     }
 }