mirror of
https://github.com/a-b-street/abstreet.git
synced 2024-12-27 16:36:02 +03:00
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
This commit is contained in:
Dustin Carlino
parent
cdacaef1a6
commit
a6aa46dd61
@ -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;
|
||||
|
||||
@ -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<DirectedRoadID, Distance>,
|
||||
#[serde(
|
||||
serialize_with = "serialize_btreemap",
|
||||
deserialize_with = "deserialize_btreemap"
|
||||
)]
|
||||
nodes: BTreeMap<DirectedRoadID, NodeIndex<u32>>,
|
||||
graph: FastGraph,
|
||||
#[serde(deserialize_with = "deserialize_nodemap")]
|
||||
nodes: NodeMap<LaneID>,
|
||||
lane_types: Vec<LaneType>,
|
||||
}
|
||||
|
||||
pub enum Outcome {
|
||||
Success(Path),
|
||||
Failure,
|
||||
RetrySlow,
|
||||
}
|
||||
|
||||
impl VehiclePathfinder {
|
||||
pub fn new(map: &Map, lane_types: Vec<LaneType>) -> 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<u32> {
|
||||
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<Path> {
|
||||
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())
|
||||
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],
|
||||
}));
|
||||
}
|
||||
},
|
||||
) {
|
||||
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<PathStep> = 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
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<TurnID>,
|
||||
add_turns: &BTreeSet<TurnID>,
|
||||
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 t in add_turns {
|
||||
self.add_turn(map.get_t(*t), map);
|
||||
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);
|
||||
}
|
||||
}
|
||||
input_graph.freeze();
|
||||
let node_ordering = self.graph.get_node_ordering();
|
||||
self.graph = fast_paths::prepare_with_order(&input_graph, &node_ordering).unwrap();
|
||||
}
|
||||
}
|
||||
|
||||
@ -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<TurnID>,
|
||||
add_turns: &BTreeSet<TurnID>,
|
||||
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.
|
||||
}
|
||||
|
||||
@ -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<Path> {
|
||||
// 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<PathStep> {
|
||||
let mut results: Vec<PathStep> = 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<Vec<PathStep>> {
|
||||
// This should be deterministic, since cost ties would be broken by PathStep.
|
||||
let mut queue: BinaryHeap<(NotNan<f64>, 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<PathStep, PathStep> = 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<PathStep> = 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<f64> {
|
||||
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)
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user