From a6aa46dd61d1f89637044fb64745ce05cf87f8a8 Mon Sep 17 00:00:00 2001 From: Dustin Carlino Date: Mon, 17 Jun 2019 11:33:57 -0700 Subject: [PATCH] cutover driving pathfinding to fastpaths, using a full lane graph for driving instead of falling back when path stitching fails. handle edits by preparing the CH again, using the existing node ordering. one-time prep time is reasonable, recalculating works for live edit mode, and queries are FAST! :O --- map_model/src/map.rs | 3 +- map_model/src/pathfind/driving.rs | 225 +++++++++--------------------- map_model/src/pathfind/mod.rs | 43 +++--- map_model/src/pathfind/slow.rs | 128 ----------------- 4 files changed, 84 insertions(+), 315 deletions(-) delete mode 100644 map_model/src/pathfind/slow.rs diff --git a/map_model/src/map.rs b/map_model/src/map.rs index f4eedc6494..d8fcb3ba54 100644 --- a/map_model/src/map.rs +++ b/map_model/src/map.rs @@ -697,8 +697,9 @@ impl Map { } } + // Do this last, so all the changes are visible in the map. let mut pathfinder = self.pathfinder.take().unwrap(); - pathfinder.apply_edits(&delete_turns, &add_turns, self, timer); + pathfinder.apply_edits(self, timer); self.pathfinder = Some(pathfinder); self.edits = new_edits; diff --git a/map_model/src/pathfind/driving.rs b/map_model/src/pathfind/driving.rs index 749f31faa3..8a40a1b0f8 100644 --- a/map_model/src/pathfind/driving.rs +++ b/map_model/src/pathfind/driving.rs @@ -1,182 +1,87 @@ -use crate::{DirectedRoadID, LaneID, LaneType, Map, Path, PathRequest, PathStep, Turn, TurnID}; -use abstutil::{deserialize_btreemap, serialize_btreemap, Timer}; -use geom::Distance; -use petgraph::graph::NodeIndex; -use petgraph::stable_graph::StableGraph; +use crate::pathfind::node_map::{deserialize_nodemap, NodeMap}; +use crate::{LaneID, LaneType, Map, Path, PathRequest, PathStep, TurnID}; +use fast_paths::{FastGraph, InputGraph}; use serde_derive::{Deserialize, Serialize}; -use std::collections::{BTreeMap, BTreeSet, VecDeque}; -// TODO Make the graph smaller by considering RoadID, or even (directed?) bundles of roads based on -// OSM way. #[derive(Serialize, Deserialize)] pub struct VehiclePathfinder { - graph: StableGraph, - #[serde( - serialize_with = "serialize_btreemap", - deserialize_with = "deserialize_btreemap" - )] - nodes: BTreeMap>, + graph: FastGraph, + #[serde(deserialize_with = "deserialize_nodemap")] + nodes: NodeMap, lane_types: Vec, } -pub enum Outcome { - Success(Path), - Failure, - RetrySlow, -} - impl VehiclePathfinder { pub fn new(map: &Map, lane_types: Vec) -> VehiclePathfinder { - let mut g = VehiclePathfinder { - graph: StableGraph::new(), - nodes: BTreeMap::new(), + let mut input_graph = InputGraph::new(); + let mut nodes = NodeMap::new(); + + for l in map.all_lanes() { + // Insert every lane as a node. Even if the lane type is wrong now, it might change + // later, and we want the node in the graph. + let from = nodes.get_or_insert(l.id); + + for (turn, next) in map.get_next_turns_and_lanes(l.id, l.dst_i).into_iter() { + if !map.is_turn_allowed(turn.id) || !lane_types.contains(&next.lane_type) { + continue; + } + // TODO Speed limit or some other cost + let length = l.length() + turn.geom.length(); + let length_cm = (length.inner_meters() * 100.0).round() as usize; + input_graph.add_edge(from, nodes.get_or_insert(next.id), length_cm); + } + } + input_graph.freeze(); + let graph = fast_paths::prepare(&input_graph); + + VehiclePathfinder { + graph, + nodes, lane_types, - }; - - for r in map.all_roads() { - // Could omit if there aren't any matching lane types, but since those can be edited, - // it's actually a bit simpler to just have all nodes. - if !r.children_forwards.is_empty() { - let id = r.id.forwards(); - g.nodes.insert(id, g.graph.add_node(id)); - } - if !r.children_backwards.is_empty() { - let id = r.id.backwards(); - g.nodes.insert(id, g.graph.add_node(id)); - } - } - - for t in map.all_turns().values() { - g.add_turn(t, map); - } - - /*println!( - "{} nodes, {} edges", - g.graph.node_count(), - g.graph.edge_count() - );*/ - - g - } - - fn add_turn(&mut self, t: &Turn, map: &Map) { - if !map.is_turn_allowed(t.id) { - return; - } - let src_l = map.get_l(t.id.src); - let dst_l = map.get_l(t.id.dst); - if self.lane_types.contains(&src_l.lane_type) && self.lane_types.contains(&dst_l.lane_type) - { - let src = self.get_node(t.id.src, map); - let dst = self.get_node(t.id.dst, map); - // First length arbitrarily wins. - if self.graph.find_edge(src, dst).is_none() { - self.graph - .add_edge(src, dst, src_l.length() + t.geom.length()); - } } } - fn get_node(&self, lane: LaneID, map: &Map) -> NodeIndex { - self.nodes[&map.get_l(lane).get_directed_parent(map)] - } - - pub fn pathfind(&self, req: &PathRequest, map: &Map) -> Outcome { + pub fn pathfind(&self, req: &PathRequest, map: &Map) -> Option { assert!(!map.get_l(req.start.lane()).is_sidewalk()); - - let start_node = self.get_node(req.start.lane(), map); - let end_node = self.get_node(req.end.lane(), map); - let end_pt = map.get_l(req.end.lane()).first_pt(); - - let raw_nodes = match petgraph::algo::astar( + let raw_path = fast_paths::calc_path( &self.graph, - start_node, - |n| n == end_node, - |e| *e.weight(), - |n| { - let dr = self.graph[n]; - let r = map.get_r(dr.id); - if dr.forwards { - end_pt.dist_to(r.center_pts.last_pt()) - } else { - end_pt.dist_to(r.center_pts.first_pt()) - } - }, - ) { - Some((_, nodes)) => nodes, - None => { - return Outcome::Failure; - } - }; - - // TODO windows(2) would be fine for peeking, except it drops the last element for odd - // cardinality - let mut nodes = VecDeque::from(raw_nodes); - - let mut steps: Vec = Vec::new(); - while !nodes.is_empty() { - let n = nodes.pop_front().unwrap(); - let dr = self.graph[n]; - if steps.is_empty() { - steps.push(PathStep::Lane(req.start.lane())); - } else { - let from_lane = match steps.last() { - Some(PathStep::Lane(l)) => *l, - _ => unreachable!(), - }; - if let Some(turn) = map.get_turns_from_lane(from_lane).into_iter().find(|t| { - // Special case the last step - if nodes.is_empty() { - t.id.dst == req.end.lane() - } else { - let l = map.get_l(t.id.dst); - if l.get_directed_parent(map) == dr { - // TODO different case when nodes.len() == 1. - map.get_turns_from_lane(l.id).into_iter().any(|t2| { - map.get_l(t2.id.dst).get_directed_parent(map) - == self.graph[nodes[0]] - }) - } else { - false - } - } - }) { - steps.push(PathStep::Turn(turn.id)); - steps.push(PathStep::Lane(turn.id.dst)); - } else { - if steps.len() == 1 { - // Started in the wrong lane - return Outcome::RetrySlow; - } else { - // Need more lookahead to stitch together the right path - return Outcome::RetrySlow; - } - } - } + self.nodes.get(req.start.lane()), + self.nodes.get(req.end.lane()), + )?; + let mut steps = Vec::new(); + for pair in self.nodes.translate(&raw_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], + })); } - Outcome::Success(Path::new(map, steps, req.end.dist_along())) + steps.push(PathStep::Lane(req.end.lane())); + Some(Path::new(map, steps, req.end.dist_along())) } - pub fn apply_edits( - &mut self, - delete_turns: &BTreeSet, - add_turns: &BTreeSet, - map: &Map, - _timer: &mut Timer, - ) { - // Most turns will be in both lists. That's fine -- we want to re-add the same turn and - // check if the lane type is different. - for t in delete_turns { - if let Some(e) = self - .graph - .find_edge(self.get_node(t.src, map), self.get_node(t.dst, map)) - { - self.graph.remove_edge(e); + pub fn apply_edits(&mut self, map: &Map) { + // The NodeMap is just all lanes -- it won't change. So we can also reuse the node + // ordering. + // TODO Make sure the result of this is deterministic and equivalent to computing from + // scratch. + let mut input_graph = InputGraph::new(); + + for l in map.all_lanes() { + for (turn, next) in map.get_next_turns_and_lanes(l.id, l.dst_i).into_iter() { + if !map.is_turn_allowed(turn.id) || !self.lane_types.contains(&next.lane_type) { + continue; + } + // TODO Speed limit or some other cost + let length = l.length() + turn.geom.length(); + let length_cm = (length.inner_meters() * 100.0).round() as usize; + input_graph.add_edge(self.nodes.get(l.id), self.nodes.get(next.id), length_cm); } } - - for t in add_turns { - self.add_turn(map.get_t(*t), map); - } + input_graph.freeze(); + let node_ordering = self.graph.get_node_ordering(); + self.graph = fast_paths::prepare_with_order(&input_graph, &node_ordering).unwrap(); } } diff --git a/map_model/src/pathfind/mod.rs b/map_model/src/pathfind/mod.rs index 53dd47a519..51f09a2788 100644 --- a/map_model/src/pathfind/mod.rs +++ b/map_model/src/pathfind/mod.rs @@ -1,15 +1,14 @@ mod driving; mod node_map; -mod slow; mod walking; -use self::driving::{Outcome, VehiclePathfinder}; +use self::driving::VehiclePathfinder; use self::walking::SidewalkPathfinder; use crate::{BusRouteID, BusStopID, LaneID, LaneType, Map, Position, Traversable, TurnID}; use abstutil::Timer; use geom::{Distance, PolyLine}; use serde_derive::{Deserialize, Serialize}; -use std::collections::{BTreeSet, VecDeque}; +use std::collections::VecDeque; use std::fmt; #[derive(Debug, Clone, Copy, Serialize, Deserialize, PartialEq, Eq, Hash, PartialOrd, Ord)] @@ -367,22 +366,14 @@ impl Pathfinder { )); } - let outcome = if map.get_l(req.start.lane()).is_sidewalk() { - match self.walking_graph.pathfind(&req, map) { - Some(path) => Outcome::Success(path), - None => Outcome::Failure, - } + if map.get_l(req.start.lane()).is_sidewalk() { + self.walking_graph.pathfind(&req, map) } else if req.can_use_bus_lanes { self.bus_graph.pathfind(&req, map) } else if req.can_use_bike_lanes { self.bike_graph.pathfind(&req, map) } else { self.car_graph.pathfind(&req, map) - }; - match outcome { - Outcome::Success(path) => Some(path), - Outcome::Failure => None, - Outcome::RetrySlow => self::slow::shortest_distance(map, req), } } @@ -398,19 +389,19 @@ impl Pathfinder { .should_use_transit(map, start, end) } - pub fn apply_edits( - &mut self, - delete_turns: &BTreeSet, - add_turns: &BTreeSet, - map: &Map, - timer: &mut Timer, - ) { - self.car_graph - .apply_edits(delete_turns, add_turns, map, timer); - self.bike_graph - .apply_edits(delete_turns, add_turns, map, timer); - self.bus_graph - .apply_edits(delete_turns, add_turns, map, timer); + pub fn apply_edits(&mut self, map: &Map, timer: &mut Timer) { + timer.start("apply edits to car pathfinding"); + self.car_graph.apply_edits(map); + timer.stop("apply edits to car pathfinding"); + + timer.start("apply edits to bike pathfinding"); + self.bike_graph.apply_edits(map); + timer.stop("apply edits to bike pathfinding"); + + timer.start("apply edits to bus pathfinding"); + self.bus_graph.apply_edits(map); + timer.stop("apply edits to bus pathfinding"); + // TODO Can edits ever affect walking or walking+transit? If a crosswalk is entirely // banned, then yes... but actually that sounds like a bad edit to allow. } diff --git a/map_model/src/pathfind/slow.rs b/map_model/src/pathfind/slow.rs deleted file mode 100644 index 14006edb41..0000000000 --- a/map_model/src/pathfind/slow.rs +++ /dev/null @@ -1,128 +0,0 @@ -use crate::{LaneType, Map, Path, PathRequest, PathStep, Position, Traversable}; -use geom::{Distance, Pt2D}; -use ordered_float::NotNan; -use std::collections::{BinaryHeap, HashMap}; - -// Only for vehicle paths, no walking support. -pub fn shortest_distance(map: &Map, req: PathRequest) -> Option { - // TODO using first_pt here and in heuristic_dist is particularly bad for walking - // directions - let goal_pt = req.end.pt(map); - let steps = SlowPathfinder { - goal_pt, - can_use_bike_lanes: req.can_use_bike_lanes, - can_use_bus_lanes: req.can_use_bus_lanes, - } - .pathfind(map, req.start, req.end)?; - assert_eq!( - steps[0].as_traversable(), - Traversable::Lane(req.start.lane()) - ); - assert_eq!( - steps.last().unwrap().as_traversable(), - Traversable::Lane(req.end.lane()) - ); - Some(Path::new(map, steps, req.end.dist_along())) -} - -struct SlowPathfinder { - goal_pt: Pt2D, - can_use_bike_lanes: bool, - can_use_bus_lanes: bool, -} - -impl SlowPathfinder { - fn expand(&self, map: &Map, current: PathStep) -> Vec { - let mut results: Vec = Vec::new(); - match current { - PathStep::Lane(l) => { - for (turn, next) in map - .get_next_turns_and_lanes(l, map.get_l(l).dst_i) - .into_iter() - { - if !map.is_turn_allowed(turn.id) { - // Skip - } else if !self.can_use_bike_lanes && next.lane_type == LaneType::Biking { - // Skip - } else if !self.can_use_bus_lanes && next.lane_type == LaneType::Bus { - // Skip - } else { - results.push(PathStep::Turn(turn.id)); - } - } - } - PathStep::Turn(t) => { - results.push(PathStep::Lane(t.dst)); - } - PathStep::ContraflowLane(_) => unreachable!(), - }; - results - } - - fn pathfind(&self, map: &Map, start: Position, end: Position) -> Option> { - // This should be deterministic, since cost ties would be broken by PathStep. - let mut queue: BinaryHeap<(NotNan, PathStep)> = BinaryHeap::new(); - { - let step = PathStep::Lane(start.lane()); - let cost = map.get_l(start.lane()).length() - start.dist_along(); - let heuristic = heuristic(&step, self.goal_pt, map); - queue.push((dist_to_pri_queue(cost + heuristic), step)); - } - - let mut backrefs: HashMap = HashMap::new(); - - while !queue.is_empty() { - let (cost_sofar, current) = queue.pop().unwrap(); - - // Found it, now produce the path - if current == PathStep::Lane(end.lane()) { - let mut reversed_steps: Vec = Vec::new(); - let mut lookup = current; - loop { - reversed_steps.push(lookup); - if lookup == PathStep::Lane(start.lane()) { - reversed_steps.reverse(); - return Some(reversed_steps); - } - lookup = backrefs[&lookup]; - } - } - - // Expand - for next in self.expand(map, current).into_iter() { - backrefs.entry(next).or_insert_with(|| { - let cost = cost(&next, map); - let heuristic = heuristic(&next, self.goal_pt, map); - queue.push((dist_to_pri_queue(cost + heuristic) + cost_sofar, next)); - - current - }); - } - } - - // No path - None - } -} - -// Negate since BinaryHeap is a max-heap. -fn dist_to_pri_queue(dist: Distance) -> NotNan { - NotNan::new(-dist.inner_meters()).unwrap() -} - -fn cost(step: &PathStep, map: &Map) -> Distance { - match step { - PathStep::Lane(l) => map.get_l(*l).length(), - PathStep::Turn(t) => map.get_t(*t).geom.length(), - PathStep::ContraflowLane(_) => unreachable!(), - } -} - -fn heuristic(step: &PathStep, goal_pt: Pt2D, map: &Map) -> Distance { - let pt = match step { - PathStep::Lane(l) => map.get_l(*l).last_pt(), - PathStep::Turn(t) => map.get_t(*t).geom.last_pt(), - PathStep::ContraflowLane(_) => unreachable!(), - }; - pt.dist_to(goal_pt) -}