sim/mechanics/
intersection.rs

1use std::collections::{BTreeMap, BTreeSet, HashMap, HashSet};
2
3use serde::{Deserialize, Serialize};
4
5use abstutil::{deserialize_btreemap, prettyprint_usize, serialize_btreemap, FixedMap};
6use geom::{Duration, Time};
7use map_model::{
8    ControlStopSign, ControlTrafficSignal, Intersection, IntersectionID, LaneID, Map, StageType,
9    Traversable, TurnID, TurnPriority, TurnType, UberTurn,
10};
11
12use crate::mechanics::car::{Car, CarState};
13use crate::mechanics::Queue;
14use crate::{
15    AgentID, AlertLocation, CarID, Command, DelayCause, Event, Scheduler, SimOptions, Speed,
16};
17
18const WAIT_AT_STOP_SIGN: Duration = Duration::const_seconds(0.5);
19const WAIT_BEFORE_YIELD_AT_TRAFFIC_SIGNAL: Duration = Duration::const_seconds(0.2);
20
21/// Manages conflicts at intersections. When an agent has reached the end of a lane, they call
22/// maybe_start_turn to make a Request. Based on the intersection type (stop sign, traffic signal,
23/// or a "freeform policy"), the Request gets queued or immediately accepted. When agents finish
24/// turns or when some time passes (for traffic signals), the intersection also gets a chance to
25/// react, maybe granting one of the pending requests.
26///
27/// Most of the complexity comes from attempting to workaround
28/// <https://a-b-street.github.io/docs/tech/trafficsim/gridlock.html>.
29#[derive(Serialize, Deserialize, Clone)]
30pub(crate) struct IntersectionSimState {
31    state: BTreeMap<IntersectionID, State>,
32    use_freeform_policy_everywhere: bool,
33    dont_block_the_box: bool,
34    break_turn_conflict_cycles: bool,
35    handle_uber_turns: bool,
36    disable_turn_conflicts: bool,
37    // (x, y) means x is blocked by y. It's a many-to-many relationship. TODO Better data
38    // structure.
39    blocked_by: BTreeSet<(CarID, CarID)>,
40    events: Vec<Event>,
41
42    // Count how many calls to maybe_start_turn there are aside from the initial call. Break down
43    // failures by those not allowed by the current intersection state vs those blocked by a
44    // vehicle in the way in the target queue.
45    total_repeat_requests: usize,
46    not_allowed_requests: usize,
47    blocked_by_someone_requests: usize,
48}
49
50#[derive(Clone, Debug, Serialize, Deserialize)]
51struct State {
52    id: IntersectionID,
53    // The in-progress turns which any potential new turns must not conflict with
54    accepted: BTreeSet<Request>,
55    // Track when a request is first made and if it's "urgent" (because the agent is overflowing a
56    // short queue)
57    #[serde(
58        serialize_with = "serialize_btreemap",
59        deserialize_with = "deserialize_btreemap"
60    )]
61    waiting: BTreeMap<Request, (Time, bool)>,
62    // When a vehicle begins an uber-turn, reserve the future turns to ensure they're able to
63    // complete the entire sequence. This is especially necessary since groups of traffic signals
64    // are not yet configured as one.
65    reserved: BTreeSet<Request>,
66    // In some cases, a turn completing at one intersection may affect agents waiting to start an
67    // uber-turn at nearby intersections.
68    uber_turn_neighbors: Vec<IntersectionID>,
69
70    // This is keyed by the lane the agent is approaching from. Note that:
71    // 1) the turn in the request may change by the time the leader arrives -- they might decide to
72    //    aim for a different destination lane
73    // 2) If another agent pulls out on a driveway before this agent, they become the leader and
74    //    overwrite this one
75    #[serde(
76        serialize_with = "serialize_btreemap",
77        deserialize_with = "deserialize_btreemap"
78    )]
79    leader_eta: BTreeMap<LaneID, (Request, Time)>,
80
81    signal: Option<SignalState>,
82}
83
84#[derive(Clone, Debug, Serialize, Deserialize)]
85struct SignalState {
86    // The current stage of the signal, zero based
87    current_stage: usize,
88    // The time when the signal is checked for advancing
89    stage_ends_at: Time,
90    // The number of times a variable signal has been extended during the current stage.
91    extensions_count: usize,
92}
93
94#[derive(PartialEq, Eq, PartialOrd, Ord, Serialize, Deserialize, Clone, Debug)]
95struct Request {
96    agent: AgentID,
97    turn: TurnID,
98}
99
100// Mutations
101impl IntersectionSimState {
102    pub fn new(map: &Map, scheduler: &mut Scheduler, opts: &SimOptions) -> IntersectionSimState {
103        let mut sim = IntersectionSimState {
104            state: BTreeMap::new(),
105            use_freeform_policy_everywhere: opts.use_freeform_policy_everywhere,
106            dont_block_the_box: !opts.allow_block_the_box,
107            break_turn_conflict_cycles: !opts.dont_break_turn_conflict_cycles,
108            handle_uber_turns: !opts.dont_handle_uber_turns,
109            disable_turn_conflicts: opts.disable_turn_conflicts,
110            blocked_by: BTreeSet::new(),
111            events: Vec::new(),
112
113            total_repeat_requests: 0,
114            not_allowed_requests: 0,
115            blocked_by_someone_requests: 0,
116        };
117        if sim.disable_turn_conflicts {
118            sim.use_freeform_policy_everywhere = true;
119        }
120
121        for i in map.all_intersections() {
122            let mut state = State {
123                id: i.id,
124                accepted: BTreeSet::new(),
125                waiting: BTreeMap::new(),
126                reserved: BTreeSet::new(),
127                uber_turn_neighbors: Vec::new(),
128                signal: None,
129                leader_eta: BTreeMap::new(),
130            };
131            if i.is_traffic_signal() {
132                state.signal = Some(SignalState::new(i.id, Time::START_OF_DAY, map, scheduler));
133            }
134            if let Some(mut set) = map_model::IntersectionCluster::autodetect(i.id, map) {
135                set.remove(&i.id);
136                state.uber_turn_neighbors.extend(set);
137            }
138            sim.state.insert(i.id, state);
139        }
140        sim
141    }
142
143    pub fn turn_finished(
144        &mut self,
145        now: Time,
146        agent: AgentID,
147        turn: TurnID,
148        scheduler: &mut Scheduler,
149        map: &Map,
150        handling_live_edits: bool,
151    ) {
152        let state = self.state.get_mut(&turn.parent).unwrap();
153        assert!(state.accepted.remove(&Request { agent, turn }));
154
155        state.reserved.remove(&Request { agent, turn });
156        if !handling_live_edits && map.get_t(turn).turn_type != TurnType::SharedSidewalkCorner {
157            self.wakeup_waiting(now, turn.parent, scheduler, map);
158        }
159        if self.break_turn_conflict_cycles {
160            if let AgentID::Car(car) = agent {
161                self.blocked_by.retain(|(_, c)| *c != car);
162            }
163        }
164
165        // If this intersection has uber-turns going through it, then we may need to wake up agents
166        // with a reserved uber-turn that're waiting at nearby intersections. They may have
167        // reserved their sequence of turns but then gotten stuck by somebody filling up a queue a
168        // few steps away.
169        for id in &self.state[&turn.parent].uber_turn_neighbors {
170            self.wakeup_waiting(now, *id, scheduler, map);
171        }
172    }
173
174    /// For deleting cars
175    pub fn cancel_request(&mut self, agent: AgentID, turn: TurnID) {
176        let state = self.state.get_mut(&turn.parent).unwrap();
177        state.waiting.remove(&Request { agent, turn });
178        if self.break_turn_conflict_cycles {
179            if let AgentID::Car(car) = agent {
180                self.blocked_by.retain(|(c1, c2)| *c1 != car && *c2 != car);
181            }
182        }
183    }
184
185    pub fn space_freed(
186        &mut self,
187        now: Time,
188        i: IntersectionID,
189        scheduler: &mut Scheduler,
190        map: &Map,
191    ) {
192        self.wakeup_waiting(now, i, scheduler, map);
193    }
194
195    /// Vanished at border, stopped biking, etc -- a vehicle disappeared, and didn't have one last
196    /// turn.
197    pub fn vehicle_gone(&mut self, car: CarID) {
198        self.blocked_by.retain(|(c1, c2)| *c1 != car && *c2 != car);
199    }
200
201    pub fn agent_deleted_mid_turn(&mut self, agent: AgentID, turn: TurnID) {
202        let state = self.state.get_mut(&turn.parent).unwrap();
203        assert!(state.accepted.remove(&Request { agent, turn }));
204
205        // This agent might have a few more nearby turns reserved, because they're part of an
206        // uber-turn. It's a blunt response to just clear them all out, but it should be correct.
207        for state in self.state.values_mut() {
208            state.reserved.retain(|req| req.agent != agent);
209        }
210    }
211
212    fn wakeup_waiting(&self, now: Time, i: IntersectionID, scheduler: &mut Scheduler, map: &Map) {
213        let mut all: Vec<(Request, Time, bool)> = self.state[&i]
214            .waiting
215            .iter()
216            .map(|(r, (t, urgent))| (r.clone(), *t, *urgent))
217            .collect();
218        // Sort by waiting time, so things like stop signs actually are first-come, first-served.
219        // But with an override: if somebody is currently on a queue that's overflowing, they're
220        // very likely to be part of a cycle causing gridlock. Let them go first.
221        all.sort_by_key(|(_, t, urgent)| (!*urgent, *t));
222
223        // Wake up Priority turns before Yield turns. Don't wake up Banned turns at all. This makes
224        // sure priority vehicles should get the head-start, without blocking yield vehicles
225        // unnecessarily.
226        let mut protected = Vec::new();
227        let mut yielding = Vec::new();
228
229        if self.use_freeform_policy_everywhere {
230            for (req, _, _) in all {
231                protected.push(req);
232            }
233        } else if let Some(signal) = map.maybe_get_traffic_signal(i) {
234            let current_stage = self.state[&i].signal.as_ref().unwrap().current_stage;
235            let stage = &signal.stages[current_stage];
236            let reserved = &self.state[&i].reserved;
237            let i = map.get_i(i);
238            for (req, _, _) in all {
239                match stage.get_priority_of_turn(req.turn, i) {
240                    TurnPriority::Protected => {
241                        protected.push(req);
242                    }
243                    TurnPriority::Yield => {
244                        yielding.push(req);
245                    }
246                    // No need to wake up unless it has reserved
247                    TurnPriority::Banned => {
248                        if reserved.contains(&req) {
249                            protected.push(req);
250                        }
251                    }
252                }
253            }
254        } else if let Some(sign) = map.maybe_get_stop_sign(i) {
255            for (req, _, _) in all {
256                match sign.get_priority(req.turn, map) {
257                    TurnPriority::Protected => {
258                        protected.push(req);
259                    }
260                    TurnPriority::Yield => {
261                        yielding.push(req);
262                    }
263                    TurnPriority::Banned => unreachable!(),
264                }
265            }
266        } else {
267            // This could either be a border intersection or an intersection that was just closed
268            // in the middle of simulation. In either case, there shouldn't be any other turns at
269            // it.
270            assert!(protected.is_empty());
271            assert!(yielding.is_empty());
272        };
273        protected.extend(yielding);
274
275        // We now have all the requests in the order that we want to wake them up. The scheduler
276        // arbitrarily (but deterministically) orders commands with the same time, so preserve the
277        // ordering by adding little epsilons.
278        let mut delay = Duration::ZERO;
279        for req in protected {
280            // Use update because multiple agents could finish a turn at the same time, before the
281            // waiting one has a chance to try again.
282            scheduler.update(now + delay, Command::update_agent(req.agent));
283            delay += Duration::EPSILON;
284        }
285    }
286
287    /// This is only triggered for traffic signals.
288    pub fn update_intersection(
289        &mut self,
290        now: Time,
291        id: IntersectionID,
292        map: &Map,
293        scheduler: &mut Scheduler,
294    ) {
295        let i = map.get_i(id);
296
297        // trivial function that advances the signal stage and returns duration
298        fn advance(
299            signal_state: &mut SignalState,
300            signal: &ControlTrafficSignal,
301            i: &Intersection,
302            allow_crosswalk_skip: bool,
303        ) -> Duration {
304            signal_state.current_stage = (signal_state.current_stage + 1) % signal.stages.len();
305            let stage = &signal.stages[signal_state.current_stage];
306            // only skip for variable all-walk crosswalk
307            if let StageType::Variable(_, _, _) = stage.stage_type {
308                if allow_crosswalk_skip && stage.max_crosswalk_time(i).is_some() {
309                    // we can skip this stage, as its all walk and we're allowed to skip (no
310                    // pedestrian waiting).
311                    signal_state.current_stage =
312                        (signal_state.current_stage + 1) % signal.stages.len();
313                }
314            }
315            signal.stages[signal_state.current_stage]
316                .stage_type
317                .simple_duration()
318        }
319        let state = self.state.get_mut(&id).unwrap();
320        let signal_state = state.signal.as_mut().unwrap();
321        let signal = map.get_traffic_signal(id);
322        let ped_waiting = state.waiting.keys().any(|req| {
323            if let AgentID::Pedestrian(_) = req.agent {
324                return true;
325            }
326            false
327        });
328        let duration: Duration;
329        // Switch to a new stage?
330        assert_eq!(now, signal_state.stage_ends_at);
331        let old_stage = &signal.stages[signal_state.current_stage];
332        match old_stage.stage_type {
333            StageType::Fixed(_) => {
334                duration = advance(signal_state, signal, i, !ped_waiting);
335            }
336            StageType::Variable(min, delay, additional) => {
337                // test if anyone is waiting in current stage, and if so, extend the signal cycle.
338                // Filter out pedestrians, as they've had their chance and the delay
339                // could be short enough to keep them on the curb.
340                let delay = std::cmp::max(Duration::const_seconds(1.0), delay);
341                // Only extend for the fixed additional time
342                if signal_state.extensions_count as f64 * delay.inner_seconds()
343                    >= additional.inner_seconds()
344                {
345                    self.events.push(Event::Alert(
346                        AlertLocation::Intersection(id),
347                        format!(
348                            "exhausted a variable stage {},{},{},{}",
349                            min, delay, additional, signal_state.extensions_count
350                        ),
351                    ));
352                    duration = advance(signal_state, signal, i, !ped_waiting);
353                    signal_state.extensions_count = 0;
354                } else if state.waiting.keys().all(|req| {
355                    if let AgentID::Pedestrian(_) = req.agent {
356                        return true;
357                    }
358                    // Should we only allow protected to extend or any not banned?
359                    // currently only the protected demand control extended.
360                    old_stage.get_priority_of_turn(req.turn, i) != TurnPriority::Protected
361                }) {
362                    signal_state.extensions_count = 0;
363                    duration = advance(signal_state, signal, i, !ped_waiting);
364                } else {
365                    signal_state.extensions_count += 1;
366                    duration = delay;
367                    self.events.push(Event::Alert(
368                        AlertLocation::Intersection(id),
369                        format!(
370                            "Extending a variable stage {},{},{},{}",
371                            min, delay, additional, signal_state.extensions_count
372                        ),
373                    ));
374                }
375            }
376        }
377
378        signal_state.stage_ends_at = now + duration;
379        scheduler.push(signal_state.stage_ends_at, Command::UpdateIntersection(id));
380        self.wakeup_waiting(now, id, scheduler, map);
381    }
382
383    /// For cars: The head car calls this when they're at the end of the lane WaitingToAdvance. If
384    /// this returns true, then the head car MUST actually start this turn.
385    /// For peds: Likewise -- only called when the ped is at the start of the turn. They must
386    /// actually do the turn if this returns true.
387    ///
388    /// If this returns false, the agent should NOT retry. IntersectionSimState will schedule a
389    /// retry event at some point.
390    pub fn maybe_start_turn(
391        &mut self,
392        agent: AgentID,
393        turn: TurnID,
394        speed: Speed,
395        now: Time,
396        map: &Map,
397        scheduler: &mut Scheduler,
398        maybe_cars_and_queues: Option<(
399            &Car,
400            &FixedMap<CarID, Car>,
401            &mut HashMap<Traversable, Queue>,
402        )>,
403    ) -> bool {
404        #![allow(clippy::logic_bug)] // Remove once TODO below is taken care of
405        let req = Request { agent, turn };
406
407        if let Some(_eta) = self
408            .state
409            .get_mut(&turn.parent)
410            .unwrap()
411            .leader_eta
412            .remove(&req.turn.src)
413        {
414            // When they're late, it's because of a slow laggy head. Conflicting turns would've
415            // been blocked anyway. Uncomment to debug
416            //info!("{} predicted ETA {}, actually {}", req.agent, eta, now);
417        }
418
419        let entry = self
420            .state
421            .get_mut(&turn.parent)
422            .unwrap()
423            .waiting
424            .entry(req.clone());
425        let repeat_request = match entry {
426            std::collections::btree_map::Entry::Vacant(_) => false,
427            std::collections::btree_map::Entry::Occupied(_) => true,
428        };
429        let urgent = if let Some((car, _, queues)) = maybe_cars_and_queues.as_ref() {
430            queues[&car.router.head()].is_overflowing()
431        } else {
432            false
433        };
434        entry.or_insert((now, urgent));
435
436        if repeat_request {
437            self.total_repeat_requests += 1;
438        }
439
440        let shared_sidewalk_corner =
441            map.get_t(req.turn).turn_type == TurnType::SharedSidewalkCorner;
442
443        let readonly_pair = maybe_cars_and_queues.as_ref().map(|(_, c, q)| (*c, &**q));
444        let started_uber_turn = |state: &Self, car: &Car| {
445            state.handle_uber_turns && car.router.get_path().currently_inside_ut().is_some()
446        };
447        #[allow(clippy::if_same_then_else)]
448        let allowed = if shared_sidewalk_corner {
449            // SharedSidewalkCorner doesn't conflict with anything -- fastpath!
450            true
451        } else if !self.handle_accepted_conflicts(&req, map, readonly_pair, Some((now, scheduler)))
452        {
453            // It's never OK to perform a conflicting turn
454            false
455        } else if maybe_cars_and_queues
456            .as_ref()
457            .map(|(car, _, _)| started_uber_turn(self, *car))
458            .unwrap_or(false)
459        {
460            // If we started an uber-turn, then finish it! But alert if we're running a red light.
461            // TODO: Consider reenabling alert
462            if let Some(signal) = map.maybe_get_traffic_signal(turn.parent) {
463                // Don't pass in the scheduler, aka, don't pause before yielding.
464                if !self.traffic_signal_policy(&req, map, signal, speed, now, None) && false {
465                    self.events.push(Event::Alert(
466                        AlertLocation::Intersection(req.turn.parent),
467                        format!("Running a red light inside an uber-turn: {:?}", req),
468                    ));
469                }
470            }
471
472            true
473        } else if self.use_freeform_policy_everywhere {
474            // If we made it this far, we don't conflict with an accepted turn
475            true
476        } else if let Some(signal) = map.maybe_get_traffic_signal(turn.parent) {
477            self.traffic_signal_policy(&req, map, signal, speed, now, Some(scheduler))
478        } else if let Some(sign) = map.maybe_get_stop_sign(turn.parent) {
479            self.stop_sign_policy(&req, map, sign, speed, now, scheduler)
480        } else {
481            unreachable!()
482        };
483        if !allowed {
484            if repeat_request {
485                self.not_allowed_requests += 1;
486            }
487            // remove the reservation if we're about to start a UT and can't move
488            if self.handle_uber_turns {
489                if let Some(ut) = maybe_cars_and_queues
490                    .as_ref()
491                    .and_then(|(car, _, _)| car.router.get_path().about_to_start_ut())
492                {
493                    for t in &ut.path {
494                        self.state
495                            .get_mut(&t.parent)
496                            .unwrap()
497                            .reserved
498                            .remove(&Request { agent, turn: *t });
499                    }
500                }
501            }
502            return false;
503        }
504
505        // Lock the entire uber-turn.
506        if self.handle_uber_turns {
507            if let Some(ut) = maybe_cars_and_queues
508                .as_ref()
509                .and_then(|(car, _, _)| car.router.get_path().about_to_start_ut())
510            {
511                // If there's a problem up ahead, don't start.
512                for t in &ut.path {
513                    let req = Request { agent, turn: *t };
514                    if !self.handle_accepted_conflicts(&req, map, readonly_pair, None) {
515                        if repeat_request {
516                            self.blocked_by_someone_requests += 1;
517                        }
518                        return false;
519                    }
520                }
521                // If the way is clear, make sure it stays that way.
522                for t in &ut.path {
523                    self.state
524                        .get_mut(&t.parent)
525                        .unwrap()
526                        .reserved
527                        .insert(Request { agent, turn: *t });
528                }
529            }
530        }
531
532        // Don't block the box.
533        if let Some((car, cars, queues)) = maybe_cars_and_queues {
534            assert_eq!(agent, AgentID::Car(car.vehicle.id));
535            let inside_ut = self.handle_uber_turns
536                && (car.router.get_path().currently_inside_ut().is_some()
537                    || car.router.get_path().about_to_start_ut().is_some());
538            let queue = queues.get_mut(&Traversable::Lane(turn.dst)).unwrap();
539            if !queue.try_to_reserve_entry(
540                car,
541                !self.dont_block_the_box
542                    || allow_block_the_box(map.get_i(turn.parent))
543                    || inside_ut,
544            ) {
545                let mut actually_did_reserve_entry = false;
546                if self.break_turn_conflict_cycles {
547                    if let Some(c) = queue.laggy_head {
548                        self.blocked_by.insert((car.vehicle.id, c));
549                    } else if let Some(c) = queue.get_active_cars().get(0) {
550                        self.blocked_by.insert((car.vehicle.id, *c));
551                    } else {
552                        // try_to_reserve_entry must have failed because somebody has filled up
553                        // reserved_length. That only happens while a turn is in progress, so this
554                        // unwrap() must work.
555                        let blocking_req = self.state[&turn.parent]
556                            .accepted
557                            .iter()
558                            .find(|r| r.turn.dst == turn.dst)
559                            .unwrap();
560                        self.blocked_by
561                            .insert((car.vehicle.id, blocking_req.agent.as_car()));
562                    }
563
564                    // Allow blocking the box if we're part of a cycle.
565                    if self
566                        .detect_conflict_cycle(car.vehicle.id, (cars, queues))
567                        .is_some()
568                    {
569                        // Reborrow
570                        let queue = queues.get_mut(&Traversable::Lane(turn.dst)).unwrap();
571                        actually_did_reserve_entry = queue.try_to_reserve_entry(car, true);
572                    }
573                }
574
575                if !actually_did_reserve_entry {
576                    if repeat_request {
577                        self.blocked_by_someone_requests += 1;
578                    }
579                    return false;
580                }
581            }
582        }
583
584        // TODO For now, we're only interested in signals, and there's too much raw data to store
585        // for stop signs too.
586        let state = self.state.get_mut(&turn.parent).unwrap();
587        state.waiting.remove(&req).unwrap();
588        state.accepted.insert(req);
589        if self.break_turn_conflict_cycles {
590            if let AgentID::Car(car) = agent {
591                self.blocked_by.retain(|(c, _)| *c != car);
592            }
593        }
594        true
595    }
596
597    pub fn collect_events(&mut self) -> Vec<Event> {
598        std::mem::take(&mut self.events)
599    }
600
601    pub fn handle_live_edited_traffic_signals(
602        &mut self,
603        now: Time,
604        map: &Map,
605        scheduler: &mut Scheduler,
606    ) {
607        for state in self.state.values_mut() {
608            match (
609                map.maybe_get_traffic_signal(state.id),
610                state.signal.as_mut(),
611            ) {
612                (Some(ts), Some(signal_state)) => {
613                    if signal_state.current_stage >= ts.stages.len() {
614                        // Just jump back to the first one. Shrug.
615                        signal_state.current_stage = 0;
616                        println!(
617                            "WARNING: Traffic signal {} was live-edited in the middle of a stage, \
618                             so jumping back to the first stage",
619                            state.id
620                        );
621                    }
622                }
623                (Some(_), None) => {
624                    state.signal = Some(SignalState::new(state.id, now, map, scheduler));
625                }
626                (None, Some(_)) => {
627                    state.signal = None;
628                    scheduler.cancel(Command::UpdateIntersection(state.id));
629                }
630                (None, None) => {}
631            }
632
633            // It's unlikely, but the player might create/destroy traffic signals close together and
634            // change the uber-turns that exist. To be safe, recalculate everywhere.
635            state.uber_turn_neighbors.clear();
636            if let Some(mut set) = map_model::IntersectionCluster::autodetect(state.id, map) {
637                set.remove(&state.id);
638                state.uber_turn_neighbors.extend(set);
639            }
640        }
641    }
642
643    pub fn handle_live_edits(&self, map: &Map) {
644        // Just sanity check that we don't have any references to deleted turns
645        let mut errors = Vec::new();
646        for state in self.state.values() {
647            for req in &state.accepted {
648                if map.maybe_get_t(req.turn).is_none() {
649                    errors.push(format!("{} accepted for {}", req.agent, req.turn));
650                }
651            }
652            for req in state.waiting.keys() {
653                if map.maybe_get_t(req.turn).is_none() {
654                    errors.push(format!("{} waiting for {}", req.agent, req.turn));
655                }
656            }
657            for req in &state.reserved {
658                if map.maybe_get_t(req.turn).is_none() {
659                    errors.push(format!("{} has reserved {}", req.agent, req.turn));
660                }
661            }
662        }
663        if !errors.is_empty() {
664            for x in errors {
665                error!("{}", x);
666            }
667            panic!("After live map edits, intersection state refers to deleted turns!");
668        }
669    }
670
671    // Not calling this for pedestrians right now.
672    // This is "best effort". If we get something wrong, somebody might start a turn and cut off an
673    // approaching vehicle.
674    // And it's idempotent -- can call to update an ETA.
675    pub fn approaching_leader(&mut self, agent: AgentID, turn: TurnID, eta: Time) {
676        let state = self.state.get_mut(&turn.parent).unwrap();
677        // If there was a previous entry here for turn.src, then this leader is spawning in front
678        // of the previous leader on a driveway
679        state
680            .leader_eta
681            .insert(turn.src, (Request { agent, turn }, eta));
682    }
683}
684
685// Queries
686impl IntersectionSimState {
687    pub fn nobody_headed_towards(&self, lane: LaneID, i: IntersectionID) -> bool {
688        let state = &self.state[&i];
689        !state
690            .accepted
691            .iter()
692            .chain(state.reserved.iter())
693            .any(|req| req.turn.dst == lane)
694    }
695
696    pub fn debug_json(&self, id: IntersectionID, map: &Map) -> String {
697        let json1 = abstutil::to_json(&self.state[&id]);
698        let json2 = if let Some(ref sign) = map.maybe_get_stop_sign(id) {
699            abstutil::to_json(sign)
700        } else if let Some(ref signal) = map.maybe_get_traffic_signal(id) {
701            abstutil::to_json(signal)
702        } else {
703            "\"Border\"".to_string()
704        };
705        format!("[{json1}, {json2}]")
706    }
707
708    pub fn get_accepted_agents(&self, id: IntersectionID) -> Vec<(AgentID, TurnID)> {
709        self.state[&id]
710            .accepted
711            .iter()
712            .map(|req| (req.agent, req.turn))
713            .collect()
714    }
715
716    pub fn get_waiting_agents(&self, id: IntersectionID) -> Vec<(AgentID, TurnID, Time)> {
717        self.state[&id]
718            .waiting
719            .iter()
720            .map(|(req, (time, _))| (req.agent, req.turn, *time))
721            .collect()
722    }
723
724    /// Returns intersections with travelers waiting for at least `threshold` since `now`, ordered
725    /// so the longest delayed intersection is first.
726    pub fn delayed_intersections(
727        &self,
728        now: Time,
729        threshold: Duration,
730    ) -> Vec<(IntersectionID, Time)> {
731        let mut candidates = Vec::new();
732        for state in self.state.values() {
733            if let Some((earliest, _)) = state.waiting.values().min() {
734                if now - *earliest >= threshold {
735                    candidates.push((state.id, *earliest));
736                }
737            }
738        }
739        candidates.sort_by_key(|(_, t)| *t);
740        candidates
741    }
742
743    pub fn current_stage_and_remaining_time(
744        &self,
745        now: Time,
746        i: IntersectionID,
747    ) -> (usize, Duration) {
748        let state = &self.state[&i].signal.as_ref().unwrap();
749        if now > state.stage_ends_at {
750            panic!(
751                "At {}, but {} should have advanced its stage at {}",
752                now, i, state.stage_ends_at
753            );
754        }
755        (state.current_stage, state.stage_ends_at - now)
756    }
757
758    pub fn describe_stats(&self) -> Vec<String> {
759        vec![
760            "intersection stats".to_string(),
761            format!(
762                "{} total turn requests repeated after the initial attempt",
763                prettyprint_usize(self.total_repeat_requests)
764            ),
765            format!(
766                "{} not allowed by intersection ({}%)",
767                prettyprint_usize(self.not_allowed_requests),
768                (100.0 * (self.not_allowed_requests as f64) / (self.total_repeat_requests as f64))
769                    .round()
770            ),
771            format!(
772                "{} blocked by someone in the way ({}%)",
773                prettyprint_usize(self.blocked_by_someone_requests),
774                (100.0 * (self.blocked_by_someone_requests as f64)
775                    / (self.total_repeat_requests as f64))
776                    .round()
777            ),
778        ]
779    }
780
781    pub fn populate_blocked_by(
782        &self,
783        now: Time,
784        graph: &mut BTreeMap<AgentID, (Duration, DelayCause)>,
785        map: &Map,
786        cars: &FixedMap<CarID, Car>,
787        queues: &HashMap<Traversable, Queue>,
788    ) {
789        // Don't use self.blocked_by -- that gets complicated with uber-turns and such.
790        //
791        // This also assumes default values for handle_uber_turns, disable_turn_conflicts, etc!
792        for state in self.state.values() {
793            for (req, (started_at, _)) in &state.waiting {
794                let turn = map.get_t(req.turn);
795                // In the absence of other explanations, the agent must be pausing at a stop sign
796                // or before making an unprotected movement, aka, in the middle of
797                // WAIT_AT_STOP_SIGN or WAIT_BEFORE_YIELD_AT_TRAFFIC_SIGNAL. Or they're waiting for
798                // a signal to change.
799                let mut cause = DelayCause::Intersection(state.id);
800                if let Some(other) = state.accepted.iter().find(|other| {
801                    turn.conflicts_with(map.get_t(other.turn)) || turn.id == other.turn
802                }) {
803                    cause = DelayCause::Agent(other.agent);
804                } else if let AgentID::Car(car) = req.agent {
805                    let queue = &queues[&Traversable::Lane(req.turn.dst)];
806                    let car = cars.get(&car).unwrap();
807                    if !queue.room_for_car(car) {
808                        // TODO Or it's reserved due to an uber turn or something
809                        let blocker = queue
810                            .get_active_cars()
811                            .last()
812                            .cloned()
813                            .or(queue.laggy_head)
814                            .unwrap();
815                        cause = DelayCause::Agent(AgentID::Car(blocker));
816                    } else if let Some(ut) = car.router.get_path().about_to_start_ut() {
817                        if let Some(blocker) = self.check_for_conflicts_before_uber_turn(ut, map) {
818                            cause = DelayCause::Agent(blocker);
819                        }
820                    }
821                }
822                graph.insert(req.agent, (now - *started_at, cause));
823            }
824        }
825    }
826
827    /// See if any agent is currently performing a turn that conflicts with an uber-turn. Doesn't
828    /// check for room on the queues.
829    fn check_for_conflicts_before_uber_turn(&self, ut: &UberTurn, map: &Map) -> Option<AgentID> {
830        for t in &ut.path {
831            let turn = map.get_t(*t);
832            let state = &self.state[&turn.id.parent];
833            for other in state.accepted.iter().chain(state.reserved.iter()) {
834                if map.get_t(other.turn).conflicts_with(turn) {
835                    return Some(other.agent);
836                }
837            }
838        }
839        None
840    }
841}
842
843// Stuff to support maybe_start_turn
844impl IntersectionSimState {
845    fn stop_sign_policy(
846        &mut self,
847        req: &Request,
848        map: &Map,
849        sign: &ControlStopSign,
850        speed: Speed,
851        now: Time,
852        scheduler: &mut Scheduler,
853    ) -> bool {
854        let our_priority = sign.get_priority(req.turn, map);
855        assert!(our_priority != TurnPriority::Banned);
856        let (our_time, _) = self.state[&req.turn.parent].waiting[req];
857
858        if our_priority == TurnPriority::Yield && now < our_time + WAIT_AT_STOP_SIGN {
859            // Since we have "ownership" of scheduling for req.agent, don't need to use
860            // scheduler.update.
861            scheduler.push(
862                our_time + WAIT_AT_STOP_SIGN,
863                Command::update_agent(req.agent),
864            );
865            return false;
866        }
867
868        // Once upon a time, we'd make sure that this request doesn't conflict with another in
869        // self.waiting:
870        // 1) Higher-ranking turns get to go first.
871        // 2) Equal-ranking turns that started waiting before us get to go first.
872        // But the exceptions started stacking -- if the other agent is blocked or the turns don't
873        // even conflict, then allow it. Except determining if the other agent is blocked or not is
874        // tough and kind of recursive.
875        //
876        // So instead, don't do any of that! The WAIT_AT_STOP_SIGN scheduling above and the fact
877        // that events are processed in time order mean that case #2 is magically handled anyway.
878        // If a case #1 could've started by now, then they would have. Since they didn't, they must
879        // be blocked.
880
881        // TODO Make sure we can optimistically finish this turn before an approaching
882        // higher-priority vehicle wants to begin.
883
884        // If a pedestrian is going to cut off a car, check how long the car has been waiting and
885        // maybe yield (regardless of stop sign priority). This is a very rough start to more
886        // realistic "batching" of pedestrians to cross a street. Without this, if there's one
887        // pedestrian almost clear of a crosswalk, cars are totally stopped for them, and so a new
888        // pedestrian arriving will win.
889        if req.agent.is_pedestrian() {
890            let our_turn = map.get_t(req.turn);
891            let time_to_cross = our_turn.geom.length() / speed;
892            for (other_req, (other_time, _)) in &self.state[&req.turn.parent].waiting {
893                if matches!(other_req.agent, AgentID::Car(_)) {
894                    if our_turn.conflicts_with(map.get_t(other_req.turn)) {
895                        let our_waiting = now - our_time;
896                        let other_waiting = now - *other_time;
897                        // We can't tell if a car has been waiting for a while due to pedestrians
898                        // crossing, or due to a blockage in their destination queue. Always let
899                        // pedestrians muscle their way in eventually.
900                        if our_waiting > other_waiting {
901                            continue;
902                        }
903                        // Intuition: another pedestrian trying to enter a crosswalk has a 3s
904                        // buffer to "join" the first pedestrian who started crossing and caused
905                        // cars to stop. We're using the time for _this_ pedestrian to cross _this_
906                        // turn, so it's a very rough definition.
907                        if other_waiting > time_to_cross + Duration::seconds(3.0) {
908                            return false;
909                        }
910                    }
911                }
912            }
913        }
914
915        true
916    }
917
918    fn traffic_signal_policy(
919        &mut self,
920        req: &Request,
921        map: &Map,
922        signal: &ControlTrafficSignal,
923        speed: Speed,
924        now: Time,
925        scheduler: Option<&mut Scheduler>,
926    ) -> bool {
927        let turn = map.get_t(req.turn);
928
929        let state = &self.state[&req.turn.parent];
930        let signal_state = state.signal.as_ref().unwrap();
931        let stage = &signal.stages[signal_state.current_stage];
932        let full_stage_duration = stage.stage_type.simple_duration();
933        let remaining_stage_time = signal_state.stage_ends_at - now;
934        let (our_time, _) = state.waiting[req];
935
936        // Can't go at all this stage.
937        let our_priority = stage.get_priority_of_turn(req.turn, map.get_i(state.id));
938        if our_priority == TurnPriority::Banned {
939            return false;
940        }
941
942        if our_priority == TurnPriority::Yield
943            && now < our_time + WAIT_BEFORE_YIELD_AT_TRAFFIC_SIGNAL
944        {
945            // Since we have "ownership" of scheduling for req.agent, don't need to use
946            // scheduler.update.
947            if let Some(s) = scheduler {
948                s.push(
949                    our_time + WAIT_BEFORE_YIELD_AT_TRAFFIC_SIGNAL,
950                    Command::update_agent(req.agent),
951                );
952            }
953            return false;
954        }
955
956        // Previously: A yield loses to a conflicting Priority turn.
957        // But similar to the description in stop_sign_policy, this caused unnecessary gridlock.
958        // Priority vehicles getting scheduled first just requires a little tweak in
959        // update_intersection.
960
961        // TODO Make sure we can optimistically finish this turn before an approaching
962        // higher-priority vehicle wants to begin.
963
964        // Optimistically if nobody else is in the way, this is how long it'll take to finish the
965        // turn. Don't start the turn if we won't finish by the time the light changes. If we get
966        // it wrong, that's fine -- block the box a bit.
967        let time_to_cross = turn.geom.length() / speed;
968        if time_to_cross > remaining_stage_time {
969            // Signals enforce a minimum crosswalk time, but some pedestrians are configured to
970            // walk very slowly. In that case, allow them to go anyway and wind up in the crosswalk
971            // during a red. This matches reality reasonably.
972            if time_to_cross <= full_stage_duration {
973                return false;
974            }
975        }
976
977        true
978    }
979
980    // If true, the request can go.
981    fn handle_accepted_conflicts(
982        &mut self,
983        req: &Request,
984        map: &Map,
985        maybe_cars_and_queues: Option<(&FixedMap<CarID, Car>, &HashMap<Traversable, Queue>)>,
986        wakeup_stuck_cycle: Option<(Time, &mut Scheduler)>,
987    ) -> bool {
988        let turn = map.get_t(req.turn);
989        let mut cycle_detected = false;
990        let mut ok = true;
991        for other in self.state[&req.turn.parent]
992            .accepted
993            .iter()
994            .chain(self.state[&req.turn.parent].reserved.iter())
995        {
996            // Never short-circuit; always record all of the dependencies; it might help someone
997            // else unstick things.
998            if map.get_t(other.turn).conflicts_with(turn) {
999                if self.break_turn_conflict_cycles {
1000                    if let AgentID::Car(c) = req.agent {
1001                        if let AgentID::Car(c2) = other.agent {
1002                            self.blocked_by.insert((c, c2));
1003                        }
1004                        if !cycle_detected {
1005                            if let Some(cycle) =
1006                                self.detect_conflict_cycle(c, maybe_cars_and_queues.unwrap())
1007                            {
1008                                // Allow the conflicting turn!
1009                                self.events.push(Event::Alert(
1010                                    AlertLocation::Intersection(req.turn.parent),
1011                                    format!(
1012                                        "{} found turn conflict cycle involving {:?}",
1013                                        req.agent, cycle
1014                                    ),
1015                                ));
1016                                cycle_detected = true;
1017                            }
1018                        }
1019                    }
1020                }
1021
1022                if !cycle_detected && !self.disable_turn_conflicts {
1023                    ok = false;
1024                }
1025
1026                // It's never safe for two vehicles to go for the same lane.
1027                // TODO I'm questioning this now. If the source is the same, then queueing will
1028                // work normally. If not, then... maybe we need to allow concurrent turns from
1029                // different lanes into the same lane, and somehow make the queueing work out.
1030                if turn.id.dst == other.turn.dst {
1031                    if let (Some((now, scheduler)), AgentID::Car(blocker), Some((cars, _))) = (
1032                        wakeup_stuck_cycle,
1033                        other.agent,
1034                        maybe_cars_and_queues.as_ref(),
1035                    ) {
1036                        // Sometimes the vehicle blocking us is actually queued in the turn;
1037                        // don't wake them up in that case.
1038                        if cycle_detected
1039                            && matches!(cars[&blocker].state, CarState::WaitingToAdvance { .. })
1040                        {
1041                            self.events.push(Event::Alert(
1042                                AlertLocation::Intersection(req.turn.parent),
1043                                format!(
1044                                    "{} waking up {}, who's blocking it as part of a cycle",
1045                                    req.agent, other.agent
1046                                ),
1047                            ));
1048                            scheduler.update(
1049                                now + Duration::EPSILON,
1050                                Command::update_agent(other.agent),
1051                            );
1052                        }
1053                    }
1054                    return false;
1055                }
1056            }
1057        }
1058        ok
1059    }
1060
1061    fn detect_conflict_cycle(
1062        &self,
1063        car: CarID,
1064        pair: (&FixedMap<CarID, Car>, &HashMap<Traversable, Queue>),
1065    ) -> Option<HashSet<CarID>> {
1066        let (cars, queues) = pair;
1067
1068        let mut queue = vec![car];
1069        let mut seen = HashSet::new();
1070        while !queue.is_empty() {
1071            let current = queue.pop().unwrap();
1072            // Might not actually be a cycle. Insist on seeing the original req.agent
1073            // again.
1074            if !seen.is_empty() && current == car {
1075                return Some(seen);
1076            }
1077            if !seen.contains(&current) {
1078                seen.insert(current);
1079
1080                for (c1, c2) in &self.blocked_by {
1081                    if *c1 == current {
1082                        queue.push(*c2);
1083                    }
1084                }
1085
1086                // If this car isn't the head of its queue, add that dependency. (Except for
1087                // the original car, which we already know is the head of its queue)
1088                // TODO Maybe store this in blocked_by?
1089                if current != car {
1090                    let q = &queues[&cars[&current].router.head()];
1091                    let head = if let Some(c) = q.laggy_head {
1092                        c
1093                    } else {
1094                        q.get_active_cars()[0]
1095                    };
1096                    if current != head {
1097                        queue.push(head);
1098                    }
1099                }
1100            }
1101        }
1102        None
1103    }
1104}
1105
1106impl SignalState {
1107    fn new(id: IntersectionID, now: Time, map: &Map, scheduler: &mut Scheduler) -> SignalState {
1108        let mut state = SignalState {
1109            current_stage: 0,
1110            stage_ends_at: now,
1111            extensions_count: 0,
1112        };
1113
1114        let signal = map.get_traffic_signal(id);
1115        // What stage are we starting with?
1116        let mut offset = (now - Time::START_OF_DAY) + signal.offset;
1117        loop {
1118            let dt = signal.stages[state.current_stage]
1119                .stage_type
1120                .simple_duration();
1121            if offset >= dt {
1122                offset -= dt;
1123                state.current_stage += 1;
1124                if state.current_stage == signal.stages.len() {
1125                    state.current_stage = 0;
1126                }
1127            } else {
1128                state.stage_ends_at = now + dt - offset;
1129                break;
1130            }
1131        }
1132        scheduler.push(state.stage_ends_at, Command::UpdateIntersection(id));
1133        state
1134    }
1135}
1136
1137fn allow_block_the_box(i: &Intersection) -> bool {
1138    // Degenerate intersections are often just artifacts of how roads are split up in OSM. Allow
1139    // vehicles to get stuck in them, since the only possible thing they could block is pedestrians
1140    // from using the crosswalk. Those crosswalks usually don't exist in reality, so this behavior
1141    // is more realistic.
1142    if i.roads.len() == 2 {
1143        return true;
1144    }
1145
1146    // TODO Sometimes a traffic signal is surrounded by tiny lanes with almost no capacity.
1147    // Workaround for now.
1148    //
1149    // When adding new cases:
1150    // 1) Organize by which map the intersection fixes
1151    // 2) Ensure a prebaked scenario covers this, to track regressions and make sure it actually
1152    //    helps.
1153    let id = i.orig_id.0;
1154    // lakeslice
1155    if id == 53211693
1156        || id == 53214134
1157        || id == 53214133
1158        || id == 987334546
1159        || id == 848817336
1160        || id == 1726088131
1161        || id == 1726088130
1162        || id == 53217946
1163        || id == 53223864
1164        || id == 53211694
1165        || id == 5440360144
1166        || id == 246768814
1167    {
1168        return true;
1169    }
1170    // poundbury
1171    if id == 18030505 || id == 2124133018 || id == 30024649 {
1172        return true;
1173    }
1174    false
1175}