Convert the Dijkstra's vehicle pathfinding implementation to use road… (#594)

map_model/src/map.rs

@@ -13,10 +13,10 @@ use geom::{Bounds, Distance, GPSBounds, Polygon, Pt2D, Ring, Time};
 use crate::raw::{OriginalRoad, RawMap};
 use crate::{
     osm, Area, AreaID, AreaType, Building, BuildingID, BuildingType, BusRoute, BusRouteID, BusStop,
-    BusStopID, ControlStopSign, ControlTrafficSignal, Intersection, IntersectionID, Lane, LaneID,
-    LaneType, Map, MapEdits, MovementID, OffstreetParking, ParkingLot, ParkingLotID, Path,
-    PathConstraints, PathRequest, Pathfinder, Position, Road, RoadID, RoutingParams, Turn, TurnID,
-    TurnType, Zone,
+    BusStopID, ControlStopSign, ControlTrafficSignal, DirectedRoadID, Intersection, IntersectionID,
+    Lane, LaneID, LaneType, Map, MapEdits, MovementID, OffstreetParking, ParkingLot, ParkingLotID,
+    Path, PathConstraints, PathRequest, Pathfinder, Position, Road, RoadID, RoutingParams, Turn,
+    TurnID, TurnType, Zone,
 };
 
 #[derive(Clone, Debug, Serialize, Deserialize)]
@@ -391,6 +391,25 @@ impl Map {
         turns
     }
 
+    /// Find all movements from one road to another that're usable by someone.
+    pub fn get_movements_for(
+        &self,
+        from: DirectedRoadID,
+        constraints: PathConstraints,
+    ) -> Vec<MovementID> {
+        let mut result = BTreeSet::new();
+        for t in &self.get_i(from.dst_i(self)).turns {
+            if self.get_l(t.src).get_directed_parent(self) == from
+                && constraints.can_use(self.get_l(t.dst), self)
+            {
+                result.insert(t.to_movement(self));
+            }
+        }
+        // TODO Sidewalks are bidirectional
+        assert!(constraints != PathConstraints::Pedestrian);
+        result.into_iter().collect()
+    }
+
     pub fn get_next_roads(&self, from: RoadID) -> BTreeSet<RoadID> {
         let mut roads: BTreeSet<RoadID> = BTreeSet::new();
         let r = self.get_r(from);
@@ -476,6 +495,20 @@ impl Map {
         result
     }
 
+    /// Find all directed roads usable by somebody.
+    pub(crate) fn all_directed_roads_for(
+        &self,
+        constraints: PathConstraints,
+    ) -> Vec<DirectedRoadID> {
+        let mut result = BTreeSet::new();
+        for l in &self.lanes {
+            if constraints.can_use(l, self) {
+                result.insert(l.get_directed_parent(self));
+            }
+        }
+        result.into_iter().collect()
+    }
+
     pub fn save(&self) {
         assert!(self.edits.edits_name.starts_with("Untitled Proposal"));
         assert!(self.edits.commands.is_empty());

map_model/src/objects/road.rs

@@ -87,6 +87,16 @@ impl DirectedRoadID {
         let r = map.get_r(self.id);
         constraints.filter_lanes(r.children(self.dir).iter().map(|(l, _)| *l).collect(), map)
     }
+
+    /// Does this directed road have any lanes of a certain type?
+    pub fn has_lanes(self, lane_type: LaneType, map: &Map) -> bool {
+        for (_, lt) in map.get_r(self.id).children(self.dir) {
+            if lt == lane_type {
+                return true;
+            }
+        }
+        false
+    }
 }
 
 /// A Road represents a segment between exactly two Intersections. It contains Lanes as children.

map_model/src/objects/turn.rs

@@ -178,6 +178,13 @@ pub struct MovementID {
     pub crosswalk: bool,
 }
 
+impl MovementID {
+    // TODO Expensive! Should we natively store movements everywhere?
+    pub(crate) fn get(self, map: &Map) -> Result<Movement> {
+        Ok(Movement::for_i(self.parent, map)?.remove(&self).unwrap())
+    }
+}
+
 /// This is cheaper to store than a MovementID. It simply indexes into the list of movements.
 #[derive(Clone, Copy, Debug, Eq, Hash, PartialEq, PartialOrd, Ord, Serialize, Deserialize)]
 pub struct CompressedMovementID {
@@ -360,3 +367,14 @@ fn movement_geom(
     }
     PolyLine::deduping_new(pts)
 }
+
+impl TurnID {
+    pub fn to_movement(self, map: &Map) -> MovementID {
+        MovementID {
+            from: map.get_l(self.src).get_directed_parent(map),
+            to: map.get_l(self.dst).get_directed_parent(map),
+            parent: self.parent,
+            crosswalk: map.get_l(self.src).is_walkable(),
+        }
+    }
+}

map_model/src/pathfind/dijkstra.rs

@@ -1,18 +1,18 @@
 //! Pathfinding without needing to build a separate contraction hierarchy.
 
-// TODO Dijkstra's for vehicles currently ignores uber-turns!
-
 use std::collections::BTreeSet;
 
 use petgraph::graphmap::DiGraphMap;
 
 use geom::Duration;
 
-use crate::pathfind::vehicles::vehicle_cost;
+use crate::pathfind::v2::path_v2_to_v1;
+use crate::pathfind::vehicles::{vehicle_cost, vehicle_cost_v2};
 use crate::pathfind::walking::WalkingNode;
-use crate::pathfind::zone_cost;
+use crate::pathfind::{zone_cost, zone_cost_v2};
 use crate::{
-    LaneID, Map, Path, PathConstraints, PathRequest, PathStep, RoutingParams, Traversable, TurnID,
+    DirectedRoadID, LaneID, Map, MovementID, Path, PathConstraints, PathRequest, PathStep,
+    RoutingParams, Traversable, TurnID,
 };
 
 // TODO These should maybe keep the DiGraphMaps as state. It's cheap to recalculate it for edits.
@@ -22,8 +22,8 @@ pub fn simple_pathfind(
     params: &RoutingParams,
     map: &Map,
 ) -> Option<(Path, Duration)> {
-    let graph = build_graph_for_vehicles(map, req.constraints);
-    calc_path(graph, req, params, map)
+    let graph = build_graph_for_vehicles_v2(map, req.constraints);
+    calc_path_v2(graph, req, params, map)
 }
 
 pub fn build_graph_for_vehicles(
@@ -41,6 +41,19 @@ pub fn build_graph_for_vehicles(
     graph
 }
 
+fn build_graph_for_vehicles_v2(
+    map: &Map,
+    constraints: PathConstraints,
+) -> DiGraphMap<DirectedRoadID, MovementID> {
+    let mut graph = DiGraphMap::new();
+    for dr in map.all_directed_roads_for(constraints) {
+        for mvmnt in map.get_movements_for(dr, constraints) {
+            graph.add_edge(mvmnt.from, mvmnt.to, mvmnt);
+        }
+    }
+    graph
+}
+
 pub fn pathfind_avoiding_lanes(
     req: PathRequest,
     avoid: BTreeSet<LaneID>,
@@ -89,9 +102,37 @@ fn calc_path(
     }
     steps.push(PathStep::Lane(req.end.lane()));
     assert_eq!(steps[0], PathStep::Lane(req.start.lane()));
+    // TODO Dijkstra's for vehicles currently ignores uber-turns!
     Some((Path::new(map, steps, req.clone(), Vec::new()), cost))
 }
 
+fn calc_path_v2(
+    graph: DiGraphMap<DirectedRoadID, MovementID>,
+    req: &PathRequest,
+    params: &RoutingParams,
+    map: &Map,
+) -> Option<(Path, Duration)> {
+    let end = map.get_l(req.end.lane()).get_directed_parent(map);
+    let (cost, path) = petgraph::algo::astar(
+        &graph,
+        map.get_l(req.start.lane()).get_directed_parent(map),
+        |dr| dr == end,
+        |(_, _, mvmnt)| {
+            vehicle_cost_v2(mvmnt.from, *mvmnt, req.constraints, params, map)
+                + zone_cost_v2(*mvmnt, req.constraints, map)
+        },
+        |_| Duration::ZERO,
+    )?;
+
+    let mut steps = Vec::new();
+    for pair in path.windows(2) {
+        steps.push(pair[0]);
+    }
+    steps.push(end);
+    let path = path_v2_to_v1(req.clone(), steps, map).ok()?;
+    Some((path, cost))
+}
+
 pub fn build_graph_for_pedestrians(map: &Map) -> DiGraphMap<WalkingNode, Duration> {
     let max_speed = Some(crate::MAX_WALKING_SPEED);
     let mut graph: DiGraphMap<WalkingNode, Duration> = DiGraphMap::new();

map_model/src/pathfind/mod.rs

@@ -15,7 +15,8 @@ pub use self::pathfinder::Pathfinder;
 pub use self::vehicles::vehicle_cost;
 pub use self::walking::WalkingNode;
 use crate::{
-    osm, BuildingID, Lane, LaneID, LaneType, Map, Position, Traversable, Turn, TurnID, UberTurn,
+    osm, BuildingID, Lane, LaneID, LaneType, Map, MovementID, Position, Traversable, Turn, TurnID,
+    UberTurn,
 };
 
 mod ch;
@@ -24,6 +25,7 @@ mod node_map;
 mod pathfinder;
 // TODO tmp
 pub mod uber_turns;
+mod v2;
 mod vehicles;
 mod walking;
 
@@ -675,6 +677,29 @@ pub fn zone_cost(turn: &Turn, constraints: PathConstraints, map: &Map) -> Durati
     }
 }
 
+/// Heavily penalize crossing into an access-restricted zone that doesn't allow this mode.
+pub fn zone_cost_v2(mvmnt: MovementID, constraints: PathConstraints, map: &Map) -> Duration {
+    // Detect when we cross into a new zone that doesn't allow constraints.
+    if map
+        .get_r(mvmnt.from.id)
+        .access_restrictions
+        .allow_through_traffic
+        .contains(constraints)
+        && !map
+            .get_r(mvmnt.to.id)
+            .access_restrictions
+            .allow_through_traffic
+            .contains(constraints)
+    {
+        // This should be high enough to achieve the desired effect of somebody not entering
+        // the zone unless absolutely necessary. Someone would violate that and cut through anyway
+        // only when the alternative route would take more than 3 hours longer!
+        Duration::hours(3)
+    } else {
+        Duration::ZERO
+    }
+}
+
 /// Tuneable parameters for all types of routing.
 // These will maybe become part of the PathRequest later, but that's an extremely invasive and
 // space-expensive change right now.

map_model/src/pathfind/uber_turns.rs

@@ -269,12 +269,7 @@ impl IntersectionCluster {
         for ut in self.uber_turns {
             let mut path = Vec::new();
             for turn in ut.path {
-                path.push(MovementID {
-                    from: map.get_l(turn.src).get_directed_parent(map),
-                    to: map.get_l(turn.dst).get_directed_parent(map),
-                    parent: turn.parent,
-                    crosswalk: false,
-                });
+                path.push(turn.to_movement(map));
             }
             result.insert(UberTurnV2 { path });
         }

map_model/src/pathfind/v2.rs

@@ -0,0 +1,146 @@
+//! Structures related to the new road-based pathfinding
+//! (https://github.com/a-b-street/abstreet/issues/555) live here. When the transition is done,
+//! things here will probably move into pathfind/mod.rs.
+
+use anyhow::Result;
+
+use crate::{DirectedRoadID, Map, Path, PathRequest, PathStep, TurnID};
+
+/// Transform a sequence of roads representing a path into the current lane-based path, by picking
+/// particular lanes and turns to use.
+pub fn path_v2_to_v1(req: PathRequest, road_steps: Vec<DirectedRoadID>, map: &Map) -> Result<Path> {
+    // This is a somewhat brute-force method: run Dijkstra's algorithm on a graph of lanes and
+    // turns, but only build the graph along the path of roads we've already found. This handles
+    // arbitrary lookahead needed, and forces use of the original start/end lanes requested.
+    //
+    // Eventually we'll directly return road-based paths. Most callers will actually just use that
+    // directly, and mainly the simulation will need to expand to specific lanes, but it'll do so
+    // dynamically/lazily to account for current traffic conditions.
+    let mut graph = petgraph::graphmap::DiGraphMap::new();
+    for pair in road_steps.windows(2) {
+        for src in pair[0].lanes(req.constraints, map) {
+            for dst in pair[1].lanes(req.constraints, map) {
+                let turn = TurnID {
+                    parent: map.get_l(src).dst_i,
+                    src,
+                    dst,
+                };
+                if map.maybe_get_t(turn).is_some() {
+                    graph.add_edge(src, dst, turn);
+                }
+            }
+        }
+    }
+
+    match petgraph::algo::astar(
+        &graph,
+        req.start.lane(),
+        |l| l == req.end.lane(),
+        // TODO We could include the old lane-changing penalties here, but I'm not sure it's worth
+        // the complication. The simulation layer will end up tuning those anyway.
+        |_| 1,
+        |_| 0,
+    ) {
+        Some((_, path)) => {
+            let mut steps = Vec::new();
+            for pair in path.windows(2) {
+                steps.push(PathStep::Lane(pair[0]));
+                // We don't need to look for this turn in the map; we know it exists.
+                steps.push(PathStep::Turn(TurnID {
+                    parent: map.get_l(pair[0]).dst_i,
+                    src: pair[0],
+                    dst: pair[1],
+                }));
+            }
+            steps.push(PathStep::Lane(req.end.lane()));
+            assert_eq!(steps[0], PathStep::Lane(req.start.lane()));
+            // TODO No uber-turns yet!
+            Ok(Path::new(map, steps, req, Vec::new()))
+        }
+        None => bail!(
+            "path_v2_to_v1 found road-based path, but not a lane-based path matching it for {}",
+            req
+        ),
+    }
+}
+
+// TODO This is an attempt that looks at windows of 2 roads at a time to pick particular lanes and
+// turns. It doesn't work in most cases with multiple lane choices -- I think we need at least a
+// window of 3 roads. I'll write that function in the future, for the simulation layer to use
+// "lazily".
+fn _broken_path_v2_to_v1(
+    req: PathRequest,
+    mut road_steps: Vec<DirectedRoadID>,
+    map: &Map,
+) -> Result<Path> {
+    let mut path_steps = Vec::new();
+
+    // Pick the starting lane.
+    {
+        let lanes = road_steps.remove(0).lanes(req.constraints, map);
+        // TODO During the transition, try to use the original requested start lane. Relax this
+        // later to produce more realistic paths!
+        if !lanes.contains(&req.start.lane()) {
+            bail!(
+                "path_v2_to_v1 found a case where we can't start at the requested lane: {}",
+                req
+            );
+        }
+        path_steps.push(PathStep::Lane(req.start.lane()));
+    }
+    let last_road = map.get_l(req.end.lane()).get_directed_parent(map);
+
+    for road in road_steps {
+        let prev_lane = if let Some(PathStep::Lane(l)) = path_steps.last() {
+            *l
+        } else {
+            unreachable!()
+        };
+
+        let mut current_lanes = road.lanes(req.constraints, map);
+        // Filter current_lanes based on available turns.
+        let parent = map.get_l(prev_lane).dst_i;
+        current_lanes.retain(|dst| {
+            map.maybe_get_t(TurnID {
+                parent,
+                src: prev_lane,
+                dst: *dst,
+            })
+            .is_some()
+        });
+        if current_lanes.is_empty() {
+            error!("Lookahead failed. Req: {}", req);
+            error!("Path so far:");
+            for x in &path_steps {
+                error!("- {:?}", x);
+            }
+
+            bail!(
+                "path_v2_to_v1 found a case where lookahead failed at {}: {}",
+                parent,
+                req
+            );
+        }
+        if road == last_road {
+            current_lanes.retain(|l| *l == req.end.lane());
+        }
+
+        // TODO We could include the old lane-changing penalties here, but I'm not sure it's worth
+        // the complication. The simulation layer will end up tuning those anyway.
+        let next_lane = current_lanes[0];
+        path_steps.push(PathStep::Turn(TurnID {
+            parent,
+            src: prev_lane,
+            dst: next_lane,
+        }));
+        path_steps.push(PathStep::Lane(next_lane));
+    }
+
+    // Sanity check we end in the right place.
+    assert_eq!(
+        Some(PathStep::Lane(req.end.lane())),
+        path_steps.last().cloned()
+    );
+    // TODO No uber-turns yet!
+    Ok(Path::new(map, path_steps, req, Vec::new()))
+}

map_model/src/pathfind/vehicles.rs

@@ -14,8 +14,8 @@ use crate::pathfind::node_map::{deserialize_nodemap, NodeMap};
 use crate::pathfind::uber_turns::{IntersectionCluster, UberTurn};
 use crate::pathfind::zone_cost;
 use crate::{
-    DrivingSide, Lane, LaneID, Map, Path, PathConstraints, PathRequest, PathStep, RoutingParams,
-    Traversable, Turn, TurnID, TurnType,
+    DirectedRoadID, DrivingSide, Lane, LaneID, LaneType, Map, MovementID, Path, PathConstraints,
+    PathRequest, PathStep, RoutingParams, Traversable, Turn, TurnID, TurnType,
 };
 
 #[derive(Serialize, Deserialize)]
@@ -319,3 +319,73 @@ pub fn vehicle_cost(
 
     base + Duration::seconds(extra_penalty as f64)
 }
+
+/// This returns the pathfinding cost of crossing one road and turn. This is also expressed in
+/// units of time. It factors in the ideal time to cross the space, along with penalties for
+/// entering an access-restricted zone, taking an unprotected turn, and so on.
+// TODO Remove vehicle_cost after pathfinding v2 transition is done.
+pub fn vehicle_cost_v2(
+    dr: DirectedRoadID,
+    mvmnt: MovementID,
+    constraints: PathConstraints,
+    params: &RoutingParams,
+    map: &Map,
+) -> Duration {
+    let mvmnt = mvmnt.get(map).unwrap();
+    let max_speed = match constraints {
+        PathConstraints::Car | PathConstraints::Bus | PathConstraints::Train => None,
+        PathConstraints::Bike => Some(crate::MAX_BIKE_SPEED),
+        PathConstraints::Pedestrian => unreachable!(),
+    };
+    let t1 = map.get_r(dr.id).center_pts.length()
+        / Traversable::max_speed_along_road(dr, max_speed, constraints, map);
+    let t2 = mvmnt.geom.length()
+        / Traversable::max_speed_along_movement(mvmnt.id, max_speed, constraints, map);
+
+    let base = match constraints {
+        PathConstraints::Car | PathConstraints::Train => t1 + t2,
+        PathConstraints::Bike => {
+            // TODO If we're on a driving lane, higher speed limit is worse.
+            // TODO Bike lanes next to parking is dangerous.
+
+            // TODO Prefer bike lanes, then bus lanes, then driving lanes. For now, express that by
+            // multiplying the base cost.
+            let lt_penalty = if dr.has_lanes(LaneType::Biking, map) {
+                params.bike_lane_penalty
+            } else if dr.has_lanes(LaneType::Bus, map) {
+                params.bus_lane_penalty
+            } else {
+                params.driving_lane_penalty
+            };
+
+            lt_penalty * (t1 + t2)
+        }
+        PathConstraints::Bus => {
+            // Like Car, but prefer bus lanes.
+            let lt_penalty = if dr.has_lanes(LaneType::Bus, map) {
+                1.0
+            } else {
+                1.1
+            };
+            lt_penalty * (t1 + t2)
+        }
+        PathConstraints::Pedestrian => unreachable!(),
+    };
+
+    // Penalize unprotected turns at a stop sign from smaller to larger roads.
+    let unprotected_turn_type = if map.get_config().driving_side == DrivingSide::Right {
+        TurnType::Left
+    } else {
+        TurnType::Right
+    };
+    let rank_from = map.get_r(dr.id).get_detailed_rank();
+    let rank_to = map.get_r(mvmnt.id.to.id).get_detailed_rank();
+    if mvmnt.turn_type == unprotected_turn_type
+        && rank_from < rank_to
+        && map.get_i(mvmnt.id.parent).is_stop_sign()
+    {
+        base + params.unprotected_turn_penalty
+    } else {
+        base
+    }
+}

map_model/src/traversable.rs

@@ -5,7 +5,7 @@ use serde::{Deserialize, Serialize};
 
 use geom::{Angle, Distance, PolyLine, Pt2D, Speed};
 
-use crate::{Direction, LaneID, Map, PathConstraints, TurnID};
+use crate::{DirectedRoadID, Direction, LaneID, Map, MovementID, PathConstraints, TurnID};
 
 /// Represents a specific point some distance along a lane.
 #[derive(Clone, Copy, Debug, PartialEq, Eq, PartialOrd, Ord, Serialize, Deserialize)]
@@ -212,32 +212,69 @@ impl Traversable {
         constraints: PathConstraints,
         map: &Map,
     ) -> Speed {
-        let base = match self {
-            Traversable::Lane(l) => {
-                let road = map.get_parent(*l);
-                let percent_incline = if road.dir(*l) == Direction::Fwd {
-                    road.percent_incline
-                } else {
-                    -road.percent_incline
-                };
+        match self {
+            Traversable::Lane(l) => Traversable::max_speed_along_road(
+                map.get_l(*l).get_directed_parent(map),
+                max_speed_on_flat_ground,
+                constraints,
+                map,
+            ),
+            Traversable::Turn(t) => Traversable::max_speed_along_movement(
+                t.to_movement(map),
+                max_speed_on_flat_ground,
+                constraints,
+                map,
+            ),
+        }
+    }
 
-                if constraints == PathConstraints::Bike {
-                    // We assume every bike has a max_speed defined.
-                    bike_speed_on_incline(max_speed_on_flat_ground.unwrap(), percent_incline)
-                } else if constraints == PathConstraints::Pedestrian {
-                    // We assume every pedestrian has a max_speed defined.
-                    walking_speed_on_incline(max_speed_on_flat_ground.unwrap(), percent_incline)
-                } else {
-                    // Incline doesn't affect cars, buses, or trains
-                    road.speed_limit
-                }
-            }
-            // TODO Ignore elevation on turns?
-            Traversable::Turn(t) => map
-                .get_parent(t.src)
-                .speed_limit
-                .min(map.get_parent(t.dst).speed_limit),
+    /// The single definitive place to determine how fast somebody could go along a single road.
+    /// This should be used for pathfinding and simulation.
+    pub fn max_speed_along_road(
+        dr: DirectedRoadID,
+        max_speed_on_flat_ground: Option<Speed>,
+        constraints: PathConstraints,
+        map: &Map,
+    ) -> Speed {
+        let road = map.get_r(dr.id);
+        let percent_incline = if dr.dir == Direction::Fwd {
+            road.percent_incline
+        } else {
+            -road.percent_incline
         };
+
+        let base = if constraints == PathConstraints::Bike {
+            // We assume every bike has a max_speed defined.
+            bike_speed_on_incline(max_speed_on_flat_ground.unwrap(), percent_incline)
+        } else if constraints == PathConstraints::Pedestrian {
+            // We assume every pedestrian has a max_speed defined.
+            walking_speed_on_incline(max_speed_on_flat_ground.unwrap(), percent_incline)
+        } else {
+            debug_assert!(max_speen_on_flat_ground.is_none());
+            // Incline doesn't affect cars, buses, or trains
+            road.speed_limit
+        };
+
+        if let Some(s) = max_speed_on_flat_ground {
+            base.min(s)
+        } else {
+            base
+        }
+    }
+
+    /// The single definitive place to determine how fast somebody could go along a single
+    /// movement.  This should be used for pathfinding and simulation.
+    pub fn max_speed_along_movement(
+        mvmnt: MovementID,
+        max_speed_on_flat_ground: Option<Speed>,
+        _: PathConstraints,
+        map: &Map,
+    ) -> Speed {
+        // TODO Ignore elevation on turns?
+        let base = map
+            .get_r(mvmnt.from.id)
+            .speed_limit
+            .min(map.get_r(mvmnt.to.id).speed_limit);
         if let Some(s) = max_speed_on_flat_ground {
             base.min(s)
         } else {