diff --git a/abstutil/src/logger.rs b/abstutil/src/logger.rs index 027b4a2d69..6604a54617 100644 --- a/abstutil/src/logger.rs +++ b/abstutil/src/logger.rs @@ -16,7 +16,7 @@ impl Logger { pub fn setup() { #[cfg(target_arch = "wasm32")] { - console_log::init_with_level(log::Level::Debug).unwrap(); + console_log::init_with_level(log::Level::Info).unwrap(); } #[cfg(not(target_arch = "wasm32"))] @@ -25,7 +25,7 @@ impl Logger { last_fast_paths_note: RwLock::new(None), })) .unwrap(); - log::set_max_level(log::LevelFilter::Debug); + log::set_max_level(log::LevelFilter::Info); } } } diff --git a/map_model/src/pathfind/mod.rs b/map_model/src/pathfind/mod.rs index 95dca74de1..2c5419d18c 100644 --- a/map_model/src/pathfind/mod.rs +++ b/map_model/src/pathfind/mod.rs @@ -237,22 +237,11 @@ impl Path { self.steps.push_back(step); } - // TODO This is a brittle, tied to exactly what opportunistically_lanechange does. - pub fn approaching_uber_turn(&self) -> bool { - if self.steps.len() < 5 || self.uber_turns.is_empty() { - return false; - } - if let PathStep::Turn(t) = self.steps[1] { - if self.uber_turns[0].path[0] == t { - return true; - } - } - if let PathStep::Turn(t) = self.steps[3] { - if self.uber_turns[0].path[0] == t { - return true; - } - } - false + pub fn is_upcoming_uber_turn_component(&self, t: TurnID) -> bool { + self.uber_turns + .front() + .map(|ut| ut.path.contains(&t)) + .unwrap_or(false) } /// Trusting the caller to do this in valid ways. @@ -260,6 +249,20 @@ impl Path { assert!(self.currently_inside_ut.is_none()); assert!(idx != 0); self.total_length -= self.steps[idx].as_traversable().length(map); + + // When replacing a turn, also update any references to it in uber_turns + if let PathStep::Turn(old_turn) = self.steps[idx] { + for uts in &mut self.uber_turns { + if let Some(turn_idx) = uts.path.iter().position(|i| i == &old_turn) { + if let PathStep::Turn(new_turn) = step { + uts.path[turn_idx] = new_turn; + } else { + panic!("expected turn, but found {:?}", step); + } + } + } + } + self.steps[idx] = step; self.total_length += self.steps[idx].as_traversable().length(map); diff --git a/sim/src/mechanics/intersection.rs b/sim/src/mechanics/intersection.rs index 6e5307f4c7..85a37fb608 100644 --- a/sim/src/mechanics/intersection.rs +++ b/sim/src/mechanics/intersection.rs @@ -52,6 +52,7 @@ pub(crate) struct IntersectionSimState { #[derive(Clone, Serialize, Deserialize)] struct State { id: IntersectionID, + // The in-progress turns which any potential new turns must not conflict with accepted: BTreeSet, // Track when a request is first made. #[serde( diff --git a/sim/src/mechanics/queue.rs b/sim/src/mechanics/queue.rs index 2079910e4d..41292821f4 100644 --- a/sim/src/mechanics/queue.rs +++ b/sim/src/mechanics/queue.rs @@ -258,7 +258,12 @@ impl Queue { pub fn free_reserved_space(&mut self, car: &Car) { self.reserved_length -= car.vehicle.length + FOLLOWING_DISTANCE; - assert!(self.reserved_length >= Distance::ZERO); + assert!( + self.reserved_length >= Distance::ZERO, + "invalid reserved length: {:?}, car: {:?}", + self.reserved_length, + car + ); } pub fn target_lane_penalty(&self) -> (usize, usize) { diff --git a/sim/src/router.rs b/sim/src/router.rs index 89b4715a29..39a80ad214 100644 --- a/sim/src/router.rs +++ b/sim/src/router.rs @@ -8,7 +8,7 @@ use serde::{Deserialize, Serialize}; use geom::Distance; use map_model::{ BuildingID, IntersectionID, LaneID, Map, Path, PathConstraints, PathRequest, PathStep, - Position, Traversable, TurnID, + Position, Traversable, Turn, TurnID, }; use crate::mechanics::Queue; @@ -334,59 +334,34 @@ impl Router { map: &Map, handle_uber_turns: bool, ) { - if handle_uber_turns - && (self.path.approaching_uber_turn() || self.path.currently_inside_ut().is_some()) - { + // if we're already in the uber-turn, we're committed, but if we're about to enter one, lock + // in the best path through it now. + if handle_uber_turns && self.path.currently_inside_ut().is_some() { return; } - let (current_turn, next_lane) = { - let steps = self.path.get_steps(); - if steps.len() < 5 { - return; - } - match (steps[1], steps[4]) { - (PathStep::Turn(t), PathStep::Lane(l)) => (t, l), - _ => { + + let mut segment = 0; + loop { + let (current_turn, next_lane) = { + let steps = self.path.get_steps(); + if steps.len() < 5 + segment * 2 { return; } - } - }; - - let orig_target_lane = current_turn.dst; - let parent = map.get_parent(orig_target_lane); - let next_parent = map.get_l(next_lane).src_i; - - // Look for other candidates, and assign a cost to each. - let constraints = self.owner.1.to_constraints(); - let dir = parent.dir(orig_target_lane); - let (_, turn1, best_lane, turn2) = parent - .lanes_ltr() - .into_iter() - .filter(|(l, d, _)| dir == *d && constraints.can_use(map.get_l(*l), map)) - .filter_map(|(l, _, _)| { - let t1 = TurnID { - parent: current_turn.parent, - src: current_turn.src, - dst: l, - }; - if let Some(turn1) = map.maybe_get_t(t1) { - // Make sure we can go from this lane to next_lane. - if let Some(turn2) = map.maybe_get_t(TurnID { - parent: next_parent, - src: l, - dst: next_lane, - }) { - Some((turn1, l, turn2)) - } else { - None + match (steps[1 + segment * 2], steps[4 + segment * 2]) { + (PathStep::Turn(t), PathStep::Lane(l)) => (t, l), + _ => { + return; } - } else { - None } - }) - .map(|(turn1, l, turn2)| { + }; + + let orig_target_lane = current_turn.dst; + let parent = map.get_parent(orig_target_lane); + let next_parent = map.get_l(next_lane).src_i; + + let compute_cost = |turn1: &Turn, lane: LaneID| { let (lt, lc, mut slow_lane) = turn1.penalty(map); - let (vehicles, mut bike) = queues[&Traversable::Lane(l)].target_lane_penalty(); + let (vehicles, mut bike) = queues[&Traversable::Lane(lane)].target_lane_penalty(); // The magic happens here. We have different penalties: // @@ -399,29 +374,89 @@ impl Router { // 4) Are there lots of vehicles stacked up in one lane? // 5) Are we changing lanes? // - // A linear combination of these penalties is hard to reason about. Instead, we + // A linear combination of these penalties is hard to reason about. We mostly // make our choice based on each penalty in order, breaking ties by moving onto the - // next thing. + // next thing. With one exception: To produce more realistic behavior, we combine + // `vehicles + lc` as one score to avoid switching lanes just to get around one car. if self.owner.1 == VehicleType::Bike { bike = 0; } else { slow_lane = 0; } - let cost = (lt, bike, slow_lane, vehicles, lc); - - (cost, turn1, l, turn2) - }) - .min_by_key(|(cost, _, _, _)| *cost) - .unwrap(); - // TODO Only switch if the target queue is some amount better; don't oscillate - // unnecessarily. - if best_lane == orig_target_lane { - return; - } - self.path.modify_step(1, PathStep::Turn(turn1.id), map); - self.path.modify_step(2, PathStep::Lane(best_lane), map); - self.path.modify_step(3, PathStep::Turn(turn2.id), map); + (lt, bike, slow_lane, vehicles + lc) + }; + + // Look for other candidates, and assign a cost to each. + let mut original_cost = None; + let constraints = self.owner.1.to_constraints(); + let dir = parent.dir(orig_target_lane); + let best = parent + .lanes_ltr() + .into_iter() + .filter(|(l, d, _)| dir == *d && constraints.can_use(map.get_l(*l), map)) + .filter_map(|(l, _, _)| { + // Make sure we can go from this lane to next_lane. + + let t1 = TurnID { + parent: current_turn.parent, + src: current_turn.src, + dst: l, + }; + let turn1 = map.maybe_get_t(t1)?; + + let t2 = TurnID { + parent: next_parent, + src: l, + dst: next_lane, + }; + let turn2 = map.maybe_get_t(t2)?; + + return Some((turn1, l, turn2)); + }) + .map(|(turn1, l, turn2)| { + let cost = compute_cost(turn1, l); + if turn1.id == current_turn { + original_cost = Some(cost); + } + (cost, turn1, l, turn2) + }) + .min_by_key(|(cost, _, _, _)| *cost); + + if best.is_none() { + error!("no valid paths found: {:?}", self.owner); + return; + } + let (best_cost, turn1, best_lane, turn2) = best.unwrap(); + + if original_cost.is_none() { + error!("original_cost was unexpectedly None {:?}", self.owner); + return; + } + let original_cost = original_cost.unwrap(); + + // Only switch if the target queue is some amount better; don't oscillate + // unnecessarily. + if best_cost < original_cost { + debug!( + "changing lanes {:?} -> {:?}, cost: {:?} -> {:?}", + orig_target_lane, best_lane, original_cost, best_cost + ); + self.path + .modify_step(1 + segment * 2, PathStep::Turn(turn1.id), map); + self.path + .modify_step(2 + segment * 2, PathStep::Lane(best_lane), map); + self.path + .modify_step(3 + segment * 2, PathStep::Turn(turn2.id), map); + } + + if self.path.is_upcoming_uber_turn_component(turn2.id) { + segment += 1; + } else { + // finished + break; + } + } } pub fn is_parking(&self) -> bool {