From 3060f94989fa72f7469505fe61c8ced638ae8d63 Mon Sep 17 00:00:00 2001 From: Dustin Carlino Date: Thu, 8 Apr 2021 08:57:59 -0700 Subject: [PATCH] Start a pathfinding v2 API. #555 (#596) The v1 path that all callers need is available by transforming PathV2 to v1. The plan is to now gradually change callers to natively use PathV2 instead. --- map_model/src/connectivity/mod.rs | 3 +- map_model/src/lib.rs | 4 +- map_model/src/map.rs | 17 +- map_model/src/pathfind/ch.rs | 9 +- map_model/src/pathfind/dijkstra.rs | 23 ++- map_model/src/pathfind/mod.rs | 1 + map_model/src/pathfind/pathfinder.rs | 15 +- map_model/src/pathfind/v2.rs | 235 +++++++++++++++++++-------- map_model/src/pathfind/vehicles.rs | 14 +- map_model/src/pathfind/walking.rs | 97 +++++------ sim/src/cap.rs | 6 +- 11 files changed, 267 insertions(+), 157 deletions(-) diff --git a/map_model/src/connectivity/mod.rs b/map_model/src/connectivity/mod.rs index 6a9423f4cf..1ad7f9aea3 100644 --- a/map_model/src/connectivity/mod.rs +++ b/map_model/src/connectivity/mod.rs @@ -106,7 +106,8 @@ pub fn debug_vehicle_costs( return None; } - let (_, cost) = crate::pathfind::dijkstra::pathfind(req.clone(), map.routing_params(), map)?; + let cost = + crate::pathfind::dijkstra::pathfind(req.clone(), map.routing_params(), map)?.get_cost(); let graph = build_graph_for_vehicles(map, req.constraints); let road_costs = petgraph::algo::dijkstra( diff --git a/map_model/src/lib.rs b/map_model/src/lib.rs index 0adda4ca1f..c33787d4f1 100644 --- a/map_model/src/lib.rs +++ b/map_model/src/lib.rs @@ -56,7 +56,9 @@ pub use crate::objects::turn::{ pub use crate::objects::zone::{AccessRestrictions, Zone}; pub use crate::pathfind::uber_turns::{IntersectionCluster, UberTurn}; use crate::pathfind::Pathfinder; -pub use crate::pathfind::{Path, PathConstraints, PathRequest, PathStep, RoutingParams}; +pub use crate::pathfind::{ + Path, PathConstraints, PathRequest, PathStep, PathStepV2, PathV2, RoutingParams, +}; pub use crate::traversable::{Position, Traversable, MAX_BIKE_SPEED, MAX_WALKING_SPEED}; mod city; diff --git a/map_model/src/map.rs b/map_model/src/map.rs index 5a21e04dea..a6ccb87225 100644 --- a/map_model/src/map.rs +++ b/map_model/src/map.rs @@ -579,23 +579,28 @@ impl Map { pub fn pathfind(&self, req: PathRequest) -> Result { assert!(!self.pathfinder_dirty); - self.pathfinder + let path = self + .pathfinder .pathfind(req.clone(), self) - .ok_or_else(|| anyhow!("can't fulfill {}", req)) + .ok_or_else(|| anyhow!("can't fulfill {}", req))?; + path.to_v1(self) } pub fn pathfind_avoiding_roads( &self, req: PathRequest, avoid: BTreeSet, - ) -> Option { + ) -> Result { assert!(!self.pathfinder_dirty); - self.pathfinder.pathfind_avoiding_roads(req, avoid, self) + let path = self.pathfinder.pathfind_avoiding_roads(req, avoid, self)?; + path.to_v1(self) } pub fn pathfind_with_params(&self, req: PathRequest, params: &RoutingParams) -> Result { assert!(!self.pathfinder_dirty); - self.pathfinder + let path = self + .pathfinder .pathfind_with_params(req.clone(), params, self) - .ok_or_else(|| anyhow!("can't fulfill {}", req)) + .ok_or_else(|| anyhow!("can't fulfill {}", req))?; + path.to_v1(self) } pub fn should_use_transit( diff --git a/map_model/src/pathfind/ch.rs b/map_model/src/pathfind/ch.rs index abc692a16c..800620a315 100644 --- a/map_model/src/pathfind/ch.rs +++ b/map_model/src/pathfind/ch.rs @@ -9,7 +9,7 @@ use geom::Duration; use crate::pathfind::dijkstra; use crate::pathfind::vehicles::VehiclePathfinder; use crate::pathfind::walking::SidewalkPathfinder; -use crate::{BusRouteID, BusStopID, Map, Path, PathConstraints, PathRequest, Position}; +use crate::{BusRouteID, BusStopID, Map, PathConstraints, PathRequest, PathV2, Position}; #[derive(Serialize, Deserialize)] pub struct ContractionHierarchyPathfinder { @@ -53,8 +53,8 @@ impl ContractionHierarchyPathfinder { } } - pub fn pathfind(&self, req: PathRequest, map: &Map) -> Option { - (match req.constraints { + pub fn pathfind(&self, req: PathRequest, map: &Map) -> Option { + match req.constraints { PathConstraints::Pedestrian => self.walking_graph.pathfind(req, map), PathConstraints::Car => self.car_graph.pathfind(req, map), PathConstraints::Bike => self.bike_graph.pathfind(req, map), @@ -62,8 +62,7 @@ impl ContractionHierarchyPathfinder { // Light rail networks are absolutely tiny; using a contraction hierarchy for them is // overkill. And in fact, it costs a bit of memory and file size, so don't do it! PathConstraints::Train => dijkstra::pathfind(req, map.routing_params(), map), - }) - .map(|(path, _)| path) + } } pub fn should_use_transit( diff --git a/map_model/src/pathfind/dijkstra.rs b/map_model/src/pathfind/dijkstra.rs index f07aecc56b..4c8c860cb7 100644 --- a/map_model/src/pathfind/dijkstra.rs +++ b/map_model/src/pathfind/dijkstra.rs @@ -2,21 +2,21 @@ use std::collections::BTreeSet; +use anyhow::Result; use petgraph::graphmap::DiGraphMap; use geom::Duration; -use crate::pathfind::v2::path_v2_to_v1; use crate::pathfind::walking::{one_step_walking_path, walking_path_to_steps, WalkingNode}; use crate::pathfind::{vehicle_cost, zone_cost}; use crate::{ - DirectedRoadID, Map, MovementID, Path, PathConstraints, PathRequest, RoadID, RoutingParams, + DirectedRoadID, Map, MovementID, PathConstraints, PathRequest, PathV2, RoadID, RoutingParams, Traversable, }; // TODO These should maybe keep the DiGraphMaps as state. It's cheap to recalculate it for edits. -pub fn pathfind(req: PathRequest, params: &RoutingParams, map: &Map) -> Option<(Path, Duration)> { +pub fn pathfind(req: PathRequest, params: &RoutingParams, map: &Map) -> Option { if req.constraints == PathConstraints::Pedestrian { pathfind_walking(req, map) } else { @@ -42,7 +42,7 @@ pub fn pathfind_avoiding_roads( req: PathRequest, avoid: BTreeSet, map: &Map, -) -> Option<(Path, Duration)> { +) -> Result { assert_eq!(req.constraints, PathConstraints::Car); let mut graph = DiGraphMap::new(); for dr in map.all_directed_roads_for(req.constraints) { @@ -54,7 +54,8 @@ pub fn pathfind_avoiding_roads( } } - calc_path(graph, req, map.routing_params(), map) + calc_path(graph, req.clone(), map.routing_params(), map) + .ok_or_else(|| anyhow!("No path for {} avoiding {} roads", req, avoid.len())) } fn calc_path( @@ -62,9 +63,9 @@ fn calc_path( req: PathRequest, params: &RoutingParams, map: &Map, -) -> Option<(Path, Duration)> { +) -> Option { let end = map.get_l(req.end.lane()).get_directed_parent(); - let (cost, path) = petgraph::algo::astar( + let (cost, steps) = petgraph::algo::astar( &graph, map.get_l(req.start.lane()).get_directed_parent(), |dr| dr == end, @@ -74,10 +75,8 @@ fn calc_path( }, |_| Duration::ZERO, )?; - // TODO No uber-turns yet - let path = path_v2_to_v1(req, path, Vec::new(), map).ok()?; - Some((path, cost)) + Some(PathV2::from_roads(steps, req, cost, Vec::new(), map)) } pub fn build_graph_for_pedestrians(map: &Map) -> DiGraphMap { @@ -120,7 +119,7 @@ pub fn build_graph_for_pedestrians(map: &Map) -> DiGraphMap Option<(Path, Duration)> { +fn pathfind_walking(req: PathRequest, map: &Map) -> Option { if req.start.lane() == req.end.lane() { return Some(one_step_walking_path(req, map)); } @@ -137,5 +136,5 @@ fn pathfind_walking(req: PathRequest, map: &Map) -> Option<(Path, Duration)> { |_| Duration::ZERO, )?; let steps = walking_path_to_steps(nodes, map); - Some((Path::new(map, steps, req, Vec::new()), cost)) + Some(PathV2::new(steps, req, cost, Vec::new())) } diff --git a/map_model/src/pathfind/mod.rs b/map_model/src/pathfind/mod.rs index 4928cf9530..e8161ae19a 100644 --- a/map_model/src/pathfind/mod.rs +++ b/map_model/src/pathfind/mod.rs @@ -9,6 +9,7 @@ pub use self::ch::ContractionHierarchyPathfinder; pub use self::dijkstra::{build_graph_for_pedestrians, build_graph_for_vehicles}; pub use self::pathfinder::Pathfinder; pub use self::v1::{Path, PathRequest, PathStep}; +pub use self::v2::{PathStepV2, PathV2}; pub use self::vehicles::vehicle_cost; pub use self::walking::WalkingNode; use crate::{osm, Lane, LaneID, LaneType, Map, MovementID}; diff --git a/map_model/src/pathfind/pathfinder.rs b/map_model/src/pathfind/pathfinder.rs index ae2e9e6090..c0b3327f81 100644 --- a/map_model/src/pathfind/pathfinder.rs +++ b/map_model/src/pathfind/pathfinder.rs @@ -1,12 +1,13 @@ use std::collections::BTreeSet; +use anyhow::Result; use serde::{Deserialize, Serialize}; use abstutil::Timer; use crate::pathfind::ch::ContractionHierarchyPathfinder; use crate::pathfind::dijkstra; -use crate::{BusRouteID, BusStopID, Map, Path, PathRequest, Position, RoadID, RoutingParams}; +use crate::{BusRouteID, BusStopID, Map, PathRequest, PathV2, Position, RoadID, RoutingParams}; /// Most of the time, prefer using the faster contraction hierarchies. But sometimes, callers can /// explicitly opt into a slower (but preparation-free) pathfinder that just uses Dijkstra's @@ -19,7 +20,7 @@ pub enum Pathfinder { impl Pathfinder { /// Finds a path from a start to an end for a certain type of agent. - pub fn pathfind(&self, req: PathRequest, map: &Map) -> Option { + pub fn pathfind(&self, req: PathRequest, map: &Map) -> Option { self.pathfind_with_params(req, map.routing_params(), map) } @@ -30,17 +31,17 @@ impl Pathfinder { req: PathRequest, params: &RoutingParams, map: &Map, - ) -> Option { + ) -> Option { if params != map.routing_params() { // If the params differ from the ones baked into the map, the CHs won't match. This // should only be happening from the debug UI; be very obnoxious if we start calling it // from the simulation or something else. warn!("Pathfinding slowly for {} with custom params", req); - return dijkstra::pathfind(req, params, map).map(|(path, _)| path); + return dijkstra::pathfind(req, params, map); } match self { - Pathfinder::Dijkstra => dijkstra::pathfind(req, params, map).map(|(path, _)| path), + Pathfinder::Dijkstra => dijkstra::pathfind(req, params, map), Pathfinder::CH(ref p) => p.pathfind(req, map), } } @@ -52,8 +53,8 @@ impl Pathfinder { req: PathRequest, avoid: BTreeSet, map: &Map, - ) -> Option { - dijkstra::pathfind_avoiding_roads(req, avoid, map).map(|(path, _)| path) + ) -> Result { + dijkstra::pathfind_avoiding_roads(req, avoid, map) } // TODO Consider returning the walking-only path in the failure case, to avoid wasting work diff --git a/map_model/src/pathfind/v2.rs b/map_model/src/pathfind/v2.rs index 6b81e35e0a..337057c6ab 100644 --- a/map_model/src/pathfind/v2.rs +++ b/map_model/src/pathfind/v2.rs @@ -3,82 +3,181 @@ //! things here will probably move into pathfind/mod.rs. use anyhow::Result; +use serde::{Deserialize, Serialize}; + +use geom::Duration; use crate::pathfind::uber_turns::UberTurnV2; -use crate::{DirectedRoadID, Map, Path, PathConstraints, PathRequest, PathStep, TurnID, UberTurn}; +use crate::{ + DirectedRoadID, Map, MovementID, Path, PathConstraints, PathRequest, PathStep, TurnID, UberTurn, +}; -/// 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( +/// One step along a path. +#[derive(Clone, Debug, Serialize, Deserialize)] +pub enum PathStepV2 { + /// Original direction + Along(DirectedRoadID), + /// Opposite direction, sidewalks only + Contraflow(DirectedRoadID), + Movement(MovementID), +} + +/// A path between two endpoints for a particular mode. This representation is immutable and doesn't +/// prescribe specific lanes and turns to follow. +#[derive(Clone, Debug, Serialize, Deserialize)] +pub struct PathV2 { + steps: Vec, + // TODO There will be a PathRequestV2, but I'm not sure how it'll change yet. req: PathRequest, - road_steps: Vec, - uber_turns_v2: Vec, - map: &Map, -) -> Result { - // 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); - } - } + cost: Duration, + // TODO Temporarily we'll keep plumbing these along for path_v2_to_v1 to work, but we'll + // probably just discover uber-turns lazily at the simulation layer instead. + uber_turns: Vec, +} + +impl PathV2 { + pub(crate) fn new( + steps: Vec, + req: PathRequest, + cost: Duration, + uber_turns: Vec, + ) -> PathV2 { + // TODO Port validate_continuity and validate_restrictions? + PathV2 { + steps, + req, + cost, + uber_turns, } } - match petgraph::algo::astar( - &graph, - req.start.lane(), - |l| l == req.end.lane(), - |(_, _, t)| { - // Normally opportunistic lane-changing adjusts the path live, but that doesn't work - // near uber-turns. So still use some of the penalties here. - let (lt, lc, slow_lane) = map.get_t(*t).penalty(map); - let mut extra_penalty = lt + lc; - if req.constraints == PathConstraints::Bike { - extra_penalty = slow_lane; - } - // Always treat every lane/turn as at least cost 1; otherwise A* can't understand that - // a final path with 10 steps costs more than one with 5. The road-based pathfinding - // has already chosen the overall route; when we're picking individual lanes, the - // length of each lane along one road is going to be about the same. - let base = 1; - base + extra_penalty - }, - |_| 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())); - let uber_turns = find_uber_turns(&steps, map, uber_turns_v2); - Ok(Path::new(map, steps, req, uber_turns)) + /// Vehicle implementations often just calculate the sequence of roads. Turn that into + /// PathStepV2 here. + pub(crate) fn from_roads( + mut roads: Vec, + req: PathRequest, + cost: Duration, + uber_turns: Vec, + map: &Map, + ) -> PathV2 { + let mut steps = Vec::new(); + for pair in roads.windows(2) { + steps.push(PathStepV2::Along(pair[0])); + steps.push(PathStepV2::Movement(MovementID { + from: pair[0], + to: pair[1], + parent: pair[0].dst_i(map), + crosswalk: false, + })); } - None => bail!( - "path_v2_to_v1 found road-based path, but not a lane-based path matching it for {}", - req - ), + steps.push(PathStepV2::Along(roads.pop().unwrap())); + PathV2::new(steps, req, cost, uber_turns) + } + + /// The original PathRequest used to produce this path. + pub fn get_req(&self) -> &PathRequest { + &self.req + } + + /// All steps in this path. + pub fn get_steps(&self) -> &Vec { + &self.steps + } + + /// The time needed to perform this path. This time is not a lower bound; physically following + /// the path might be faster. This time incorporates costs like using sub-optimal lanes or + /// taking difficult turns. + pub fn get_cost(&self) -> Duration { + self.cost + } + + /// Transform a sequence of roads representing a path into the current lane-based path, by + /// picking particular lanes and turns to use. + pub fn to_v1(self, map: &Map) -> Result { + if self.req.constraints == PathConstraints::Pedestrian { + return self.to_v1_walking(map); + } + + // 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. + let mut graph = petgraph::graphmap::DiGraphMap::new(); + for step in &self.steps { + if let PathStepV2::Movement(mvmnt) = step { + for src in mvmnt.from.lanes(self.req.constraints, map) { + for dst in mvmnt.to.lanes(self.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, + self.req.start.lane(), + |l| l == self.req.end.lane(), + |(_, _, t)| { + // Normally opportunistic lane-changing adjusts the path live, but that doesn't work + // near uber-turns. So still use some of the penalties here. + let (lt, lc, slow_lane) = map.get_t(*t).penalty(map); + let mut extra_penalty = lt + lc; + if self.req.constraints == PathConstraints::Bike { + extra_penalty = slow_lane; + } + // Always treat every lane/turn as at least cost 1; otherwise A* can't understand + // that a final path with 10 steps costs more than one with 5. The + // road-based pathfinding has already chosen the overall route; when + // we're picking individual lanes, the length of each lane along one + // road is going to be about the same. + let base = 1; + base + extra_penalty + }, + |_| 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(self.req.end.lane())); + assert_eq!(steps[0], PathStep::Lane(self.req.start.lane())); + let uber_turns = find_uber_turns(&steps, map, self.uber_turns); + Ok(Path::new(map, steps, self.req, uber_turns)) + } + None => bail!( + "Can't transform a road-based path to a lane-based path for {}", + self.req + ), + } + } + + fn to_v1_walking(self, map: &Map) -> Result { + let mut steps = Vec::new(); + for step in self.steps { + steps.push(match step { + PathStepV2::Along(r) => PathStep::Lane(r.must_get_sidewalk(map)), + PathStepV2::Contraflow(r) => PathStep::ContraflowLane(r.must_get_sidewalk(map)), + PathStepV2::Movement(mvmnt) => PathStep::Turn(TurnID { + src: mvmnt.from.must_get_sidewalk(map), + dst: mvmnt.to.must_get_sidewalk(map), + parent: mvmnt.parent, + }), + }); + } + Ok(Path::new(map, steps, self.req, Vec::new())) } } diff --git a/map_model/src/pathfind/vehicles.rs b/map_model/src/pathfind/vehicles.rs index 859d70ffdd..773b62ae57 100644 --- a/map_model/src/pathfind/vehicles.rs +++ b/map_model/src/pathfind/vehicles.rs @@ -12,11 +12,10 @@ use geom::{Distance, Duration}; use crate::pathfind::ch::round; use crate::pathfind::node_map::{deserialize_nodemap, NodeMap}; use crate::pathfind::uber_turns::{IntersectionCluster, UberTurnV2}; -use crate::pathfind::v2::path_v2_to_v1; use crate::pathfind::zone_cost; use crate::{ - DirectedRoadID, Direction, DrivingSide, LaneType, Map, MovementID, Path, PathConstraints, - PathRequest, RoadID, RoutingParams, Traversable, TurnType, + DirectedRoadID, Direction, DrivingSide, LaneType, Map, MovementID, PathConstraints, + PathRequest, PathV2, RoadID, RoutingParams, Traversable, TurnType, }; #[derive(Serialize, Deserialize)] @@ -93,7 +92,7 @@ impl VehiclePathfinder { } } - pub fn pathfind(&self, req: PathRequest, map: &Map) -> Option<(Path, Duration)> { + pub fn pathfind(&self, req: PathRequest, map: &Map) -> Option { assert!(!map.get_l(req.start.lane()).is_walkable()); let mut calc = self .path_calc @@ -120,14 +119,13 @@ impl VehiclePathfinder { road_steps.push(mvmnt.to); } road_steps.pop(); - // Also remember the uber-turn exists, so we can reconstruct it in - // path_v2_to_v1. + // Also remember the uber-turn exists. uber_turns.push(self.uber_turns[ut].clone()); } } } - let path = path_v2_to_v1(req, road_steps, uber_turns, map).ok()?; - Some((path, Duration::seconds(raw_path.get_weight() as f64))) + let cost = Duration::seconds(raw_path.get_weight() as f64); + Some(PathV2::from_roads(road_steps, req, cost, uber_turns, map)) } pub fn apply_edits(&mut self, map: &Map) { diff --git a/map_model/src/pathfind/walking.rs b/map_model/src/pathfind/walking.rs index d5659e8b37..49c2322c02 100644 --- a/map_model/src/pathfind/walking.rs +++ b/map_model/src/pathfind/walking.rs @@ -16,8 +16,8 @@ use crate::pathfind::node_map::{deserialize_nodemap, NodeMap}; use crate::pathfind::vehicles::VehiclePathfinder; use crate::pathfind::zone_cost; use crate::{ - BusRoute, BusRouteID, BusStopID, DirectedRoadID, IntersectionID, Map, Path, PathConstraints, - PathRequest, PathStep, Position, Traversable, + BusRoute, BusRouteID, BusStopID, DirectedRoadID, IntersectionID, Map, MovementID, + PathConstraints, PathRequest, PathStepV2, PathV2, Position, Traversable, }; #[derive(Serialize, Deserialize)] @@ -107,7 +107,7 @@ impl SidewalkPathfinder { self.graph = fast_paths::prepare_with_order(&input_graph, &node_ordering).unwrap(); } - pub fn pathfind(&self, req: PathRequest, map: &Map) -> Option<(Path, Duration)> { + pub fn pathfind(&self, req: PathRequest, map: &Map) -> Option { if req.start.lane() == req.end.lane() { return Some(one_step_walking_path(req, map)); } @@ -124,7 +124,7 @@ impl SidewalkPathfinder { let nodes = self.nodes.translate(&raw_path); let steps = walking_path_to_steps(nodes, map); let cost = Duration::seconds(raw_path.get_weight() as f64); - Some((Path::new(map, steps, req, Vec::new()), cost)) + Some(PathV2::new(steps, req, cost, Vec::new())) } /// Attempt the pathfinding and see if we should ride a bus. If so, says (stop1, optional stop @@ -328,10 +328,10 @@ fn transit_input_graph( constraints: route.route_type, }; let maybe_driving_cost = match route.route_type { - PathConstraints::Bus => bus_graph.pathfind(req, map).map(|(_, cost)| cost), + PathConstraints::Bus => bus_graph.pathfind(req, map).map(|p| p.get_cost()), // We always use Dijkstra for trains PathConstraints::Train => { - dijkstra::pathfind(req, map.routing_params(), map).map(|(_, cost)| cost) + dijkstra::pathfind(req, map.routing_params(), map).map(|p| p.get_cost()) } _ => unreachable!(), }; @@ -357,10 +357,10 @@ fn transit_input_graph( constraints: route.route_type, }; let maybe_driving_cost = match route.route_type { - PathConstraints::Bus => bus_graph.pathfind(req, map).map(|(_, cost)| cost), + PathConstraints::Bus => bus_graph.pathfind(req, map).map(|p| p.get_cost()), // We always use Dijkstra for trains PathConstraints::Train => { - dijkstra::pathfind(req, map.routing_params(), map).map(|(_, cost)| cost) + dijkstra::pathfind(req, map.routing_params(), map).map(|p| p.get_cost()) } _ => unreachable!(), }; @@ -404,11 +404,11 @@ fn transit_input_graph( } } -pub fn walking_path_to_steps(path: Vec, map: &Map) -> Vec { - let mut steps: Vec = Vec::new(); +pub fn walking_path_to_steps(path: Vec, map: &Map) -> Vec { + let mut steps = Vec::new(); for pair in path.windows(2) { - let (r1, l1_endpt) = match pair[0] { + let (r1, r1_endpt) = match pair[0] { WalkingNode::SidewalkEndpoint(r, endpt) => (r, endpt), WalkingNode::RideBus(_) => unreachable!(), WalkingNode::LeaveMap(_) => unreachable!(), @@ -419,52 +419,59 @@ pub fn walking_path_to_steps(path: Vec, map: &Map) -> Vec WalkingNode::LeaveMap(_) => unreachable!(), }; - let l1 = r1.must_get_sidewalk(map); - let l2 = r2.must_get_sidewalk(map); - - if l1 == l2 { - if l1_endpt { - steps.push(PathStep::ContraflowLane(l1)); + if r1 == r2 { + if r1_endpt { + steps.push(PathStepV2::Contraflow(r1)); } else { - steps.push(PathStep::Lane(l1)); + steps.push(PathStepV2::Along(r1)); } } else { - let i = { - let l = map.get_l(l1); - if l1_endpt { - l.dst_i - } else { - l.src_i - } + let i = if r1_endpt { + r1.dst_i(map) + } else { + r1.src_i(map) }; - // Could assert the intersection matches (l2, l2_endpt). - if let Some(turn) = map.get_turn_between(l1, l2, i) { - steps.push(PathStep::Turn(turn)); + // Could assert the intersection matches (r2, r2_endpt). + if map + .get_turn_between(r1.must_get_sidewalk(map), r2.must_get_sidewalk(map), i) + .is_some() + { + steps.push(PathStepV2::Movement(MovementID { + from: r1, + to: r2, + parent: i, + crosswalk: true, + })); } else { println!("walking_path_to_steps has a weird path:"); for s in &path { println!("- {:?}", s); } - panic!("No turn from {} to {} at {}", l1, l2, i); + panic!( + "No turn from {} ({}) to {} ({}) at {}", + r1, + r1.must_get_sidewalk(map), + r2, + r2.must_get_sidewalk(map), + i + ); } } } // Don't start or end a path in a turn; sim layer breaks. - if let PathStep::Turn(t) = steps[0] { - let lane = map.get_l(t.src); - if lane.src_i == t.parent { - steps.insert(0, PathStep::ContraflowLane(lane.id)); + if let PathStepV2::Movement(mvmnt) = steps[0] { + if mvmnt.from.src_i(map) == mvmnt.parent { + steps.insert(0, PathStepV2::Contraflow(mvmnt.from)); } else { - steps.insert(0, PathStep::Lane(lane.id)); + steps.insert(0, PathStepV2::Along(mvmnt.from)); } } - if let PathStep::Turn(t) = steps.last().unwrap() { - let lane = map.get_l(t.dst); - if lane.src_i == t.parent { - steps.push(PathStep::Lane(lane.id)); + if let PathStepV2::Movement(mvmnt) = steps.last().cloned().unwrap() { + if mvmnt.to.src_i(map) == mvmnt.parent { + steps.push(PathStepV2::Along(mvmnt.to)); } else { - steps.push(PathStep::ContraflowLane(lane.id)); + steps.push(PathStepV2::Contraflow(mvmnt.to)); } } @@ -472,15 +479,13 @@ pub fn walking_path_to_steps(path: Vec, map: &Map) -> Vec } // TODO Do we even need this at all? -pub fn one_step_walking_path(req: PathRequest, map: &Map) -> (Path, Duration) { +pub fn one_step_walking_path(req: PathRequest, map: &Map) -> PathV2 { // Weird case, but it can happen for walking from a building path to a bus stop that're // actually at the same spot. - let steps = if req.start.dist_along() == req.end.dist_along() { - vec![PathStep::Lane(req.start.lane())] - } else if req.start.dist_along() < req.end.dist_along() { - vec![PathStep::Lane(req.start.lane())] + let step = if req.start.dist_along() <= req.end.dist_along() { + PathStepV2::Along(map.get_l(req.start.lane()).get_directed_parent()) } else { - vec![PathStep::ContraflowLane(req.start.lane())] + PathStepV2::Contraflow(map.get_l(req.start.lane()).get_directed_parent()) }; let mut cost = (req.start.dist_along() - req.end.dist_along()).abs() / Traversable::Lane(req.start.lane()).max_speed_along( @@ -491,5 +496,5 @@ pub fn one_step_walking_path(req: PathRequest, map: &Map) -> (Path, Duration) { if map.get_l(req.start.lane()).is_shoulder() { cost = 2.0 * cost; } - (Path::new(map, steps, req, Vec::new()), cost) + PathV2::new(vec![step], req, cost, Vec::new()) } diff --git a/sim/src/cap.rs b/sim/src/cap.rs index 6ef5095bd3..18e89a521c 100644 --- a/sim/src/cap.rs +++ b/sim/src/cap.rs @@ -108,13 +108,13 @@ impl CapSimState { } } match map.pathfind_avoiding_roads(path.get_req().clone(), avoid_roads) { - Some(path) => CapResult::Reroute(path), - None => { + Ok(path) => CapResult::Reroute(path), + Err(err) => { if let Some(delay) = self.delay_trips_instead_of_cancelling { CapResult::Delay(delay) } else { CapResult::Cancel { - reason: format!("no path avoiding caps: {}", path.get_req()), + reason: err.to_string(), } } }