Improve lane changing vis-a-vis uber turns.

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);
         }
     }
 }

map_model/src/pathfind/mod.rs

@@ -237,29 +237,32 @@ 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.
     pub fn modify_step(&mut self, idx: usize, step: PathStep, map: &Map) {
         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);
 

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<Request>,
     // Track when a request is first made.
     #[serde(

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) {

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 {