def visualize(pathFinder, source=False, target=False):
g, weight = buildWeightedGraph(pathFinder)
ecolor = g.new_edge_property("string")
ewidth = g.new_edge_property("double")
ewidth.a = 1
touch_v = g.new_vertex_property("bool")
touch_e = g.new_edge_property("bool")
if not source:
source = pathFinder.source
if not target:
target = pathFinder.target
dist, pred = gt.astar_search(g, source, weight,
VisitorExample(touch_v, touch_e, target),
heuristic=lambda v: pathFinder.jaccard(v, target))
for e in g.edges():
ecolor[e] = "blue" if touch_e[e] else "black"
v = target
while v != source:
p = g.vertex(pred[v])
for e in v.out_edges():
if e.target() == p:
ecolor[e] = "#a40000"
ewidth[e] = 3
v = p
gt.graph_draw(g, output_size=(600, 600), vertex_fill_color=touch_v, edge_color=ecolor,
edge_pen_width=ewidth, output="astar-demo.pdf")
def path(pathFinder):
"""Computes the astar path if it exists in the given PathFinder class"""
G = pathFinder.getGraph()
target = pathFinder.target
source = pathFinder.source
try:
if pathExists(pathFinder):
G, weight = buildWeightedGraph(pathFinder)
#for vertex in G.vertices():
# print (vertex)
touch_v = G.new_vertex_property("bool")
touch_e = G.new_edge_property("bool")
dist, pred = gt.astar_search(
G,
source,
weight,
VisitorExample(touch_v, touch_e, target),
heuristic=lambda v: pathFinder.jaccard(v, target))
#print ([pred])
return [pred]
#return list(nx.all_simple_paths(G,0,1,cutoff=8))
else:
return None
except:
logger.error(sys.exc_info())
return None
def visualize(pathFinder, source=False, target=False):
g, weight = buildWeightedGraph(pathFinder)
ecolor = g.new_edge_property("string")
ewidth = g.new_edge_property("double")
ewidth.a = 1
touch_v = g.new_vertex_property("bool")
touch_e = g.new_edge_property("bool")
if not source:
source = pathFinder.source
if not target:
target = pathFinder.target
dist, pred = gt.astar_search(
g,
source,
weight,
VisitorExample(touch_v, touch_e, target),
heuristic=lambda v: pathFinder.jaccard(v, target))
for e in g.edges():
ecolor[e] = "blue" if touch_e[e] else "black"
v = target
while v != source:
p = g.vertex(pred[v])
for e in v.out_edges():
if e.target() == p:
ecolor[e] = "#a40000"
ewidth[e] = 3
v = p
gt.graph_draw(g,
output_size=(600, 600),
vertex_fill_color=touch_v,
edge_color=ecolor,
edge_pen_width=ewidth,
output="astar-demo.pdf")
def get_route(self, src, tgt): # astar search
src_node = self.graph.vertex(src)
tgt_node = self.graph.vertex(tgt)
dists = self.graph.ep["weight"]
dist_map, pred_map = gt.astar_search(self.graph,
source=src_node,
weight=dists)
min_dist = np.inf
node_order = [int(src)]
if dist_map[tgt] < min_dist:
node_order.extend(
self.reconstruct_path(src_node, tgt_node, pred_map))
return node_order
def pathLength(pathFinder):
"""Checks the length of a path if it exists in the given PathFinder class"""
G = pathFinder.getGraph()
target = pathFinder.target
source = pathFinder.source
try:
if pathExists(pathFinder):
G, weight = buildWeightedGraph(pathFinder)
touch_v = G.new_vertex_property("bool")
touch_e = G.new_edge_property("bool")
dist, pred = gt.astar_search(G, source, weight,
VisitorExample(touch_v, touch_e, target),
heuristic=lambda v: pathFinder.jaccard(v, target))
return dist
else:
return None
except:
logger.error (sys.exc_info())
return None
def pathLength(pathFinder):
"""Checks the length of a path if it exists in the given PathFinder class"""
G = pathFinder.getGraph()
target = pathFinder.target
source = pathFinder.source
try:
if pathExists(pathFinder):
G, weight = buildWeightedGraph(pathFinder)
touch_v = G.new_vertex_property("bool")
touch_e = G.new_edge_property("bool")
dist, pred = gt.astar_search(
G,
source,
weight,
VisitorExample(touch_v, touch_e, target),
heuristic=lambda v: pathFinder.jaccard(v, target))
return dist
else:
return None
except:
logger.error(sys.exc_info())
return None
def astar(
G: Graph,
source: int,
target: int,
edge_attribute: EdgePropertyMap,
heuristic,
pos: np.ndarray,
) -> np.ndarray:
"""
Perform A* with given heuristic
Args:
G: graph
source: start vertex
target: end vertex (search terminates here)
edge_attribute: the edge attribute that defines the cost of an edge
heuristic: a function that underestimates the distance from any vertex to the target
pos: positional attribute for vertices
Returns: a list of vertices from the source to the target
"""
# run A*
pred = astar_search(
G,
weight=edge_attribute,
source=source,
visitor=RouteVisitor(target),
heuristic=lambda v: heuristic(v, target, pos),
)[1]
# backtrack through the graph to the source
route = []
v = target
while v != source:
if v == pred[v]:
raise Exception("The start is not connected to the target")
route.append(v)
v = G.vertex(pred[v])
route.append(v)
return route
def path(pathFinder):
"""Computes the astar path if it exists in the given PathFinder class"""
G = pathFinder.getGraph()
target = pathFinder.target
source = pathFinder.source
try:
if pathExists(pathFinder):
G, weight = buildWeightedGraph(pathFinder)
#for vertex in G.vertices():
# print (vertex)
touch_v = G.new_vertex_property("bool")
touch_e = G.new_edge_property("bool")
dist, pred = gt.astar_search(G, source, weight,
VisitorExample(touch_v, touch_e, target),
heuristic=lambda v: pathFinder.jaccard(v, target))
#print ([pred])
return [pred]
#return list(nx.all_simple_paths(G,0,1,cutoff=8))
else:
return None
except:
logger.error (sys.exc_info())
return None
def useGraphTool(pd):
# Extract the graphml representation of the planner data
graphml = pd.printGraphML()
f = open("graph.graphml", 'w')
f.write(graphml)
f.close()
# Load the graphml data using graph-tool
graph = gt.load_graph("graph.graphml", fmt="xml")
edgeweights = graph.edge_properties["weight"]
# Write some interesting statistics
avgdeg, stddevdeg = gt.vertex_average(graph, "total")
avgwt, stddevwt = gt.edge_average(graph, edgeweights)
print("---- PLANNER DATA STATISTICS ----")
print(
str(graph.num_vertices()) + " vertices and " + str(graph.num_edges()) +
" edges")
print("Average vertex degree (in+out) = " + str(avgdeg) + " St. Dev = " +
str(stddevdeg))
print("Average edge weight = " + str(avgwt) + " St. Dev = " +
str(stddevwt))
_, hist = gt.label_components(graph)
print("Strongly connected components: " + str(len(hist)))
# Make the graph undirected (for weak components, and a simpler drawing)
graph.set_directed(False)
_, hist = gt.label_components(graph)
print("Weakly connected components: " + str(len(hist)))
# Plotting the graph
gt.remove_parallel_edges(graph) # Removing any superfluous edges
edgeweights = graph.edge_properties["weight"]
colorprops = graph.new_vertex_property("string")
vertexsize = graph.new_vertex_property("double")
start = -1
goal = -1
for v in range(graph.num_vertices()):
# Color and size vertices by type: start, goal, other
if pd.isStartVertex(v):
start = v
colorprops[graph.vertex(v)] = "cyan"
vertexsize[graph.vertex(v)] = 10
elif pd.isGoalVertex(v):
goal = v
colorprops[graph.vertex(v)] = "green"
vertexsize[graph.vertex(v)] = 10
else:
colorprops[graph.vertex(v)] = "yellow"
vertexsize[graph.vertex(v)] = 5
# default edge color is black with size 0.5:
edgecolor = graph.new_edge_property("string")
edgesize = graph.new_edge_property("double")
for e in graph.edges():
edgecolor[e] = "black"
edgesize[e] = 0.5
# using A* to find shortest path in planner data
if start != -1 and goal != -1:
_, pred = gt.astar_search(graph, graph.vertex(start), edgeweights)
# Color edges along shortest path red with size 3.0
v = graph.vertex(goal)
while v != graph.vertex(start):
p = graph.vertex(pred[v])
for e in p.out_edges():
if e.target() == v:
edgecolor[e] = "red"
edgesize[e] = 2.0
v = p
pos = graph.new_vertex_property("vector<double>")
for v in range(graph.num_vertices()):
vtx = pd.getVertex(v)
st = vtx.getState()
pos[graph.vertex(v)] = [st[0], st[1]]
# Writing graph to file:
# pos indicates the desired vertex positions, and pin=True says that we
# really REALLY want the vertices at those positions
# gt.graph_draw(graph, pos=pos, vertex_size=vertexsize, vertex_fill_color=colorprops,
# edge_pen_width=edgesize, edge_color=edgecolor,
# output="graph.pdf")
gt.graph_draw(graph, pos=pos, output="graph.pdf")
print('\nGraph written to graph.pdf')
graph.vertex_properties["pos"] = pos
graph.vertex_properties["vsize"] = vertexsize
graph.vertex_properties["vcolor"] = colorprops
graph.edge_properties["esize"] = edgesize
graph.edge_properties["ecolor"] = edgecolor
graph.save("mgraph.graphml")
print('\nGraph saved to mgraph.graphml')
def useGraphTool(pd, space):
# Extract the graphml representation of the planner data
graphml = pd.printGraphML()
f = open("graph.xml", 'w')
f.write(graphml)
f.close()
# Load the graphml data using graph-tool
graph = gt.load_graph("graph.xml")
edgeweights = graph.edge_properties["weight"]
# Write some interesting statistics
avgdeg, stddevdeg = gt.vertex_average(graph, "total")
avgwt, stddevwt = gt.edge_average(graph, edgeweights)
print "---- PLANNER DATA STATISTICS ----"
print str(graph.num_vertices()) + " vertices and " + str(graph.num_edges()) + " edges"
print "Average vertex degree (in+out) = " + str(avgdeg) + " St. Dev = " + str(stddevdeg)
print "Average edge weight = " + str(avgwt) + " St. Dev = " + str(stddevwt)
comps, hist = gt.label_components(graph)
print "Strongly connected components: " + str(len(hist))
graph.set_directed(False) # Make the graph undirected (for weak components, and a simpler drawing)
comps, hist = gt.label_components(graph)
print "Weakly connected components: " + str(len(hist))
# Plotting the graph
gt.remove_parallel_edges(graph) # Removing any superfluous edges
edgeweights = graph.edge_properties["weight"]
colorprops = graph.new_vertex_property("string")
vertexsize = graph.new_vertex_property("double")
start = -1
goal = -1
for v in range(graph.num_vertices()):
# Color and size vertices by type: start, goal, other
if (pd.isStartVertex(v)):
start = v
colorprops[graph.vertex(v)] = "cyan"
vertexsize[graph.vertex(v)] = 10
elif (pd.isGoalVertex(v)):
goal = v
colorprops[graph.vertex(v)] = "green"
vertexsize[graph.vertex(v)] = 10
else:
colorprops[graph.vertex(v)] = "yellow"
vertexsize[graph.vertex(v)] = 5
# default edge color is black with size 0.5:
edgecolor = graph.new_edge_property("string")
edgesize = graph.new_edge_property("double")
for e in graph.edges():
edgecolor[e] = "black"
edgesize[e] = 0.5
# using A* to find shortest path in planner data
if start != -1 and goal != -1:
dist, pred = gt.astar_search(graph, graph.vertex(start), edgeweights)
# Color edges along shortest path red with size 3.0
v = graph.vertex(goal)
while v != graph.vertex(start):
p = graph.vertex(pred[v])
for e in p.out_edges():
if e.target() == v:
edgecolor[e] = "red"
edgesize[e] = 2.0
v = p
# Writing graph to file:
# pos indicates the desired vertex positions, and pin=True says that we
# really REALLY want the vertices at those positions
gt.graph_draw (graph, vertex_size=vertexsize, vertex_fill_color=colorprops,
edge_pen_width=edgesize, edge_color=edgecolor,
output="graph.png")
print
print 'Graph written to graph.png'
def find_path(self, startLat, startLon, endLat, endLon, mode):
self.__log("Find path request! ")
if self.gr.num_vertices() == 0:
self.gr.load(self.graphFileName)
self.__log("Graph loaded")
else:
self.__log("Graph already in memory")
start = self.__getNode(startLat, startLon)
end = self.__getNode(endLat, endLon)
# start = self.__getNode(41.8880107, 12.5219164)
# end = self.__getNode(41.8887613, 12.5203503)
if start.graphid != None:
startAddedToGraph = False
startVertex = self.gr.vertex(start.graphid)
else:
startAddedToGraph = True
startVertex = self.__insert_vertex(start)
if end.graphid != None:
endAddedToGraph = False
endVertex = self.gr.vertex(end.graphid)
else:
endAddedToGraph = True
endVertex = self.__insert_vertex(end)
self.__log("Start = %d, end = %d founds" % (start.id, end.id))
if mode == "shortest":
weightPropToUse = self.gr.edge_properties['euclidean']
euclidean = True
elif mode == "less_effort":
weightPropToUse = self.gr.edge_properties['effort']
euclidean = False
else:
self.__log("Mode unknown", logTime=False)
return
_, pred = gt.astar_search(
self.gr,
startVertex,
weightPropToUse,
Visitor(endVertex),
heuristic=lambda v: self.__heuristic(v, endVertex, euclidean))
self.__log("A* terminated")
if pred[endVertex] == int(endVertex):
self.__log("Failed!!", logTime=False)
return
v = endVertex
path = [self.gr.vertex_properties['osmid'][v]]
while v != startVertex:
v = self.gr.vertex(pred[v])
path.append(self.gr.vertex_properties['osmid'][v])
path = list(reversed(path))
print path
path = completePath(path)
self.__log("Path completed")
if endAddedToGraph:
self.gr.clear_vertex(endVertex)
self.gr.remove_vertex(endVertex)
if startAddedToGraph:
self.gr.clear_vertex(startVertex)
self.gr.remove_vertex(startVertex)
return path
def find_path(self, startLat, startLon, endLat, endLon, mode):
self.__log("Find path request! ")
if self.gr.num_vertices() == 0:
self.gr.load(self.graphFileName)
self.__log("Graph loaded")
else:
self.__log("Graph already in memory")
start = self.__getNode(startLat,startLon)
end = self.__getNode(endLat,endLon)
# start = self.__getNode(41.8880107, 12.5219164)
# end = self.__getNode(41.8887613, 12.5203503)
if start.graphid != None:
startAddedToGraph = False
startVertex = self.gr.vertex(start.graphid)
else:
startAddedToGraph= True
startVertex = self.__insert_vertex(start)
if end.graphid != None:
endAddedToGraph = False
endVertex = self.gr.vertex(end.graphid)
else:
endAddedToGraph = True
endVertex = self.__insert_vertex(end)
self.__log("Start = %d, end = %d founds" %(start.id,end.id))
if mode == "shortest":
weightPropToUse = self.gr.edge_properties['euclidean']
euclidean = True
elif mode == "less_effort":
weightPropToUse = self.gr.edge_properties['effort']
euclidean = False
else:
self.__log("Mode unknown", logTime=False)
return
_, pred = gt.astar_search(self.gr, startVertex,
weightPropToUse,
Visitor(endVertex),
heuristic=lambda v: self.__heuristic(v, endVertex, euclidean))
self.__log("A* terminated")
if pred[endVertex] == int(endVertex):
self.__log("Failed!!", logTime=False)
return
v = endVertex
path = [self.gr.vertex_properties['osmid'][v]]
while v != startVertex:
v = self.gr.vertex(pred[v])
path.append(self.gr.vertex_properties['osmid'][v])
path = list(reversed(path))
print path
path = completePath(path)
self.__log("Path completed")
if endAddedToGraph:
self.gr.clear_vertex(endVertex)
self.gr.remove_vertex(endVertex)
if startAddedToGraph:
self.gr.clear_vertex(startVertex)
self.gr.remove_vertex(startVertex)
return path
if (dq < 2) and (dt < np.pi / 8):
path_exists = True
q_final = nearest.reshape(-1, 1)
if min_coord is not None:
cg.add_vertex(min_coord, min_cost)
cg.add_edge(parent_coord, min_coord, min_move_cost)
fig = plt.figure()
ax = fig.add_subplot(111)
ax = env.plot_environment(ax, [])
src = cg.get_node(q_init)
tgt = cg.get_node(q_final)
dist, pred = gt.astar_search(cg.g, src, weight=cg.g.ep["cost"])
prev_node = tgt
while prev_node != src:
q0 = cg.g.vp["pos"][prev_node][:2]
theta = cg.g.vp["pos"][prev_node][2]
r0 = 2 * np.array([np.cos(theta), np.sin(theta)])
ax.plot(*q0, "k.")
ax.plot([q0[0], q0[0] + r0[0]], [q0[1], q0[1] + r0[1]], "k-")
prev_node = cg.g.vertex(pred[prev_node])
ax.plot(np.block(cg.node_coords)[0, :], np.block(cg.node_coords)[1, :], "bo")
ax.plot(*cg.g.vp["pos"][src][:2], "ko")
ax.plot(*cg.g.vp["pos"][tgt][:2], "kx")
plt.show()
]
shapeprops[graph.vertex(v)] = "circle"
# default edge color is black with size 0.5:
edgecolor = graph.new_edge_property("string")
edgecolor2 = graph.new_edge_property("vector<float>")
edgesize = graph.new_edge_property("double")
for e in graph.edges():
edgecolor[e] = "black"
# edgecolor2[e]= [158.0/255.0, 217.0/255.0, 207.0/255.0, 1.0]
edgecolor2[e] = [110.0 / 255.0, 115.0 / 255.0, 116.0 / 255.0, 1.0]
edgesize[e] = 0.5
# using A* to find shortest path in planner data
if False and start != -1 and goal != -1:
_, pred = gt.astar_search(graph, graph.vertex(start), edgeweights)
# Color edges along shortest path red with size 3.0
v = graph.vertex(goal)
while v != graph.vertex(start):
p = graph.vertex(pred[v])
for e in p.out_edges():
if e.target() == v:
edgecolor[e] = "red"
edgesize[e] = 2.0
v = p
pos = graph.new_vertex_property("vector<double>")
for v in range(graph.num_vertices()):
vtx = pd.getVertex(v)
st = vtx.getState()
venice_weight = g.new_edge_property("double")
# paretnza 45.43988044474121 12.339807563546461
# arrivo 45.43170127993013 12.325036058157616
#coord_source = [12.339807563546461, 45.43988044474121]
#coord_target = [12.325036058157616, 45.431701279930130]
latlon = g.vertex_properties['latlon']
# source = find_vertex(g, latlon, coord_source)
# target = find_vertex(g, latlon, coord_target)
# print(f'Source {source}')
# print(f'Target {target}')
source = g.vertex(25)
target = g.vertex(100)
dist, pred = gt.astar_search(g,
source,
venice_weight,
VeniceResident(g, touch_v, touch_e, target,
venice_weight),
heuristic=lambda v: h(v, target, pos))
# implicit=True
ecolor = g.new_edge_property("string")
ewidth = g.new_edge_property("double")
ewidth.a = 1
for e in g.edges():
ecolor[e] = "#3465a4" if touch_e[e] else "#d3d7cf"
v = target
while v != source:
p = g.vertex(pred[v])
for e in v.out_edges():
