Setup

In [1]:
import contracts
contracts.disable_all()
In [2]:
import duckietown_world as dw
from duckietown_world.svg_drawing.ipython_utils import ipython_draw_html
INFO:dt-world:duckietown-world 1.0.15
In [3]:
dw.logger.setLevel(50)

Better visualization of output

In [4]:
%%html
<style>
pre {line-height: 90%}
</style>

Road Network

Let's load a map and compute the road network.

In [5]:
m = dw.load_map('robotarium1')

Use the function get_skeleton_graph:

In [6]:
sk = dw.get_skeleton_graph(m)

The return type is SkeletonGraphResult. It contains in sk.root2 a new map with the joined lane segments.

In [7]:
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.

In [8]:
# nodes
print(list(sk.G))
['P51', 'P83', 'P52', 'P43', 'P75', 'P48', 'P82', 'P55', 'P50', 'P53', 'P76', 'P77', 'P74', 'P54', 'P72', 'P73', 'P70', 'P71', 'P58', 'P38', 'P78', 'P79', 'P10', 'P11', 'P12', 'P13', 'P14', 'P15', 'P16', 'P17', 'P18', 'P19', 'P30', 'P31', 'P36', 'P37', 'P34', 'P35', 'P2', 'P3', 'P0', 'P1', 'P6', 'P7', 'P4', 'P5', 'P59', 'P8', 'P9', 'P32', 'P62', 'P49', 'P33', 'P68', 'P56', 'P61', 'P60', 'P63', 'P57', 'P65', 'P64', 'P67', 'P66', 'P69', 'P39', 'P81', 'P80', 'P42', 'P25', 'P24', 'P27', 'P26', 'P21', 'P20', 'P23', 'P22', 'P47', 'P46', 'P45', 'P44', 'P29', 'P28', 'P41', 'P40']
In [9]:
# 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))
I can go from P51 to P21 using lane ls229
I can go from P51 to P4 using lane ls111
I can go from P83 to P69 using lane L111
I can go from P52 to P44 using lane ls079
I can go from P52 to P72 using lane ls234
I can go from P52 to P28 using lane ls009
I can go from P43 to P6 using lane ls122
I can go from P43 to P66 using lane ls245
I can go from P75 to P0 using lane ls270
I can go from P75 to P49 using lane ls157
I can go from P48 to P2 using lane ls134
I can go from P48 to P23 using lane ls096
I can go from P82 to P63 using lane ls050
I can go from P82 to P12 using lane ls260
I can go from P55 to P61 using lane ls064
I can go from P55 to P27 using lane ls103
I can go from P55 to P38 using lane ls062
I can go from P50 to P47 using lane ls211
I can go from P50 to P46 using lane ls204
I can go from P50 to P68 using lane ls127
I can go from P53 to P15 using lane L112
I can go from P76 to P0 using lane ls135
I can go from P76 to P17 using lane ls092
I can go from P77 to P83 using lane ls106
I can go from P77 to P12 using lane ls136
I can go from P74 to P11 using lane ls177
I can go from P74 to P56 using lane ls146
I can go from P54 to P66 using lane ls272
I can go from P54 to P5 using lane ls186
I can go from P72 to P81 using lane L108
I can go from P73 to P25 using lane L124
I can go from P70 to P65 using lane ls020
I can go from P70 to P20 using lane ls173
I can go from P71 to P49 using lane ls123
I can go from P71 to P17 using lane ls023
I can go from P58 to P57 using lane ls057
I can go from P58 to P4 using lane ls052
I can go from P38 to P42 using lane L110
I can go from P78 to P51 using lane L105
I can go from P79 to P29 using lane ls161
I can go from P79 to P53 using lane ls196
I can go from P10 to P83 using lane ls164
I can go from P10 to P63 using lane ls206
I can go from P11 to P26 using lane L125
I can go from P12 to P37 using lane L93
I can go from P13 to P31 using lane ls163
I can go from P13 to P65 using lane ls199
I can go from P14 to P46 using lane ls182
I can go from P14 to P39 using lane ls039
I can go from P14 to P68 using lane ls104
I can go from P15 to P19 using lane ls053
I can go from P15 to P44 using lane ls207
I can go from P15 to P72 using lane ls074
I can go from P16 to P19 using lane ls030
I can go from P16 to P44 using lane ls254
I can go from P16 to P28 using lane ls183
I can go from P17 to P34 using lane ls093
I can go from P18 to P8 using lane ls046
I can go from P18 to P59 using lane ls162
I can go from P19 to P43 using lane L56
I can go from P30 to P6 using lane ls145
I can go from P30 to P5 using lane ls018
I can go from P31 to P55 using lane L127
I can go from P36 to P38 using lane ls067
I can go from P36 to P27 using lane ls192
I can go from P36 to P22 using lane ls222
I can go from P37 to P11 using lane ls149
I can go from P37 to P73 using lane ls017
I can go from P34 to P57 using lane ls244
I can go from P34 to P21 using lane ls257
I can go from P35 to P2 using lane ls078
I can go from P35 to P80 using lane ls166
I can go from P2 to P30 using lane L20
I can go from P3 to P45 using lane ls091
I can go from P3 to P40 using lane ls153
I can go from P0 to P14 using lane L121
I can go from P1 to P24 using lane ls099
I can go from P1 to P53 using lane ls160
I can go from P6 to P33 using lane L36
I can go from P7 to P59 using lane ls268
I can go from P7 to P78 using lane ls130
I can go from P4 to P75 using lane ls045
I can go from P5 to P52 using lane L137
I can go from P59 to P76 using lane L130
I can go from P8 to P58 using lane L126
I can go from P9 to P24 using lane ls124
I can go from P9 to P29 using lane ls233
I can go from P32 to P31 using lane ls033
I can go from P32 to P20 using lane ls131
I can go from P62 to P32 using lane L52
I can go from P49 to P64 using lane L129
I can go from P33 to P62 using lane ls165
I can go from P33 to P40 using lane ls210
I can go from P68 to P13 using lane L117
I can go from P56 to P10 using lane L35
I can go from P61 to P50 using lane L101
I can go from P60 to P80 using lane ls043
I can go from P60 to P23 using lane ls180
I can go from P63 to P79 using lane L97
I can go from P57 to P18 using lane L60
I can go from P65 to P3 using lane L99
I can go from P64 to P8 using lane ls170
I can go from P64 to P78 using lane ls269
I can go from P67 to P47 using lane ls141
I can go from P67 to P46 using lane ls085
I can go from P67 to P39 using lane ls024
I can go from P66 to P60 using lane L58
I can go from P69 to P19 using lane ls178
I can go from P69 to P72 using lane ls118
I can go from P69 to P28 using lane ls159
I can go from P39 to P36 using lane L135
I can go from P81 to P61 using lane ls110
I can go from P81 to P38 using lane ls240
I can go from P81 to P22 using lane ls061
I can go from P80 to P1 using lane L128
I can go from P42 to P56 using lane ls108
I can go from P42 to P73 using lane ls195
I can go from P25 to P47 using lane ls197
I can go from P25 to P68 using lane ls169
I can go from P25 to P39 using lane ls005
I can go from P24 to P77 using lane L134
I can go from P27 to P16 using lane L131
I can go from P26 to P61 using lane ls075
I can go from P26 to P27 using lane ls036
I can go from P26 to P22 using lane ls038
I can go from P21 to P7 using lane L123
I can go from P20 to P67 using lane L88
I can go from P23 to P41 using lane L113
I can go from P22 to P70 using lane L57
I can go from P47 to P71 using lane L82
I can go from P46 to P74 using lane L132
I can go from P45 to P54 using lane L37
I can go from P44 to P82 using lane L136
I can go from P29 to P48 using lane L91
I can go from P28 to P9 using lane L98
I can go from P41 to P45 using lane ls026
I can go from P41 to P62 using lane ls232
I can go from P40 to P35 using lane L133

Let's bring in the draw_graph function from some time ago.

In [10]:
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'.

In [11]:
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)

Planning example

Here is an example of how to do planning on the road network.

We select a start and end node:

In [12]:
start = 'P60'
end = 'P36'

We find the shortest path:

In [13]:
import networkx as nx
path = nx.shortest_path(sk.G, start, end)
print(path)
['P60', 'P23', 'P41', 'P62', 'P32', 'P20', 'P67', 'P39', 'P36']

We retrieve the edge names:

In [14]:
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
In [15]:
lanes = get_lanes(path);
print(lanes)
['ls180', 'L113', 'ls232', 'L52', 'ls131', 'L88', 'ls024', 'L135']

For visualization, we create a new map containing only the lanes selected:

In [16]:
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())
In [17]:
ipython_draw_html(po);