From 1e43d929e56446b710dc6cf6529ac52022cd408a Mon Sep 17 00:00:00 2001 From: Dustin Carlino Date: Sat, 19 Jun 2021 11:46:16 -0700 Subject: [PATCH] Implement dynamic lane-changing in a simple way: - only triggered when a vehicle becomes Queued - Only one adjacent lane, no contraflow (crossing the road's center line) - Don't return to the original lane after passing - Using a static blockage in the old lane (so other vehicles will wait too much) - Only using the new lane to determine position (so visually a car will clip a bike as they pass) Haven't regenerated prebaked data. #382 --- map_model/src/pathfind/v1.rs | 1 - sim/src/mechanics/car.rs | 47 ++++++- sim/src/mechanics/driving.rs | 235 +++++++++++++++++++++++++++++++++-- sim/src/mechanics/queue.rs | 35 +++++- sim/src/router.rs | 33 +++++ 5 files changed, 336 insertions(+), 15 deletions(-) diff --git a/map_model/src/pathfind/v1.rs b/map_model/src/pathfind/v1.rs index 1a3560e2a4..99e332601f 100644 --- a/map_model/src/pathfind/v1.rs +++ b/map_model/src/pathfind/v1.rs @@ -269,7 +269,6 @@ impl Path { /// 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); // We're assuming this step was in the middle of the path, meaning we were planning to // travel its full length self.total_length -= self.steps[idx].as_traversable().get_polyline(map).length(); diff --git a/sim/src/mechanics/car.rs b/sim/src/mechanics/car.rs index a1d06379be..efce8c01ae 100644 --- a/sim/src/mechanics/car.rs +++ b/sim/src/mechanics/car.rs @@ -3,7 +3,7 @@ use std::collections::{BTreeSet, VecDeque}; use serde::{Deserialize, Serialize}; use geom::{Distance, Duration, PolyLine, Time, EPSILON_DIST}; -use map_model::{Direction, Map, Traversable}; +use map_model::{Direction, LaneID, Map, Traversable}; use crate::{ CarID, CarStatus, DistanceInterval, DrawCarInput, ParkingSpot, PersonID, Router, TimeInterval, @@ -133,6 +133,33 @@ impl Car { }; let body = match self.state { + CarState::ChangingLanes { + from, + to, + ref lc_time, + .. + } => { + let percent_time = 1.0 - lc_time.percent(now); + // TODO Can probably simplify this! Lifted from the parking case + let r = map.get_parent(from); + // The car's body is already at 'to', so shift back + let mut diff = (r.offset(to) as isize) - (r.offset(from) as isize); + if map.get_l(from).dir == Direction::Fwd { + diff *= -1; + } + // TODO Careful with this width math + let width = map.get_l(from).width * (diff as f64) * percent_time; + match raw_body.shift_right(width) { + Ok(pl) => pl, + Err(err) => { + println!( + "Body for lane-changing {} at {} broken: {}", + self.vehicle.id, now, err + ); + raw_body + } + } + } CarState::Unparking(_, ref spot, ref time_int) | CarState::Parking(_, ref spot, ref time_int) => { let (percent_time, is_parking) = match self.state { @@ -232,6 +259,7 @@ impl Car { CarState::Queued { .. } => CarStatus::Moving, CarState::WaitingToAdvance { .. } => CarStatus::Moving, CarState::Crossing(_, _) => CarStatus::Moving, + CarState::ChangingLanes { .. } => CarStatus::Moving, CarState::Unparking(_, _, _) => CarStatus::Moving, CarState::Parking(_, _, _) => CarStatus::Moving, // Changing color for idling buses is helpful @@ -272,8 +300,18 @@ impl Car { #[derive(Debug, Serialize, Deserialize, Clone)] pub(crate) enum CarState { Crossing(TimeInterval, DistanceInterval), + ChangingLanes { + from: LaneID, + to: LaneID, + // For the most part, act just like a Crossing state with these intervals + new_time: TimeInterval, + new_dist: DistanceInterval, + // How long does the lane-changing itself last? This must end before new_time_int does. + lc_time: TimeInterval, + }, Queued { blocked_since: Time, + want_to_change_lanes: Option, }, WaitingToAdvance { blocked_since: Time, @@ -290,6 +328,8 @@ impl CarState { CarState::Crossing(ref time_int, _) => time_int.end, CarState::Queued { .. } => unreachable!(), CarState::WaitingToAdvance { .. } => unreachable!(), + // Note this state lasts for lc_time, NOT for new_time. + CarState::ChangingLanes { ref lc_time, .. } => lc_time.end, CarState::Unparking(_, _, ref time_int) => time_int.end, CarState::Parking(_, _, ref time_int) => time_int.end, CarState::IdlingAtStop(_, ref time_int) => time_int.end, @@ -298,9 +338,8 @@ impl CarState { pub fn time_spent_waiting(&self, now: Time) -> Duration { match self { - CarState::Queued { blocked_since } | CarState::WaitingToAdvance { blocked_since } => { - now - *blocked_since - } + CarState::Queued { blocked_since, .. } + | CarState::WaitingToAdvance { blocked_since } => now - *blocked_since, _ => Duration::ZERO, } } diff --git a/sim/src/mechanics/driving.rs b/sim/src/mechanics/driving.rs index f4d035372c..6a0c37e1bb 100644 --- a/sim/src/mechanics/driving.rs +++ b/sim/src/mechanics/driving.rs @@ -4,7 +4,7 @@ use serde::{Deserialize, Serialize}; use abstutil::{deserialize_hashmap, serialize_hashmap, FixedMap, IndexableKey}; use geom::{Distance, Duration, PolyLine, Time}; -use map_model::{IntersectionID, LaneID, Map, Path, Position, Traversable}; +use map_model::{DrivingSide, IntersectionID, LaneID, Map, Path, Position, Traversable}; use crate::mechanics::car::{Car, CarState}; use crate::mechanics::queue::{Queue, QueueEntry, Queued}; @@ -17,6 +17,7 @@ use crate::{ }; const TIME_TO_WAIT_AT_BUS_STOP: Duration = Duration::const_seconds(10.0); +const TIME_TO_CHANGE_LANES: Duration = Duration::const_seconds(1.0); // TODO Do something else. pub const BLIND_RETRY_TO_CREEP_FORWARDS: Duration = Duration::const_seconds(0.1); @@ -133,7 +134,10 @@ impl DrivingSimState { vehicle: params.vehicle, router: params.router, // Temporary - state: CarState::Queued { blocked_since: now }, + state: CarState::Queued { + blocked_since: now, + want_to_change_lanes: None, + }, last_steps: VecDeque::new(), started_at: now, total_blocked_time: Duration::ZERO, @@ -320,7 +324,10 @@ impl DrivingSimState { ) -> bool { match car.state { CarState::Crossing(_, _) => { - car.state = CarState::Queued { blocked_since: now }; + car.state = CarState::Queued { + blocked_since: now, + want_to_change_lanes: None, + }; if car.router.last_step() { // Immediately run update_car_with_distances. return true; @@ -351,6 +358,17 @@ impl DrivingSimState { Problem::OvertakeDesired(queue.id), )); } + + if let Some(target_lane) = self.pick_overtaking_lane(car, ctx.map) { + // We need the current position of the car to see if lane-changing is + // actually feasible right now, so record our intention and trigger + // update_car_with_distances. + car.state = CarState::Queued { + blocked_since: now, + want_to_change_lanes: Some(target_lane), + }; + return true; + } } } CarState::Unparking(front, _, _) => { @@ -478,6 +496,33 @@ impl DrivingSimState { .unwrap() .push_car_onto_end(car.vehicle.id); } + CarState::ChangingLanes { + from, + new_time, + new_dist, + .. + } => { + // The car is already in the target queue. Just set them in the crossing state; we + // already calculated the intervals for it. + car.state = CarState::Crossing(new_time, new_dist); + ctx.scheduler + .push(car.state.get_end_time(), Command::UpdateCar(car.vehicle.id)); + + // And remove the blockage from the old queue. Similar to the note in this function + // for Unparking, we calculate distances in that OTHER queue. + let dists = self.queues[&Traversable::Lane(from)].get_car_positions( + now, + &self.cars, + &self.queues, + ); + let idx = dists.iter().position(|entry| matches!(entry.member, Queued::StaticBlockage { cause, ..} if cause == car.vehicle.id)).unwrap(); + self.update_follower(idx, &dists, now, ctx); + + self.queues + .get_mut(&Traversable::Lane(from)) + .unwrap() + .clear_blockage(car.vehicle.id, idx); + } CarState::Queued { .. } => unreachable!(), CarState::Parking(_, _, _) => unreachable!(), CarState::IdlingAtStop(_, _) => unreachable!(), @@ -502,8 +547,19 @@ impl DrivingSimState { match car.state { CarState::Crossing(_, _) | CarState::Unparking(_, _, _) - | CarState::WaitingToAdvance { .. } => unreachable!(), - CarState::Queued { blocked_since } => { + | CarState::WaitingToAdvance { .. } + | CarState::ChangingLanes { .. } => unreachable!(), + CarState::Queued { + blocked_since, + want_to_change_lanes, + } => { + // Two totally different reasons we'll wind up here: we want to lane-change, and + // we're on our last step. + if let Some(target_lane) = want_to_change_lanes { + self.try_start_lc(car, our_dist, idx, target_lane, now, ctx); + return true; + } + match car.router.maybe_handle_end( our_dist, &car.vehicle, @@ -758,7 +814,7 @@ impl DrivingSimState { // car's back is still sticking out. Need to still be bound by them, even though they // don't exist! If the leader just parked, then we're fine. match follower.state { - CarState::Queued { blocked_since } => { + CarState::Queued { blocked_since, .. } => { // TODO If they're on their last step, they might be ending early and not right // behind us? !follower.router.last_step() @@ -780,6 +836,33 @@ impl DrivingSimState { Command::UpdateCar(follower_id), ); } + CarState::ChangingLanes { + from, to, lc_time, .. + } => { + // This is a fun case -- something stopped blocking somebody that was in the + // process of lane-changing! Similar to the Crossing case above, we just have + // to update the distance/time intervals, but otherwise leave them in the + // middle of their lane-changing. It's guaranteed that lc_time will continue to + // finish before the new time interval, because there's no possible way + // recalculating this crossing state here will speed things up from the + // original estimate. + let (new_time, new_dist) = match follower.crossing_state_with_end_dist( + DistanceInterval::new_driving(follower_dist, ctx.map.get_l(to).length()), + now, + ctx.map, + ) { + CarState::Crossing(time, dist) => (time, dist), + _ => unreachable!(), + }; + assert!(new_time.end >= lc_time.end); + follower.state = CarState::ChangingLanes { + from, + to, + new_time, + new_dist, + lc_time, + }; + } // They weren't blocked CarState::Unparking(_, _, _) | CarState::Parking(_, _, _) @@ -895,7 +978,7 @@ impl DrivingSimState { let mut follower = self.cars.get_mut(follower_id).unwrap(); match follower.state { - CarState::Queued { blocked_since } => { + CarState::Queued { blocked_since, .. } => { // If they're on their last step, they might be ending early and not // right behind us. if !follower.router.last_step() { @@ -919,6 +1002,7 @@ impl DrivingSimState { // They weren't blocked. Note that there's no way the Crossing state could // jump forwards here; the leader vanished from the end of the traversable. CarState::Crossing(_, _) + | CarState::ChangingLanes { .. } | CarState::Unparking(_, _, _) | CarState::Parking(_, _, _) | CarState::IdlingAtStop(_, _) => {} @@ -933,6 +1017,143 @@ impl DrivingSimState { } } + /// If the car wants to over-take somebody, what adjacent lane should they use? + /// - The lane must be in the same direction as the current; no support for crossing the road's + /// yellow line yet. + /// - Prefer passing on the left (for DrivingSide::Right) + /// For now, just pick one candidate lane, even if both might be usable. + fn pick_overtaking_lane(&self, car: &Car, map: &Map) -> Option { + // Don't overtake in the middle of a turn! + let current_lane = map.get_l(car.router.head().maybe_lane()?); + let road = map.get_r(current_lane.parent); + let idx = road.offset(current_lane.id); + let lanes_ltr = road.lanes_ltr(); + + let mut candidates = Vec::new(); + if idx != 0 { + candidates.push(lanes_ltr[idx - 1].0); + } + if idx != lanes_ltr.len() - 1 { + candidates.push(lanes_ltr[idx + 1].0); + } + if map.get_config().driving_side == DrivingSide::Left { + candidates.reverse(); + } + + for l in candidates { + let target_lane = map.get_l(l); + // Must be the same direction -- no crossing into oncoming traffic yet + if current_lane.dir != target_lane.dir { + continue; + } + // The lane types can differ, as long as the vehicle can use the target. Imagine + // overtaking a slower cyclist in a bike lane using the rest of the road. + if !car + .vehicle + .vehicle_type + .to_constraints() + .can_use(target_lane, map) + { + continue; + } + // Is this other lane compatible with the path? We won't make any attempts to return to the + // original lane after changing. + if !car + .router + .can_lanechange(current_lane.id, target_lane.id, map) + { + continue; + } + return Some(target_lane.id); + } + + None + } + + fn try_start_lc( + &mut self, + car: &mut Car, + front_current_queue: Distance, + idx_in_current_queue: usize, + target_lane: LaneID, + now: Time, + ctx: &mut Ctx, + ) { + // If the lanes are very different lengths and we're too close to the end at the target, + // not going to work. + if front_current_queue >= ctx.map.get_l(target_lane).length() { + return; + } + let current_lane = car.router.head().as_lane(); + let front_target_queue = Position::new(current_lane, front_current_queue) + .equiv_pos(target_lane, ctx.map) + .dist_along(); + + // Calculate the crossing state in the target queue. Pass in the DistanceInterval + // explicitly, because we haven't modified the route yet. + let (new_time, new_dist) = match car.crossing_state_with_end_dist( + DistanceInterval::new_driving(front_target_queue, ctx.map.get_l(target_lane).length()), + now, + ctx.map, + ) { + CarState::Crossing(time, dist) => (time, dist), + _ => unreachable!(), + }; + + // Do we have enough time to finish the lane-change, assuming that we go as fast as + // possible in the target? + let lc_time = TimeInterval::new(now, now + TIME_TO_CHANGE_LANES); + if lc_time.end >= new_time.end { + return; + } + + // Is there room for us to sliiiide on over into that lane's DMs? + if let Some(idx_in_target_queue) = self.queues[&Traversable::Lane(target_lane)] + .get_idx_to_insert_car( + front_target_queue, + car.vehicle.length, + now, + &self.cars, + &self.queues, + ) + { + // In the current queue, replace with a static blockage that just stays exactly where + // the car currently is. + { + let queue = self.queues.get_mut(&car.router.head()).unwrap(); + queue.remove_car_from_idx(car.vehicle.id, idx_in_current_queue); + queue.free_reserved_space(car); + queue.add_blockage( + car.vehicle.id, + front_current_queue, + front_current_queue - car.vehicle.length, + // The same index should work + idx_in_current_queue, + ); + } + + // Change the path + car.router.confirm_lanechange(target_lane, ctx.map); + + // Insert into the new queue + self.queues + .get_mut(&car.router.head()) + .unwrap() + .insert_car_at_idx(idx_in_target_queue, car); + + // Put into the new state + car.state = CarState::ChangingLanes { + from: current_lane, + to: target_lane, + new_time, + new_dist, + lc_time, + }; + ctx.scheduler + .push(car.state.get_end_time(), Command::UpdateCar(car.vehicle.id)); + } + } + pub fn collect_events(&mut self) -> Vec { std::mem::take(&mut self.events) } diff --git a/sim/src/mechanics/queue.rs b/sim/src/mechanics/queue.rs index e5ee5873d9..c6cf237a03 100644 --- a/sim/src/mechanics/queue.rs +++ b/sim/src/mechanics/queue.rs @@ -9,7 +9,7 @@ use map_model::{Map, Position, Traversable}; use crate::mechanics::car::{Car, CarState}; use crate::{CarID, VehicleType, FOLLOWING_DISTANCE}; -/// A Queue of vehicles on a single lane or turn. No over-taking or lane-changing. This is where +/// A Queue of vehicles on a single lane or turn. This is where /// https://a-b-street.github.io/docs/tech/trafficsim/discrete_event.html#exact-positions is /// implemented. /// @@ -224,6 +224,14 @@ impl Queue { // calculate this before moving this car from Crossing to another state. dist_int.lerp(time_int.percent_clamp_end(now)).min(bound) } + CarState::ChangingLanes { + ref new_time, + ref new_dist, + .. + } => { + // Same as the Crossing logic + new_dist.lerp(new_time.percent_clamp_end(now)).min(bound) + } CarState::Unparking(front, _, _) => front, CarState::Parking(front, _, _) => front, CarState::IdlingAtStop(front, _) => front, @@ -283,8 +291,19 @@ impl Queue { // Nope, there's not actually room at the front right now. // (This is overly conservative; we could figure out exactly where the laggy head is and // maybe allow it.) - if self.laggy_head.is_some() && idx == 0 { - return None; + if idx == 0 { + if let Some(c) = self.laggy_head { + // We don't know exactly where the laggy head is. So assume the worst case; that + // they've just barely started the turn, and we have to use the same + // too-close-to-leader math. + // + // TODO We can be more precise! We already call get_car_positions, and that + // calculates exactly where the laggy head is. We just need to plumb that bound + // back here. + if self.geom_len - cars[&c].vehicle.length - FOLLOWING_DISTANCE < start_dist { + return None; + } + } } // Are we too close to the leader? @@ -507,6 +526,16 @@ fn dump_cars(dists: &[QueueEntry], cars: &FixedMap, id: Traversable, dist_int.start, dist_int.end, time_int.start, time_int.end ); } + CarState::ChangingLanes { + ref new_time, + ref new_dist, + .. + } => { + println!( + " Going {} .. {} during {} .. {}, also in the middle of lane-changing", + new_dist.start, new_dist.end, new_time.start, new_time.end + ); + } CarState::Queued { .. } => { println!(" Queued currently"); } diff --git a/sim/src/router.rs b/sim/src/router.rs index a762a0273e..efa9c6dea2 100644 --- a/sim/src/router.rs +++ b/sim/src/router.rs @@ -203,6 +203,8 @@ impl Router { trip_and_person: Option<(TripID, PersonID)>, events: &mut Vec, ) -> Option { + assert!(self.path.is_last_step()); + match self.goal { Goal::EndAtBorder { end_dist, i } => { if end_dist == front { @@ -468,6 +470,37 @@ impl Router { } } + pub fn can_lanechange(&self, from: LaneID, to: LaneID, map: &Map) -> bool { + let steps = self.path.get_steps(); + if steps.len() < 3 { + return false; + } + assert_eq!(PathStep::Lane(from), steps[0]); + let current_turn = match steps[1] { + PathStep::Turn(t) => t, + _ => unreachable!(), + }; + let next_lane = current_turn.dst; + assert_eq!(PathStep::Lane(next_lane), steps[2]); + map.maybe_get_t(TurnID { + parent: current_turn.parent, + src: to, + dst: next_lane, + }) + .is_some() + } + + pub fn confirm_lanechange(&mut self, to: LaneID, map: &Map) { + // No assertions, blind trust! + self.path.modify_step(0, PathStep::Lane(to), map); + let mut turn = match self.path.get_steps()[1] { + PathStep::Turn(t) => t, + _ => unreachable!(), + }; + turn.src = to; + self.path.modify_step(1, PathStep::Turn(turn), map); + } + pub fn is_parking(&self) -> bool { match self.goal { Goal::ParkNearBuilding {