def append_bike_backward(self, g: nx.MultiDiGraph, bss_node: NodeId, orig: NodeId, init_secs: float = 0, init_cost: float = 0):
for end_node in g.adj[bss_node]:
edge = g.adj[bss_node][end_node][0]
secs = edge['length'] / self._bike_speed + init_secs
cost = edge['length'] * (self._walking_speed / self._bike_speed) + init_cost
sort_cost = cost + self._get_bike_heuristic_cost(g, end_node, orig)
edge_status = self._edges_status_bike_backward[EdgeId(bss_node, end_node)]
if edge_status.is_permanent():
# if it's permanent is this edge has less cost?
edge_label_idx = edge_status.edge_label_index
lab = weakref.proxy(self._edge_labels_bike_backward[edge_status.edge_label_index])
if lab.end_node == end_node:
if cost > lab.cost.cost:
continue
self._adjacency_list_bike_backward.insert(new_key=sort_cost,
item=edge_label_idx)
lab.pred_idx = -1
lab.end_node = end_node
lab.cost = Cost(cost, secs, init_cost, init_secs)
lab.is_origin = False
lab.is_destination = True
else:
idx = len(self._edge_labels_bike_backward)
self._edge_labels_bike_backward.append(EdgeLabel(Cost(cost, secs, init_cost, init_secs),
sort_cost,
EdgeId(bss_node, end_node),
-1,
end_node,
is_origin=False,
is_destination=True))
self._adjacency_list_bike_backward.insert(sort_cost, idx)
self._edges_status_bike_backward[EdgeId(bss_node, end_node)] = EdgeStatus(idx).set_temporary()
def append_bike(self, g: nx.MultiDiGraph, bss_node: NodeId, init_secs: float=0, init_cost: float=0):
for end_node in g.adj[bss_node]:
edge = g.adj[bss_node][end_node][0]
secs = edge['length'] / self._bike_speed + init_secs
cost = edge['length'] * (self._walking_speed / self._bike_speed) + init_cost
# let's see if the edge has already been visited?
edge_status = self._edges_status_bike[EdgeId(bss_node, end_node)]
if not edge_status.is_unreached():
# if it's visted, is this edge had less cost?
edge_label_idx = edge_status.edge_label_index
lab = weakref.proxy(self._edge_labels_bike[edge_status.edge_label_index])
if lab.end_node == end_node:
if cost > lab.cost.cost:
continue
self._adjacency_list_bike.insert(new_key=cost,
item=edge_label_idx)
lab.pred_idx = -1
lab.end_node = end_node
lab.cost = Cost(cost, secs, init_cost, init_secs)
lab.is_origin = True
lab.is_destination = False
else:
idx = len(self._edge_labels_bike)
self._edge_labels_bike.append(EdgeLabel(Cost(cost, secs, init_cost, init_secs),
cost,
EdgeId(bss_node, end_node),
-1,
end_node,
is_origin=True))
self._adjacency_list_bike.insert(cost, idx)
self._edges_status_bike[EdgeId(bss_node, end_node)] = EdgeStatus(idx).set_temporary()
def init_backward(self,
g: nx.MultiDiGraph,
orig: NodeId,
dest: NodeId,
init_secs: float = 0,
init_cost: float = 0):
for end_node in g.adj[dest]:
edge = g.adj[dest][end_node][0]
secs = edge['length'] / self._speed + init_secs
cost = edge['length'] + init_cost
sort_cost = cost + self._get_heuristic_cost(g, end_node, orig)
idx = len(self._edge_labels_backward)
self._edge_labels_backward.append(
EdgeLabel(Cost(cost, secs, init_cost, init_secs),
sort_cost,
EdgeId(dest, end_node),
-1,
end_node,
is_origin=False,
is_destination=True))
self._adjacency_list_backward.insert(sort_cost, idx)
self._edges_status_backward[EdgeId(
dest, end_node)] = EdgeStatus(idx).set_temporary()
def expand_backward(self, g, node, pred_idx, origin):
for end_node in g.adj[node]:
edge = g.adj[node][end_node][0]
edge_status = self._get_edge_status_backward(EdgeId(
node, end_node))
if edge_status.is_permanent():
continue
pred = self._edge_labels_backward[pred_idx]
new_cost = pred.cost + Cost(edge['length'],
edge['length'] / self._speed)
edge_id = EdgeId(node, end_node)
sort_cost = new_cost.cost + self._get_heuristic_cost(
g, end_node, origin)
# the edge has been visited
if edge_status.is_temporary():
lab = weakref.proxy(
self._edge_labels_backward[edge_status.edge_label_index])
if lab.end_node == end_node:
if new_cost < lab.cost:
self._adjacency_list_backward.insert(
new_key=sort_cost,
item=edge_status.edge_label_index)
lab.pred_idx = pred_idx
lab.end_node = end_node
lab.cost = new_cost
# Hmmm, we are visiting the edge in the opposing direction of last visit
elif lab.end_node == node:
if new_cost.cost < (lab.cost.cost - edge['length']):
self._adjacency_list_backward.insert(
new_key=sort_cost,
item=edge_status.edge_label_index)
lab.edge_id = EdgeId(node, end_node)
lab.pred_idx = pred_idx
lab.end_node = end_node
lab.cost = new_cost
continue
idx = len(self._edge_labels_backward)
self._edge_labels_backward.append(
EdgeLabel(new_cost, sort_cost, edge_id, pred_idx, end_node))
self._edges_status_backward[edge_id] = EdgeStatus(
idx).set_temporary()
self._adjacency_list_backward.insert(sort_cost, idx)
def append_walking(self, g, orig, can_change_mode: bool, init_secs: float=0, init_cost: float=0):
# init origin
for end_node in g.adj[orig]:
edge = g.adj[orig][end_node][0]
secs = edge['length'] / self._walking_speed + init_secs
cost = edge['length'] + init_cost
sort_cost = cost
# let's see if the edge has already been visited?
edge_status = self._edges_status_walking[EdgeId(orig, end_node, can_change_mode)]
if not edge_status.is_unreached():
# if it's visted, is this edge had less cost?
edge_label_idx = edge_status.edge_label_index
lab = weakref.proxy(self._edge_labels_walking[edge_label_idx])
if lab.end_node == end_node:
if cost > lab.cost.cost:
continue
self._adjacency_list_walking.insert(new_key=cost,
item=edge_label_idx)
lab.pred_idx = -1
lab.end_node = end_node
lab.cost = Cost(cost, secs, init_cost, init_secs)
lab.is_origin = True
lab.is_destination = False
else:
idx = len(self._edge_labels_walking)
self._edge_labels_walking.append(EdgeLabel(Cost(cost, secs, init_cost, init_secs),
sort_cost,
EdgeId(orig, end_node),
-1,
end_node,
is_origin=True,
can_change_mode=can_change_mode))
self._adjacency_list_walking.insert(sort_cost, idx)
self._edges_status_walking[EdgeId(orig, end_node, can_change_mode)] = EdgeStatus(idx).set_temporary()
def expand_walking(self, g: nx.MultiDiGraph, node: NodeId, pred_idx: EdgeLabelIdx):
for end_node in g.adj[node]:
edge = g.adj[node][end_node][0]
pred = self._edge_labels_walking[pred_idx]
edge_id = EdgeId(node, end_node, pred.can_change_mode)
edge_status = self._edges_status_walking[edge_id]
new_cost = pred.cost + Cost(edge['length'], edge['length'] / self._walking_speed)
# the edge has been visited
if not edge_status.is_unreached():
lab = weakref.proxy(self._edge_labels_walking[edge_status.edge_label_index])
if lab.end_node == end_node:
if new_cost < lab.cost:
self._adjacency_list_walking.insert(new_key=new_cost.cost,
item=edge_status.edge_label_index)
lab.pred_idx = pred_idx
lab.end_node = end_node
lab.cost = new_cost
lab.can_change_mode = pred.can_change_mode
# Hmmm, we are visiting the edge in the opposing direction of last visit
elif lab.end_node == node:
if new_cost.cost < (lab.cost.cost - edge['length']):
self._adjacency_list_walking.insert(new_key=new_cost.cost,
item=edge_status.edge_label_index)
lab.edge_id = EdgeId(node, end_node)
lab.pred_idx = pred_idx
lab.end_node = end_node
lab.cost = new_cost
lab.can_change_mode = pred.can_change_mode
continue
idx = len(self._edge_labels_walking)
self._edge_labels_walking.append(EdgeLabel(new_cost,
new_cost.cost,
edge_id,
pred_idx,
end_node,
can_change_mode=pred.can_change_mode))
self._edges_status_walking[edge_id] = EdgeStatus(idx).set_temporary()
self._adjacency_list_walking.insert(new_cost.cost, idx)
def init_origin(self, g, orig, init_secs=0, init_cost=0):
# init origin
for end_node in g.adj[orig]:
edge = g.adj[orig][end_node][0]
secs = edge['length'] / self._speed + init_secs
cost = edge['length'] + init_cost
sort_cost = cost + self._get_heuristic_cost(
g, end_node, self._dest)
idx = len(self._edge_labels)
self._edge_labels.append(
EdgeLabel(Cost(cost, secs, init_cost, init_secs), sort_cost,
EdgeId(orig, end_node), -1, end_node, True))
self._adjacency_list.insert(sort_cost, idx)
self._edges_status[EdgeId(
orig, end_node)] = EdgeStatus(idx).set_temporary()
def expand_forward(self, g: nx.MultiDiGraph, node: NodeId, pred_idx: EdgeLabelIdx, dest: NodeId, bss_nodes: Set[NodeId]):
def _get_speed(travel_mode: TravelMode):
return (self._walking_speed, self._bike_speed)[travel_mode.value]
def _normalize_factor(travel_mode: TravelMode):
return self._walking_speed / (self._walking_speed, self._bike_speed)[travel_mode.value]
def _get_heurestic_cost(travel_mode: TravelMode, *args, **kwargs):
fun = (self._get_walking_heuristic_cost, self._get_bike_heuristic_cost)[travel_mode.value]
return fun(*args, **kwargs)
for end_node in g.adj[node]:
edge = g.adj[node][end_node][0]
pred = self._edge_labels[pred_idx]
edge_status = self._get_edge_status(EdgeId(node, end_node, mode=pred.edge_id.mode))
if node not in bss_nodes and edge_status.is_permanent():
continue
if node not in bss_nodes and not edge_status.is_permanent():
new_cost = pred.cost + Cost(edge['length'] * _normalize_factor(pred.edge_id.mode),
edge['length'] / _get_speed(pred.edge_id.mode))
edge_id = EdgeId(node, end_node, pred.edge_id.mode)
sort_cost = new_cost.cost + _get_heurestic_cost(pred.edge_id.mode, g, end_node, dest)
# the edge has been visited
if edge_status.is_temporary():
lab = weakref.proxy(self._edge_labels[edge_status.edge_label_index])
if lab.end_node == end_node:
if new_cost < lab.cost:
self._adjacency_list.insert(new_key=sort_cost,
item=edge_status.edge_label_index)
lab.edge_id = EdgeId(node, end_node, pred.edge_id.mode)
lab.pred_idx = pred_idx
lab.end_node = end_node
lab.cost = new_cost
# Hmmm, we are visiting the edge in the opposing direction of last visit
elif lab.end_node == node:
if new_cost.cost < (lab.cost.cost - edge['length'] * _normalize_factor(pred.edge_id.mode)):
self._adjacency_list.insert(new_key=sort_cost,
item=edge_status.edge_label_index)
lab.edge_id = EdgeId(node, end_node, pred.edge_id.mode)
lab.pred_idx = pred_idx
lab.end_node = end_node
lab.cost = new_cost
continue
idx = len(self._edge_labels)
self._edge_labels.append(EdgeLabel(new_cost,
sort_cost,
edge_id,
pred_idx,
end_node))
self._edges_status[edge_id] = EdgeStatus(idx).set_temporary()
self._adjacency_list.insert(sort_cost, idx)
if node in bss_nodes:
for mode in TravelMode:
edge_status = self._get_edge_status(EdgeId(node, end_node, mode=mode))
if edge_status.is_permanent():
continue
new_cost = pred.cost + Cost(edge['length'] * _normalize_factor(mode),
edge['length'] / _get_speed(mode))
edge_id = EdgeId(node, end_node, mode)
if mode == TravelMode.WALKING:
sort_cost = new_cost.cost + self._get_walking_heuristic_cost(g, end_node, dest)
else:
sort_cost = new_cost.cost + self._get_bike_heuristic_cost(g, end_node, dest)
# the edge has been visited
if edge_status.is_temporary():
lab = weakref.proxy(self._edge_labels[edge_status.edge_label_index])
if lab.end_node == end_node:
if new_cost < lab.cost:
self._adjacency_list.insert(new_key=sort_cost,
item=edge_status.edge_label_index)
# lab.edge_id = EdgeId(node, end_node, mode)
lab.pred_idx = pred_idx
lab.end_node = end_node
lab.cost = new_cost
# Hmmm, we are visiting the edge in the opposing direction of last visit
elif lab.end_node == node:
if new_cost.cost < (lab.cost.cost - edge['length'] * _normalize_factor(mode)):
self._adjacency_list.insert(new_key=sort_cost,
item=edge_status.edge_label_index)
# lab.edge_id = EdgeId(node, end_node, mode)
lab.pred_idx = pred_idx
lab.end_node = end_node
lab.cost = new_cost
continue
idx = len(self._edge_labels)
self._edge_labels.append(EdgeLabel(new_cost,
sort_cost,
edge_id,
pred_idx,
end_node))
self._edges_status[edge_id] = EdgeStatus(idx).set_temporary()
self._adjacency_list.insert(sort_cost, idx)
class MultiModalDoubleExpansionAStarOneQueue(object):
_edge_labels: List[EdgeLabel] = field(default_factory=list)
_adjacency_list: PriorityQueue = PriorityQueue()
_edges_status: Dict[EdgeId, EdgeStatus] = field(default_factory=lambda: defaultdict(EdgeStatus))
_cost_factor: float = 0
_walking_speed: float = WALKING_SPEED
_bike_speed: float = BIKE_SPEED
_threshold: float = float('inf')
_destinations: Dict[EdgeId, BestPath] = field(default_factory=dict)
_best_path: BestPath = BestPath(-1, Cost(0, 0))
@staticmethod
def _get_heuristic_cost_impl(g: nx.MultiDiGraph, start_node: NodeId, end_node: NodeId, cost_factor: float) -> float:
if cost_factor == 0 or end_node is None:
return 0
start_ll = utils.osm_to_pointll(g, start_node)
end_ll = utils.osm_to_pointll(g, end_node)
return start_ll.distance_to(end_ll) * cost_factor
def _get_walking_heuristic_cost(self, g: nx.MultiDiGraph, start_node: NodeId, end_node: NodeId) -> float:
return self._get_heuristic_cost_impl(g, start_node, end_node, self._cost_factor)
def _get_bike_heuristic_cost(self, g: nx.MultiDiGraph, start_node: NodeId, end_node: NodeId) -> float:
return self._get_heuristic_cost_impl(g, start_node, end_node, self._cost_factor) * 1.2 * (self._walking_speed / self._bike_speed)
@staticmethod
def _get_edge_status_impl(container: Dict[EdgeId, EdgeStatus], edge_id: EdgeId) -> EdgeStatus:
s = container.get(edge_id)
if not s:
return container[edge_id].set_unreached()
return s
def _get_edge_status(self, edge_id: EdgeId) -> EdgeStatus:
return self._get_edge_status_impl(self._edges_status, edge_id)
def init_origin(self, g: nx.MultiDiGraph, orig: NodeId, dest: NodeId, init_secs: float = 0, init_cost: float = 0):
for end_node in g.adj[orig]:
edge = g.adj[orig][end_node][0]
secs = edge['length'] / self._walking_speed + init_secs
cost = edge['length'] + init_cost
sort_cost = cost + self._get_walking_heuristic_cost(g, end_node, dest)
idx = len(self._edge_labels)
self._edge_labels.append(EdgeLabel(Cost(cost, secs, init_cost, init_secs),
sort_cost,
EdgeId(orig, end_node),
-1,
end_node,
is_origin=True,
is_destination=False))
self._adjacency_list.insert(sort_cost, idx)
self._edges_status[EdgeId(orig, end_node)] = EdgeStatus(idx).set_temporary()
def init_destination(self, g: nx.MultiDiGraph, dest: NodeId):
# init destination
for end_node in g.adj[dest]:
edge = g.adj[dest][end_node][0]
self._destinations[EdgeId(dest, end_node, mode=TravelMode.WALKING)] = edge['length']
def expand_forward(self, g: nx.MultiDiGraph, node: NodeId, pred_idx: EdgeLabelIdx, dest: NodeId, bss_nodes: Set[NodeId]):
def _get_speed(travel_mode: TravelMode):
return (self._walking_speed, self._bike_speed)[travel_mode.value]
def _normalize_factor(travel_mode: TravelMode):
return self._walking_speed / (self._walking_speed, self._bike_speed)[travel_mode.value]
def _get_heurestic_cost(travel_mode: TravelMode, *args, **kwargs):
fun = (self._get_walking_heuristic_cost, self._get_bike_heuristic_cost)[travel_mode.value]
return fun(*args, **kwargs)
for end_node in g.adj[node]:
edge = g.adj[node][end_node][0]
pred = self._edge_labels[pred_idx]
edge_status = self._get_edge_status(EdgeId(node, end_node, mode=pred.edge_id.mode))
if node not in bss_nodes and edge_status.is_permanent():
continue
if node not in bss_nodes and not edge_status.is_permanent():
new_cost = pred.cost + Cost(edge['length'] * _normalize_factor(pred.edge_id.mode),
edge['length'] / _get_speed(pred.edge_id.mode))
edge_id = EdgeId(node, end_node, pred.edge_id.mode)
sort_cost = new_cost.cost + _get_heurestic_cost(pred.edge_id.mode, g, end_node, dest)
# the edge has been visited
if edge_status.is_temporary():
lab = weakref.proxy(self._edge_labels[edge_status.edge_label_index])
if lab.end_node == end_node:
if new_cost < lab.cost:
self._adjacency_list.insert(new_key=sort_cost,
item=edge_status.edge_label_index)
lab.edge_id = EdgeId(node, end_node, pred.edge_id.mode)
lab.pred_idx = pred_idx
lab.end_node = end_node
lab.cost = new_cost
# Hmmm, we are visiting the edge in the opposing direction of last visit
elif lab.end_node == node:
if new_cost.cost < (lab.cost.cost - edge['length'] * _normalize_factor(pred.edge_id.mode)):
self._adjacency_list.insert(new_key=sort_cost,
item=edge_status.edge_label_index)
lab.edge_id = EdgeId(node, end_node, pred.edge_id.mode)
lab.pred_idx = pred_idx
lab.end_node = end_node
lab.cost = new_cost
continue
idx = len(self._edge_labels)
self._edge_labels.append(EdgeLabel(new_cost,
sort_cost,
edge_id,
pred_idx,
end_node))
self._edges_status[edge_id] = EdgeStatus(idx).set_temporary()
self._adjacency_list.insert(sort_cost, idx)
if node in bss_nodes:
for mode in TravelMode:
edge_status = self._get_edge_status(EdgeId(node, end_node, mode=mode))
if edge_status.is_permanent():
continue
new_cost = pred.cost + Cost(edge['length'] * _normalize_factor(mode),
edge['length'] / _get_speed(mode))
edge_id = EdgeId(node, end_node, mode)
if mode == TravelMode.WALKING:
sort_cost = new_cost.cost + self._get_walking_heuristic_cost(g, end_node, dest)
else:
sort_cost = new_cost.cost + self._get_bike_heuristic_cost(g, end_node, dest)
# the edge has been visited
if edge_status.is_temporary():
lab = weakref.proxy(self._edge_labels[edge_status.edge_label_index])
if lab.end_node == end_node:
if new_cost < lab.cost:
self._adjacency_list.insert(new_key=sort_cost,
item=edge_status.edge_label_index)
# lab.edge_id = EdgeId(node, end_node, mode)
lab.pred_idx = pred_idx
lab.end_node = end_node
lab.cost = new_cost
# Hmmm, we are visiting the edge in the opposing direction of last visit
elif lab.end_node == node:
if new_cost.cost < (lab.cost.cost - edge['length'] * _normalize_factor(mode)):
self._adjacency_list.insert(new_key=sort_cost,
item=edge_status.edge_label_index)
# lab.edge_id = EdgeId(node, end_node, mode)
lab.pred_idx = pred_idx
lab.end_node = end_node
lab.cost = new_cost
continue
idx = len(self._edge_labels)
self._edge_labels.append(EdgeLabel(new_cost,
sort_cost,
edge_id,
pred_idx,
end_node))
self._edges_status[edge_id] = EdgeStatus(idx).set_temporary()
self._adjacency_list.insert(sort_cost, idx)
def make_osm_path(self) -> Tuple[List[List[NodeId]], float]:
# build path from the last walking
second_walking = []
bike = []
first_walking = []
edge_label_idx = self._best_path.edge_label_index
edge_label = self._edge_labels[edge_label_idx]
second_walking.append(edge_label.end_node)
changed_mode = 0
old_mode = edge_label.edge_id.mode
while not edge_label.is_origin:
edge_label_idx = edge_label.pred_idx
edge_label = self._edge_labels[edge_label_idx]
if old_mode != edge_label.edge_id.mode:
changed_mode += 1
old_mode = edge_label.edge_id.mode
if changed_mode == 0:
second_walking.append(edge_label.end_node)
elif changed_mode == 1:
if not bike:
second_walking.append(edge_label.end_node)
bike.append(edge_label.end_node)
elif changed_mode == 2:
if not first_walking:
bike.append(edge_label.end_node)
first_walking.append(edge_label.end_node)
first_walking.append(edge_label.edge_id.start)
first_walking = first_walking[::-1]
bike = bike[::-1]
second_walking = second_walking[::-1]
res = []
if first_walking and bike:
res.append(first_walking)
res.append(bike)
res.append(second_walking)
return res, self._best_path.cost.secs
def get_best_path(self,
g: nx.MultiDiGraph,
orig: NodeId,
dest: NodeId,
bss_nodes: Set[NodeId],
callback: Callable = lambda *args, **kwargs: None) -> Tuple[List[List[NodeId]], float]:
self.init_origin(g, orig, dest)
self.init_destination(g, dest)
i = 0
a = 0
# begin search
while True:
if i % 10 == 0 and 0:
callback(g, orig, dest, bss_nodes, self._edges_status, self._edge_labels, str(a).zfill(4))
a += 1
i += 1
current_labels = len(self._edge_labels)
if current_labels > PriorityQueue.QUEUE_MAX_SIZE:
return []
_, pred_index = self._adjacency_list.pop()
pred_edge_label = self._edge_labels[pred_index]
# Do we touch the destination?
d = self._destinations.get(pred_edge_label.edge_id)
if d:
if self._best_path.edge_label_index is -1:
self._best_path.edge_label_index = pred_index
self._best_path.cost = pred_edge_label.cost
return self.make_osm_path()
if not pred_edge_label.is_origin:
self._edges_status[pred_edge_label.edge_id].set_permanent()
self.expand_forward(g, pred_edge_label.end_node, pred_index, dest, bss_nodes)
def expand_forward(self, g, node, pred_idx, dest):
for end_node in g.adj[node]:
edge = g.adj[node][end_node][0]
edge_status = self._get_edge_status(EdgeId(node, end_node))
if edge_status.is_permanent():
continue
pred = self._edge_labels[pred_idx]
new_cost = pred.cost + Cost(edge['length'],
edge['length'] / self._speed)
edge_id = EdgeId(node, end_node)
d = self._destinations.get(edge_id)
if d is not None:
if self._best_path.edge_label_index is -1 or new_cost < self._best_path.cost:
self._best_path.edge_label_index = edge_status.edge_label_index if edge_status.is_temporary() \
else len(self._edge_labels)
self._best_path.cost = new_cost
sort_cost = new_cost.cost + self._get_heuristic_cost(
g, end_node, self._dest)
# the edge has been visited
if edge_status.is_temporary():
lab = weakref.proxy(
self._edge_labels[edge_status.edge_label_index])
# Edge:
# 4242 ------------- 4141
# start -> end
# or
# end <- start
# the edge may have been visited either from its start or end,
# let's find out in this case which one is "cheaper"
# OK, we are visiting the edge at the same direction of last visit, nothing
# to be done
if lab.end_node == end_node:
if new_cost < lab.cost:
self._adjacency_list.insert(
new_key=sort_cost,
item=edge_status.edge_label_index)
lab.pred_idx = pred_idx
lab.end_node = end_node
lab.cost = new_cost
# Hmmm, we are visiting the edge in the opposing direction of last visit
elif lab.end_node == node:
if new_cost.cost < (lab.cost.cost - edge['length']):
self._adjacency_list.insert(
new_key=sort_cost,
item=edge_status.edge_label_index)
lab.edge_id = EdgeId(node, end_node)
lab.pred_idx = pred_idx
lab.end_node = end_node
lab.cost = new_cost
continue
idx = len(self._edge_labels)
self._edge_labels.append(
EdgeLabel(new_cost, sort_cost, edge_id, pred_idx, end_node))
self._edges_status[edge_id] = EdgeStatus(idx).set_temporary()
self._adjacency_list.insert(sort_cost, idx)
def init(self):
self._edge_labels = []
self._destinations = defaultdict(Cost)
self._adjacency_list = PriorityQueue()
self._edges_status = defaultdict(EdgeStatus)
self._best_path = BestPath(-1, Cost(0, 0))
class AStar(object):
_edge_labels: List[EdgeLabel]
_destinations: Dict[EdgeId, Cost]
_adjacency_list: PriorityQueue
_edges_status: Dict[EdgeId, EdgeStatus]
_orig: NodeId = -1
_dest: NodeId = -1
_cost_factor: float = 0.4
_best_path: BestPath = BestPath(-1, Cost(0, 0))
_speed: float = 1.4
def init(self):
self._edge_labels = []
self._destinations = defaultdict(Cost)
self._adjacency_list = PriorityQueue()
self._edges_status = defaultdict(EdgeStatus)
self._best_path = BestPath(-1, Cost(0, 0))
def __init__(self, speed=1.4, cost_factor=0.4):
self.init()
self._speed = speed
self._cost_factor = cost_factor
def _get_edge_cost(self, label):
return self._edge_labels[label]
def _get_edge_status(self, edge_id: EdgeId) -> EdgeStatus:
s = self._edges_status.get(edge_id)
if not s:
return self._edges_status[edge_id].set_unreached()
return s
def _get_heuristic_cost(self, g: nx.MultiDiGraph, start_node: NodeId,
end_node: NodeId) -> float:
if self._cost_factor == 0 or end_node is None:
return 0
start_ll = osm_to_pointll(g, start_node)
end_ll = osm_to_pointll(g, end_node)
return start_ll.distance_to(end_ll) * self._cost_factor
def expand_forward(self, g, node, pred_idx, dest):
for end_node in g.adj[node]:
edge = g.adj[node][end_node][0]
edge_status = self._get_edge_status(EdgeId(node, end_node))
if edge_status.is_permanent():
continue
pred = self._edge_labels[pred_idx]
new_cost = pred.cost + Cost(edge['length'],
edge['length'] / self._speed)
edge_id = EdgeId(node, end_node)
d = self._destinations.get(edge_id)
if d is not None:
if self._best_path.edge_label_index is -1 or new_cost < self._best_path.cost:
self._best_path.edge_label_index = edge_status.edge_label_index if edge_status.is_temporary() \
else len(self._edge_labels)
self._best_path.cost = new_cost
sort_cost = new_cost.cost + self._get_heuristic_cost(
g, end_node, self._dest)
# the edge has been visited
if edge_status.is_temporary():
lab = weakref.proxy(
self._edge_labels[edge_status.edge_label_index])
# Edge:
# 4242 ------------- 4141
# start -> end
# or
# end <- start
# the edge may have been visited either from its start or end,
# let's find out in this case which one is "cheaper"
# OK, we are visiting the edge at the same direction of last visit, nothing
# to be done
if lab.end_node == end_node:
if new_cost < lab.cost:
self._adjacency_list.insert(
new_key=sort_cost,
item=edge_status.edge_label_index)
lab.pred_idx = pred_idx
lab.end_node = end_node
lab.cost = new_cost
# Hmmm, we are visiting the edge in the opposing direction of last visit
elif lab.end_node == node:
if new_cost.cost < (lab.cost.cost - edge['length']):
self._adjacency_list.insert(
new_key=sort_cost,
item=edge_status.edge_label_index)
lab.edge_id = EdgeId(node, end_node)
lab.pred_idx = pred_idx
lab.end_node = end_node
lab.cost = new_cost
continue
idx = len(self._edge_labels)
self._edge_labels.append(
EdgeLabel(new_cost, sort_cost, edge_id, pred_idx, end_node))
self._edges_status[edge_id] = EdgeStatus(idx).set_temporary()
self._adjacency_list.insert(sort_cost, idx)
def make_osm_path(self):
res = []
edge_label_idx = self._best_path.edge_label_index
edge_label = self._edge_labels[edge_label_idx]
res.append(edge_label.end_node)
while not edge_label.is_origin:
edge_label_idx = edge_label.pred_idx
edge_label = self._edge_labels[edge_label_idx]
res.append(edge_label.end_node)
res.append(edge_label.edge_id.start)
res = res[::-1]
return res, self._best_path.cost.secs
def init_origin(self, g, orig, init_secs=0, init_cost=0):
# init origin
for end_node in g.adj[orig]:
edge = g.adj[orig][end_node][0]
secs = edge['length'] / self._speed + init_secs
cost = edge['length'] + init_cost
sort_cost = cost + self._get_heuristic_cost(
g, end_node, self._dest)
idx = len(self._edge_labels)
self._edge_labels.append(
EdgeLabel(Cost(cost, secs, init_cost, init_secs), sort_cost,
EdgeId(orig, end_node), -1, end_node, True))
self._adjacency_list.insert(sort_cost, idx)
self._edges_status[EdgeId(
orig, end_node)] = EdgeStatus(idx).set_temporary()
def get_best_path(
self,
g: nx.MultiDiGraph,
orig: NodeId,
dest: NodeId,
callback: Callable = lambda *args, **kwargs: None
) -> Tuple[List[NodeId], float]:
self._orig = orig
self._dest = dest
self.init()
self.init_origin(g, orig)
# init destination
for end_node in g.adj[dest]:
edge = g.adj[dest][end_node][0]
self._destinations[EdgeId(dest, end_node)] = edge['length']
i = 0
a = 0
# begin search
while True:
if i % 200 == 0:
callback(g, orig, dest, self._edges_status, self._edge_labels,
str(a).zfill(4))
a += 1
i += 1
current_labels = len(self._edge_labels)
if current_labels > PriorityQueue.QUEUE_MAX_SIZE:
return []
_, pred_index = self._adjacency_list.pop()
pred_edge_label = self._edge_labels[pred_index]
# Do we touch the destination?
d = self._destinations.get(pred_edge_label.edge_id)
if d:
if self._best_path.edge_label_index is -1:
self._best_path.edge_label_index = pred_index
self._best_path.cost = pred_edge_label.cost
return self.make_osm_path()
if not pred_edge_label.is_origin:
self._edges_status[pred_edge_label.edge_id].set_permanent()
self.expand_forward(g, pred_edge_label.end_node, pred_index, dest)