fix an awesome parking contention bug

This commit is contained in:
Dustin Carlino 2019-09-15 14:10:54 -07:00
parent 72c391fe15
commit 0543cb015e

View File

@ -171,12 +171,9 @@ impl Router {
) { ) {
*spot = Some((new_spot, new_pos.dist_along())); *spot = Some((new_spot, new_pos.dist_along()));
} else { } else {
if let Some((new_path_steps, new_spot, new_pos)) = path_to_free_parking_spot( if let Some((new_path_steps, new_spot, new_pos)) =
self.path.current_step().as_lane(), path_to_free_parking_spot(current_lane, vehicle, map, parking)
vehicle, {
map,
parking,
) {
*spot = Some((new_spot, new_pos.dist_along())); *spot = Some((new_spot, new_pos.dist_along()));
for step in new_path_steps { for step in new_path_steps {
self.path.add(step, map); self.path.add(step, map);
@ -238,22 +235,26 @@ fn path_to_free_parking_spot(
while !queue.is_empty() { while !queue.is_empty() {
let current = queue.pop_front().unwrap(); let current = queue.pop_front().unwrap();
if let Some((spot, pos)) = // If the current lane has a spot open, we wouldn't be asking. This can happen if a spot
parking.get_first_free_spot(Position::new(current, Distance::ZERO), vehicle, map) // opens up on the 'start' lane, but behind the car.
{ if current != start {
let mut steps = vec![PathStep::Lane(current)]; if let Some((spot, pos)) =
let mut current = current; parking.get_first_free_spot(Position::new(current, Distance::ZERO), vehicle, map)
loop { {
if current == start { let mut steps = vec![PathStep::Lane(current)];
// Don't include PathStep::Lane(start) let mut current = current;
steps.pop(); loop {
steps.reverse(); if current == start {
return Some((steps, spot, pos)); // Don't include PathStep::Lane(start)
steps.pop();
steps.reverse();
return Some((steps, spot, pos));
}
let turn = backrefs[&current];
steps.push(PathStep::Turn(turn));
steps.push(PathStep::Lane(turn.src));
current = turn.src;
} }
let turn = backrefs[&current];
steps.push(PathStep::Turn(turn));
steps.push(PathStep::Lane(turn.src));
current = turn.src;
} }
} }
for turn in map.get_legal_turns(current, vec![LaneType::Driving]) { for turn in map.get_legal_turns(current, vec![LaneType::Driving]) {