a-b-street/abstreet
1
1
Fork 0
mirror of https://github.com/a-b-street/abstreet.git synced 2025-01-02 19:35:25 +03:00
Code Issues Projects Releases Wiki Activity

Implement dynamic lane-changing in a simple way:

Browse Source 
- 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
This commit is contained in:
Dustin Carlino 2021-06-19 11:46:16 -07:00
parent 0bcf01fd05
commit 1e43d929e5
5 changed files with 336 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
map_model/src/pathfind/v1.rs
View File

@ -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();

47
sim/src/mechanics/car.rs
View File

@ -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<LaneID>,
},
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,
}
}

235
sim/src/mechanics/driving.rs
View File

@ -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<LaneID> {
// 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<Event> {
std::mem::take(&mut self.events)
}

35
sim/src/mechanics/queue.rs
View File

@ -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<CarID, Car>, 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");
}

33
sim/src/router.rs
View File

@ -203,6 +203,8 @@ impl Router {
trip_and_person: Option<(TripID, PersonID)>,
events: &mut Vec<Event>,
) -> Option<ActionAtEnd> {
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 {