1 use crate::mechanics::car::Car;
2 use crate::mechanics::Queue;
3 use crate::{AgentID, AlertLocation, CarID, Command, Event, Scheduler, Speed};
4 use abstutil::{deserialize_btreemap, retain_btreeset, serialize_btreemap};
5 use geom::{Duration, Time};
6 use map_model::{
7 ControlStopSign, ControlTrafficSignal, IntersectionID, LaneID, Map, PhaseType, RoadID,
8 Traversable, TurnID, TurnPriority, TurnType,
9 };
10 use serde::{Deserialize, Serialize};
11 use std::collections::{BTreeMap, BTreeSet, HashSet};
12
13 const WAIT_AT_STOP_SIGN: Duration = Duration::const_seconds(0.5);
14 const WAIT_BEFORE_YIELD_AT_TRAFFIC_SIGNAL: Duration = Duration::const_seconds(0.2);
15
16 #[derive(Serialize, Deserialize, Clone)]
17 pub struct IntersectionSimState {
18 state: BTreeMap<IntersectionID, State>,
19 use_freeform_policy_everywhere: bool,
20 dont_block_the_box: bool,
21 break_turn_conflict_cycles: bool,
22 handle_uber_turns: bool,
23 // (x, y) means x is blocked by y. It's a many-to-many relationship. TODO Better data
24 // structure.
25 blocked_by: BTreeSet<(CarID, CarID)>,
26 events: Vec<Event>,
27 }
28
29 #[derive(Clone, Serialize, Deserialize)]
30 struct State {
31 id: IntersectionID,
32 accepted: BTreeSet<Request>,
33 // Track when a request is first made.
34 #[serde(
35 serialize_with = "serialize_btreemap",
36 deserialize_with = "deserialize_btreemap"
37 )]
38 waiting: BTreeMap<Request, Time>,
39 // When a vehicle begins an uber-turn, reserve the future turns to ensure they're able to
40 // complete the entire sequence. This is especially necessary since groups of traffic signals
41 // are not yet configured as one.
42 reserved: BTreeSet<Request>,
43
44 // Only relevant for traffic signals
45 current_stage: usize,
46 stage_ends_at: Time,
47 }
48
49 #[derive(PartialEq, Eq, PartialOrd, Ord, Serialize, Deserialize, Clone, Debug)]
50 struct Request {
51 agent: AgentID,
52 turn: TurnID,
53 }
54
55 impl IntersectionSimState {
new( map: &Map, scheduler: &mut Scheduler, use_freeform_policy_everywhere: bool, dont_block_the_box: bool, break_turn_conflict_cycles: bool, handle_uber_turns: bool, ) -> IntersectionSimState56 pub fn new(
57 map: &Map,
58 scheduler: &mut Scheduler,
59 use_freeform_policy_everywhere: bool,
60 dont_block_the_box: bool,
61 break_turn_conflict_cycles: bool,
62 handle_uber_turns: bool,
63 ) -> IntersectionSimState {
64 let mut sim = IntersectionSimState {
65 state: BTreeMap::new(),
66 use_freeform_policy_everywhere,
67 dont_block_the_box,
68 break_turn_conflict_cycles,
69 handle_uber_turns,
70 blocked_by: BTreeSet::new(),
71 events: Vec::new(),
72 };
73 for i in map.all_intersections() {
74 let mut state = State {
75 id: i.id,
76 accepted: BTreeSet::new(),
77 waiting: BTreeMap::new(),
78 reserved: BTreeSet::new(),
79 current_stage: 0,
80 stage_ends_at: Time::START_OF_DAY,
81 };
82 if i.is_traffic_signal() && !use_freeform_policy_everywhere {
83 let signal = map.get_traffic_signal(i.id);
84 // What stage are we starting with?
85 let mut offset = signal.offset;
86 loop {
87 let dt = signal.stages[state.current_stage]
88 .phase_type
89 .simple_duration();
90 if offset >= dt {
91 offset -= dt;
92 state.current_stage += 1;
93 if state.current_stage == signal.stages.len() {
94 state.current_stage = 0;
95 }
96 } else {
97 state.stage_ends_at = Time::START_OF_DAY + dt - offset;
98 break;
99 }
100 }
101 scheduler.push(state.stage_ends_at, Command::UpdateIntersection(i.id));
102 }
103 sim.state.insert(i.id, state);
104 }
105 sim
106 }
107
nobody_headed_towards(&self, lane: LaneID, i: IntersectionID) -> bool108 pub fn nobody_headed_towards(&self, lane: LaneID, i: IntersectionID) -> bool {
109 let state = &self.state[&i];
110 !state
111 .accepted
112 .iter()
113 .chain(state.reserved.iter())
114 .any(|req| req.turn.dst == lane)
115 }
116
turn_finished( &mut self, now: Time, agent: AgentID, turn: TurnID, scheduler: &mut Scheduler, map: &Map, )117 pub fn turn_finished(
118 &mut self,
119 now: Time,
120 agent: AgentID,
121 turn: TurnID,
122 scheduler: &mut Scheduler,
123 map: &Map,
124 ) {
125 let state = self.state.get_mut(&turn.parent).unwrap();
126 assert!(state.accepted.remove(&Request { agent, turn }));
127 state.reserved.remove(&Request { agent, turn });
128 if map.get_t(turn).turn_type != TurnType::SharedSidewalkCorner {
129 self.wakeup_waiting(now, turn.parent, scheduler, map);
130 }
131 if self.break_turn_conflict_cycles {
132 if let AgentID::Car(car) = agent {
133 retain_btreeset(&mut self.blocked_by, |(_, c)| *c != car);
134 }
135 }
136 }
137
138 // For deleting cars
cancel_request(&mut self, agent: AgentID, turn: TurnID)139 pub fn cancel_request(&mut self, agent: AgentID, turn: TurnID) {
140 let state = self.state.get_mut(&turn.parent).unwrap();
141 state.waiting.remove(&Request { agent, turn });
142 if self.break_turn_conflict_cycles {
143 if let AgentID::Car(car) = agent {
144 retain_btreeset(&mut self.blocked_by, |(c1, c2)| *c1 != car && *c2 != car);
145 }
146 }
147 }
148
space_freed( &mut self, now: Time, i: IntersectionID, scheduler: &mut Scheduler, map: &Map, )149 pub fn space_freed(
150 &mut self,
151 now: Time,
152 i: IntersectionID,
153 scheduler: &mut Scheduler,
154 map: &Map,
155 ) {
156 self.wakeup_waiting(now, i, scheduler, map);
157 }
158
159 // Vanished at border, stopped biking, etc -- a vehicle disappeared, and didn't have one last
160 // turn.
vehicle_gone(&mut self, car: CarID)161 pub fn vehicle_gone(&mut self, car: CarID) {
162 retain_btreeset(&mut self.blocked_by, |(c1, c2)| *c1 != car && *c2 != car);
163 }
164
wakeup_waiting(&self, now: Time, i: IntersectionID, scheduler: &mut Scheduler, map: &Map)165 fn wakeup_waiting(&self, now: Time, i: IntersectionID, scheduler: &mut Scheduler, map: &Map) {
166 /*if i == IntersectionID(64) {
167 println!("at {}: wakeup_waiting -----------------", now);
168 }*/
169 let mut all: Vec<(Request, Time)> = self.state[&i]
170 .waiting
171 .iter()
172 .map(|(r, t)| (r.clone(), *t))
173 .collect();
174 // Sort by waiting time, so things like stop signs actually are first-come, first-served.
175 all.sort_by_key(|(_, t)| *t);
176
177 // Wake up Priority turns before Yield turns. Don't wake up Banned turns at all. This makes
178 // sure priority vehicles should get the head-start, without blocking yield vehicles
179 // unnecessarily.
180 let mut protected = Vec::new();
181 let mut yielding = Vec::new();
182
183 if self.use_freeform_policy_everywhere {
184 for (req, _) in all {
185 protected.push(req);
186 }
187 } else if let Some(ref signal) = map.maybe_get_traffic_signal(i) {
188 let stage = &signal.stages[self.state[&i].current_stage];
189 for (req, _) in all {
190 match stage.get_priority_of_turn(req.turn, signal) {
191 TurnPriority::Protected => {
192 protected.push(req);
193 }
194 TurnPriority::Yield => {
195 yielding.push(req);
196 }
197 // No need to wake up
198 TurnPriority::Banned => {}
199 }
200 }
201 } else if let Some(ref sign) = map.maybe_get_stop_sign(i) {
202 for (req, _) in all {
203 // Banned is impossible
204 if sign.get_priority(req.turn, map) == TurnPriority::Protected {
205 protected.push(req);
206 } else {
207 yielding.push(req);
208 }
209 }
210 } else {
211 assert!(map.get_i(i).is_border());
212 };
213
214 for req in protected {
215 // Use update because multiple agents could finish a turn at the same time, before the
216 // waiting one has a chance to try again.
217 scheduler.update(now, Command::update_agent(req.agent));
218 }
219 // Make sure the protected movement gets first dibs. The scheduler arbitrarily (but
220 // deterministically) orders commands with the same time.
221 for req in yielding {
222 scheduler.update(
223 now + Duration::seconds(0.1),
224 Command::update_agent(req.agent),
225 );
226 }
227 }
228
229 // This is only triggered for traffic signals.
update_intersection( &mut self, now: Time, id: IntersectionID, map: &Map, scheduler: &mut Scheduler, )230 pub fn update_intersection(
231 &mut self,
232 now: Time,
233 id: IntersectionID,
234 map: &Map,
235 scheduler: &mut Scheduler,
236 ) {
237 let state = self.state.get_mut(&id).unwrap();
238 let signal = map.get_traffic_signal(id);
239
240 // Switch to a new stage?
241 assert_eq!(now, state.stage_ends_at);
242 let old_stage = &signal.stages[state.current_stage];
243 match old_stage.phase_type {
244 PhaseType::Fixed(_) => {
245 state.current_stage += 1;
246 }
247 PhaseType::Adaptive(_) => {
248 // TODO Make a better policy here. For now, if there's _anyone_ waiting to start a
249 // protected turn, repeat this stage for the full duration. Note that "waiting" is
250 // only defined as "at the end of the lane, ready to start the turn." If a
251 // vehicle/ped is a second away from the intersection, this won't detect that. We
252 // could pass in all of the Queues here and use that to count all incoming agents,
253 // even ones a little farther away.
254 if state.waiting.keys().all(|req| {
255 old_stage.get_priority_of_turn(req.turn, signal) != TurnPriority::Protected
256 }) {
257 state.current_stage += 1;
258 self.events.push(Event::Alert(
259 AlertLocation::Intersection(id),
260 "Repeating an adaptive stage".to_string(),
261 ));
262 }
263 }
264 }
265 if state.current_stage == signal.stages.len() {
266 state.current_stage = 0;
267 }
268
269 state.stage_ends_at = now
270 + signal.stages[state.current_stage]
271 .phase_type
272 .simple_duration();
273 scheduler.push(state.stage_ends_at, Command::UpdateIntersection(id));
274 self.wakeup_waiting(now, id, scheduler, map);
275 }
276
277 // For cars: The head car calls this when they're at the end of the lane WaitingToAdvance. If
278 // this returns true, then the head car MUST actually start this turn.
279 // For peds: Likewise -- only called when the ped is at the start of the turn. They must
280 // actually do the turn if this returns true.
281 //
282 // If this returns false, the agent should NOT retry. IntersectionSimState will schedule a
283 // retry event at some point.
maybe_start_turn( &mut self, agent: AgentID, turn: TurnID, speed: Speed, now: Time, map: &Map, scheduler: &mut Scheduler, maybe_cars_and_queues: Option<( &Car, &BTreeMap<CarID, Car>, &mut BTreeMap<Traversable, Queue>, )>, ) -> bool284 pub fn maybe_start_turn(
285 &mut self,
286 agent: AgentID,
287 turn: TurnID,
288 speed: Speed,
289 now: Time,
290 map: &Map,
291 scheduler: &mut Scheduler,
292 maybe_cars_and_queues: Option<(
293 &Car,
294 &BTreeMap<CarID, Car>,
295 &mut BTreeMap<Traversable, Queue>,
296 )>,
297 ) -> bool {
298 let req = Request { agent, turn };
299 self.state
300 .get_mut(&turn.parent)
301 .unwrap()
302 .waiting
303 .entry(req.clone())
304 .or_insert(now);
305
306 let shared_sidewalk_corner =
307 map.get_t(req.turn).turn_type == TurnType::SharedSidewalkCorner;
308
309 let readonly_pair = maybe_cars_and_queues.as_ref().map(|(_, c, q)| (*c, &**q));
310 let allowed = if shared_sidewalk_corner {
311 // SharedSidewalkCorner doesn't conflict with anything -- fastpath!
312 true
313 } else if !self.handle_accepted_conflicts(&req, map, readonly_pair) {
314 // It's never OK to perform a conflicting turn
315 false
316 } else if maybe_cars_and_queues
317 .as_ref()
318 .map(|(car, _, _)| {
319 self.handle_uber_turns && car.router.get_path().currently_inside_ut().is_some()
320 })
321 .unwrap_or(false)
322 {
323 // If we started an uber-turn, then finish it! But alert if we're running a red light.
324 if let Some(ref signal) = map.maybe_get_traffic_signal(turn.parent) {
325 // Don't pass in the scheduler, aka, don't pause before yielding.
326 if !self.traffic_signal_policy(&req, map, signal, speed, now, None) && false {
327 self.events.push(Event::Alert(
328 AlertLocation::Intersection(req.turn.parent),
329 format!("Running a red light inside an uber-turn: {:?}", req),
330 ));
331 }
332 }
333
334 true
335 } else if self.use_freeform_policy_everywhere {
336 // If we made it this far, we don't conflict with an accepted turn
337 true
338 } else if let Some(ref signal) = map.maybe_get_traffic_signal(turn.parent) {
339 self.traffic_signal_policy(&req, map, signal, speed, now, Some(scheduler))
340 } else if let Some(ref sign) = map.maybe_get_stop_sign(turn.parent) {
341 self.stop_sign_policy(&req, map, sign, now, scheduler)
342 } else {
343 unreachable!()
344 };
345 if !allowed {
346 return false;
347 }
348
349 // Lock the entire uber-turn.
350 if self.handle_uber_turns {
351 if let Some(ut) = maybe_cars_and_queues
352 .as_ref()
353 .and_then(|(car, _, _)| car.router.get_path().about_to_start_ut())
354 {
355 // If there's a problem up ahead, don't start.
356 for t in &ut.path {
357 let req = Request { agent, turn: *t };
358 if !self.handle_accepted_conflicts(&req, map, readonly_pair) {
359 return false;
360 }
361 }
362 // If the way is clear, make sure it stays that way.
363 for t in &ut.path {
364 self.state
365 .get_mut(&t.parent)
366 .unwrap()
367 .reserved
368 .insert(Request { agent, turn: *t });
369 }
370 }
371 }
372
373 // Don't block the box.
374 if let Some((car, _, queues)) = maybe_cars_and_queues {
375 assert_eq!(agent, AgentID::Car(car.vehicle.id));
376 let inside_ut = self.handle_uber_turns
377 && (car.router.get_path().currently_inside_ut().is_some()
378 || car.router.get_path().about_to_start_ut().is_some());
379 let queue = queues.get_mut(&Traversable::Lane(turn.dst)).unwrap();
380 if !queue.try_to_reserve_entry(
381 car,
382 !self.dont_block_the_box
383 || allow_block_the_box(map.get_i(turn.parent).orig_id.0)
384 || inside_ut,
385 ) {
386 if self.break_turn_conflict_cycles {
387 // TODO Should we run the detector here?
388 if let Some(c) = queue.laggy_head {
389 self.blocked_by.insert((car.vehicle.id, c));
390 } else if let Some(c) = queue.cars.get(0) {
391 self.blocked_by.insert((car.vehicle.id, *c));
392 } else {
393 // Nobody's in the target lane, but there's somebody already in the
394 // intersection headed there, taking up all of the space.
395 // I guess we shouldn't count reservations for uber-turns here, because
396 // we're not going to do block-the-box resolution in the interior at
397 // all?
398 self.blocked_by.insert((
399 car.vehicle.id,
400 self.state[&turn.parent]
401 .accepted
402 .iter()
403 .find(|r| r.turn.dst == turn.dst)
404 .unwrap()
405 .agent
406 .as_car(),
407 ));
408 }
409 }
410
411 return false;
412 }
413 }
414
415 // TODO For now, we're only interested in signals, and there's too much raw data to store
416 // for stop signs too.
417 let state = self.state.get_mut(&turn.parent).unwrap();
418 let delay = now - state.waiting.remove(&req).unwrap();
419 // SharedSidewalkCorner are always no-conflict, immediate turns; they're not interesting.
420 if !shared_sidewalk_corner {
421 if let Some(ts) = map.maybe_get_traffic_signal(state.id) {
422 self.events.push(Event::IntersectionDelayMeasured(
423 ts.compressed_id(turn),
424 delay,
425 agent,
426 ));
427 }
428 }
429 state.accepted.insert(req);
430 if self.break_turn_conflict_cycles {
431 if let AgentID::Car(car) = agent {
432 retain_btreeset(&mut self.blocked_by, |(c, _)| *c != car);
433 }
434 }
435
436 true
437 }
438
debug(&self, id: IntersectionID, map: &Map)439 pub fn debug(&self, id: IntersectionID, map: &Map) {
440 println!("{}", abstutil::to_json(&self.state[&id]));
441 if let Some(ref sign) = map.maybe_get_stop_sign(id) {
442 println!("{}", abstutil::to_json(sign));
443 } else if let Some(ref signal) = map.maybe_get_traffic_signal(id) {
444 println!("{}", abstutil::to_json(signal));
445 } else {
446 println!("Border");
447 }
448 }
449
get_accepted_agents(&self, id: IntersectionID) -> HashSet<AgentID>450 pub fn get_accepted_agents(&self, id: IntersectionID) -> HashSet<AgentID> {
451 self.state[&id]
452 .accepted
453 .iter()
454 .map(|req| req.agent)
455 .collect()
456 }
457
get_blocked_by(&self, a: AgentID) -> HashSet<AgentID>458 pub fn get_blocked_by(&self, a: AgentID) -> HashSet<AgentID> {
459 let mut blocked_by = HashSet::new();
460 if let AgentID::Car(c) = a {
461 for (c1, c2) in &self.blocked_by {
462 if *c1 == c {
463 blocked_by.insert(AgentID::Car(*c2));
464 }
465 }
466 }
467 blocked_by
468 }
469
collect_events(&mut self) -> Vec<Event>470 pub fn collect_events(&mut self) -> Vec<Event> {
471 std::mem::replace(&mut self.events, Vec::new())
472 }
473
474 /// returns intersections with travelers waiting for at least `threshold` since `now`, ordered
475 /// so the longest delayed intersection is first.
delayed_intersections( &self, now: Time, threshold: Duration, ) -> Vec<(IntersectionID, Time)>476 pub fn delayed_intersections(
477 &self,
478 now: Time,
479 threshold: Duration,
480 ) -> Vec<(IntersectionID, Time)> {
481 let mut candidates = Vec::new();
482 for state in self.state.values() {
483 if let Some(earliest) = state.waiting.values().min() {
484 if now - *earliest >= threshold {
485 candidates.push((state.id, *earliest));
486 }
487 }
488 }
489 candidates.sort_by_key(|(_, t)| *t);
490 candidates
491 }
492
493 // Weird way to measure this, but it works.
worst_delay( &self, now: Time, map: &Map, ) -> ( BTreeMap<RoadID, Duration>, BTreeMap<IntersectionID, Duration>, )494 pub fn worst_delay(
495 &self,
496 now: Time,
497 map: &Map,
498 ) -> (
499 BTreeMap<RoadID, Duration>,
500 BTreeMap<IntersectionID, Duration>,
501 ) {
502 let mut per_road = BTreeMap::new();
503 let mut per_intersection = BTreeMap::new();
504 for (i, state) in &self.state {
505 for (req, t) in &state.waiting {
506 {
507 let r = map.get_l(req.turn.src).parent;
508 let worst = per_road
509 .get(&r)
510 .cloned()
511 .unwrap_or(Duration::ZERO)
512 .max(now - *t);
513 per_road.insert(r, worst);
514 }
515 {
516 let worst = per_intersection
517 .get(i)
518 .cloned()
519 .unwrap_or(Duration::ZERO)
520 .max(now - *t);
521 per_intersection.insert(*i, worst);
522 }
523 }
524 }
525 (per_road, per_intersection)
526 }
527
current_stage_and_remaining_time( &self, now: Time, i: IntersectionID, ) -> (usize, Duration)528 pub fn current_stage_and_remaining_time(
529 &self,
530 now: Time,
531 i: IntersectionID,
532 ) -> (usize, Duration) {
533 let state = &self.state[&i];
534 if now > state.stage_ends_at {
535 panic!(
536 "At {}, but {} should have advanced its stage at {}",
537 now, i, state.stage_ends_at
538 );
539 }
540 (state.current_stage, state.stage_ends_at - now)
541 }
542
handle_live_edited_traffic_signals(&mut self, map: &Map)543 pub fn handle_live_edited_traffic_signals(&mut self, map: &Map) {
544 for state in self.state.values_mut() {
545 if let Some(ts) = map.maybe_get_traffic_signal(state.id) {
546 if state.current_stage >= ts.stages.len() {
547 // Just jump back to the first one. Shrug.
548 state.current_stage = 0;
549 println!(
550 "WARNING: Traffic signal {} was live-edited in the middle of a stage, so \
551 jumping back to the first stage",
552 state.id
553 );
554 }
555 }
556 }
557 }
558 }
559
560 impl IntersectionSimState {
stop_sign_policy( &mut self, req: &Request, map: &Map, sign: &ControlStopSign, now: Time, scheduler: &mut Scheduler, ) -> bool561 fn stop_sign_policy(
562 &mut self,
563 req: &Request,
564 map: &Map,
565 sign: &ControlStopSign,
566 now: Time,
567 scheduler: &mut Scheduler,
568 ) -> bool {
569 let our_priority = sign.get_priority(req.turn, map);
570 assert!(our_priority != TurnPriority::Banned);
571 let our_time = self.state[&req.turn.parent].waiting[req];
572
573 if our_priority == TurnPriority::Yield && now < our_time + WAIT_AT_STOP_SIGN {
574 // Since we have "ownership" of scheduling for req.agent, don't need to use
575 // scheduler.update.
576 scheduler.push(
577 our_time + WAIT_AT_STOP_SIGN,
578 Command::update_agent(req.agent),
579 );
580 return false;
581 }
582
583 // Once upon a time, we'd make sure that this request doesn't conflict with another in
584 // self.waiting:
585 // 1) Higher-ranking turns get to go first.
586 // 2) Equal-ranking turns that started waiting before us get to go first.
587 // But the exceptions started stacking -- if the other agent is blocked or the turns don't
588 // even conflict, then allow it. Except determining if the other agent is blocked or not is
589 // tough and kind of recursive.
590 //
591 // So instead, don't do any of that! The WAIT_AT_STOP_SIGN scheduling above and the fact
592 // that events are processed in time order mean that case #2 is magically handled anyway.
593 // If a case #1 could've started by now, then they would have. Since they didn't, they must
594 // be blocked.
595
596 // TODO Make sure we can optimistically finish this turn before an approaching
597 // higher-priority vehicle wants to begin.
598
599 true
600 }
601
traffic_signal_policy( &mut self, req: &Request, map: &Map, signal: &ControlTrafficSignal, speed: Speed, now: Time, scheduler: Option<&mut Scheduler>, ) -> bool602 fn traffic_signal_policy(
603 &mut self,
604 req: &Request,
605 map: &Map,
606 signal: &ControlTrafficSignal,
607 speed: Speed,
608 now: Time,
609 scheduler: Option<&mut Scheduler>,
610 ) -> bool {
611 let turn = map.get_t(req.turn);
612
613 let state = &self.state[&req.turn.parent];
614 let stage = &signal.stages[state.current_stage];
615 let full_stage_duration = stage.phase_type.simple_duration();
616 let remaining_stage_time = state.stage_ends_at - now;
617 let our_time = state.waiting[req];
618
619 // Can't go at all this stage.
620 let our_priority = stage.get_priority_of_turn(req.turn, signal);
621 if our_priority == TurnPriority::Banned {
622 return false;
623 }
624
625 if our_priority == TurnPriority::Yield
626 && now < our_time + WAIT_BEFORE_YIELD_AT_TRAFFIC_SIGNAL
627 {
628 // Since we have "ownership" of scheduling for req.agent, don't need to use
629 // scheduler.update.
630 if let Some(s) = scheduler {
631 s.push(
632 our_time + WAIT_BEFORE_YIELD_AT_TRAFFIC_SIGNAL,
633 Command::update_agent(req.agent),
634 );
635 }
636 return false;
637 }
638
639 // Previously: A yield loses to a conflicting Priority turn.
640 // But similar to the description in stop_sign_policy, this caused unnecessary gridlock.
641 // Priority vehicles getting scheduled first just requires a little tweak in
642 // update_intersection.
643
644 // TODO Make sure we can optimistically finish this turn before an approaching
645 // higher-priority vehicle wants to begin.
646
647 // Optimistically if nobody else is in the way, this is how long it'll take to finish the
648 // turn. Don't start the turn if we won't finish by the time the light changes. If we get
649 // it wrong, that's fine -- block the box a bit.
650 let time_to_cross = turn.geom.length() / speed;
651 if time_to_cross > remaining_stage_time {
652 // Actually, we might have bigger problems...
653 if time_to_cross > full_stage_duration {
654 self.events.push(Event::Alert(
655 AlertLocation::Intersection(req.turn.parent),
656 format!(
657 "{:?} is impossible to fit into stage duration of {}",
658 req, full_stage_duration
659 ),
660 ));
661 } else {
662 return false;
663 }
664 }
665
666 true
667 }
668
669 // If true, the request can go.
handle_accepted_conflicts( &mut self, req: &Request, map: &Map, maybe_cars_and_queues: Option<(&BTreeMap<CarID, Car>, &BTreeMap<Traversable, Queue>)>, ) -> bool670 fn handle_accepted_conflicts(
671 &mut self,
672 req: &Request,
673 map: &Map,
674 maybe_cars_and_queues: Option<(&BTreeMap<CarID, Car>, &BTreeMap<Traversable, Queue>)>,
675 ) -> bool {
676 let turn = map.get_t(req.turn);
677 let mut cycle_detected = false;
678 let mut ok = true;
679 for other in &self.state[&req.turn.parent].accepted {
680 // Never short-circuit; always record all of the dependencies; it might help someone
681 // else unstick things.
682 if map.get_t(other.turn).conflicts_with(turn) {
683 if self.break_turn_conflict_cycles {
684 if let AgentID::Car(c) = req.agent {
685 if let AgentID::Car(c2) = other.agent {
686 self.blocked_by.insert((c, c2));
687 }
688 if !cycle_detected {
689 if let Some(cycle) =
690 self.detect_conflict_cycle(c, maybe_cars_and_queues.unwrap())
691 {
692 // Allow the conflicting turn!
693 self.events.push(Event::Alert(
694 AlertLocation::Intersection(req.turn.parent),
695 format!("Turn conflict cycle involving {:?}", cycle),
696 ));
697 cycle_detected = true;
698 }
699 }
700 }
701 }
702
703 if !cycle_detected {
704 ok = false;
705 }
706
707 // It's never safe for two vehicles to go for the same lane.
708 if turn.id.dst == other.turn.dst {
709 return false;
710 }
711 }
712 }
713 if !ok {
714 return false;
715 }
716 for other in &self.state[&req.turn.parent].reserved {
717 if map.get_t(other.turn).conflicts_with(turn) {
718 return false;
719 }
720 }
721 true
722 }
723
detect_conflict_cycle( &self, car: CarID, pair: (&BTreeMap<CarID, Car>, &BTreeMap<Traversable, Queue>), ) -> Option<HashSet<CarID>>724 fn detect_conflict_cycle(
725 &self,
726 car: CarID,
727 pair: (&BTreeMap<CarID, Car>, &BTreeMap<Traversable, Queue>),
728 ) -> Option<HashSet<CarID>> {
729 let (cars, queues) = pair;
730
731 let mut queue = vec![car];
732 let mut seen = HashSet::new();
733 while !queue.is_empty() {
734 let current = queue.pop().unwrap();
735 // Might not actually be a cycle. Insist on seeing the original req.agent
736 // again.
737 if !seen.is_empty() && current == car {
738 return Some(seen);
739 }
740 if !seen.contains(¤t) {
741 seen.insert(current);
742
743 for (c1, c2) in &self.blocked_by {
744 if *c1 == current {
745 queue.push(*c2);
746 }
747 }
748
749 // If this car isn't the head of its queue, add that dependency. (Except for
750 // the original car, which we already know is the head of its queue)
751 // TODO Maybe store this in blocked_by?
752 if current != car {
753 let q = &queues[&cars[¤t].router.head()];
754 let head = if let Some(c) = q.laggy_head {
755 c
756 } else {
757 *q.cars.get(0).unwrap()
758 };
759 if current != head {
760 queue.push(head);
761 }
762 }
763 }
764 }
765 None
766 }
767 }
768
769 // TODO Sometimes a traffic signal is surrounded by tiny lanes with almost no capacity. Workaround
770 // for now.
allow_block_the_box(osm_node_id: i64) -> bool771 fn allow_block_the_box(osm_node_id: i64) -> bool {
772 // 23rd and Madison, Madison and John, Boren and 12th, Boren and Yesler
773 osm_node_id == 53211694
774 || osm_node_id == 53211693
775 || osm_node_id == 53214134
776 || osm_node_id == 53214133
777 || osm_node_id == 53165712
778 || osm_node_id == 281487826
779 || osm_node_id == 53209840
780 || osm_node_id == 4249361353
781 }
782