import contracts
contracts.disable_all()
import duckietown_world as dw
from duckietown_world.svg_drawing.ipython_utils import ipython_draw_html
dw.logger.setLevel(50)
Better visualization of output
%%html
<style>
pre {line-height: 90%}
</style>
Let's load a map and compute the road network.
m = dw.load_map('robotarium1')
Use the function get_skeleton_graph
:
sk = dw.get_skeleton_graph(m)
The return type is SkeletonGraphResult
. It contains in sk.root2
a new map with the joined lane segments.
ipython_draw_html(sk.root2);
While in the attribute sk.G
we find a graph describing the topology.
This is a graph where each node is a meeting point between lanes, and each edge represents a lane.
# nodes
print(list(sk.G))
# edges
for n1, n2 in sk.G.edges():
data = sk.G.get_edge_data(n1, n2)
one_lane = data[0]['lane']
print('I can go from %s to %s using lane %s' % (n1, n2, one_lane))
Let's bring in the draw_graph
function from some time ago.
def draw_graph(G0, pos=None):
import networkx as nx
from matplotlib import pyplot as plt
pos = pos or nx.spring_layout(G0)
plt.figure(figsize=(12, 12))
nx.draw(G0,pos,labels={node:node for node in G0.nodes()})
def edge_label(a, b):
datas = G0.get_edge_data(a, b)
s = '%d edge%s' % (len(datas), 's' if len(datas)>=2 else '')
for k, v in datas.items():
if v:
if 'label' in v:
s += '\n %s' % v['label']
else:
s += '\n %s' %v
return s
edge_labels = dict([ ((a,b), edge_label(a,b)) for a,b in G0.edges()])
nx.draw_networkx_edge_labels(G0,pos,edge_labels=edge_labels,font_color='red')
plt.axis('off')
plt.show()
Set the position of each node in the graph based on the attribute 'point'.
import geometry as geo
pos = {}
for n in sk.G:
q = sk.G.nodes[n]['point'].as_SE2()
t, _ = geo.translation_angle_from_SE2(q)
pos[n] = t
draw_graph(sk.G, pos=pos)
Here is an example of how to do planning on the road network.
We select a start and end node:
start = 'P60'
end = 'P36'
We find the shortest path:
import networkx as nx
path = nx.shortest_path(sk.G, start, end)
print(path)
We retrieve the edge names:
def get_lanes(path):
edges = zip(path[:-1], path[1:])
lanes = []
for a, b in edges:
lane = sk.G.get_edge_data(a, b)[0]['lane']
lanes.append(lane)
return lanes
lanes = get_lanes(path);
print(lanes)
For visualization, we create a new map containing only the lanes selected:
po = dw.PlacedObject()
for lane_name in lanes:
lane = sk.root2.children[lane_name]
po.set_object(lane_name, lane, ground_truth=dw.SE2Transform.identity())
ipython_draw_html(po);