tourel beacon
[aversive.git] / projects / microb2010 / tests / tourel_beacon / graph.py
1 import sys, re, math
2 import numpy as np
3 import matplotlib
4 import matplotlib.path as mpath
5 import matplotlib.patches as mpatches
6 import matplotlib.pyplot as plt
7 from matplotlib.patches import Arrow, Circle, Wedge, Polygon
8 from matplotlib.collections import PatchCollection
9 from numpy.random import randn
10 import pylab
11 import popen2, random
12
13 Path = mpath.Path
14 FLOAT = "([-+]?[0-9]*\.?[0-9]+)"
15 INT = "([-+]?[0-9][0-9]*)"
16 RANDOM_ERROR_A = 0.5 # deg
17 RANDOM_ERROR_D = 0.5 # percent
18 beacons = [ (0.0, 1050.0), (3000.0, 0.0), (3000.0, 2100.0) ]
19
20 def build_poly(ptlist):
21     polydata = []
22     polydata.append((Path.MOVETO, (ptlist[0])))
23     for pt in ptlist[1:]:
24         polydata.append((Path.LINETO, (pt)))
25     polydata.append((Path.CLOSEPOLY, (ptlist[0])))
26     codes, verts = zip(*polydata)
27     poly = mpath.Path(verts, codes)
28     x, y = zip(*poly.vertices)
29     return x,y
30
31 def build_path(ptlist):
32     polydata = []
33     polydata.append((Path.MOVETO, (ptlist[0])))
34     for pt in ptlist[1:]:
35         polydata.append((Path.LINETO, (pt)))
36     codes, verts = zip(*polydata)
37     poly = mpath.Path(verts, codes)
38     x, y = zip(*poly.vertices)
39     return x,y
40
41 def get_angle(ref, b):
42     """get angle from robot point of view (ref) of beacon 'b'"""
43     a = math.atan2(b[1]-ref[1], b[0]-ref[0])
44     ea = (math.pi/180.) * RANDOM_ERROR_A * random.random()
45     ea = random.choice([ea, -ea])
46     return a + ea, ea
47
48 def get_distance(ref, b):
49     """get distance between robot (ref) and beacon 'b'"""
50     d = math.sqrt((b[1]-ref[1])**2 + (b[0]-ref[0])**2)
51     ed = RANDOM_ERROR_D * random.random()
52     ed = random.choice([ed, -ed])
53     return d + (d * ed / 100.), ed
54
55 def dist(p1, p2):
56     return math.sqrt((p1[0]-p2[0])**2+(p1[1]-p2[1])**2)
57
58 # graph position from distance + angle
59 def graph_da(filename, real_x, real_y, real_a):
60     pcol = []
61
62     print "real_pt = %s"%(str((real_x, real_y, real_a)))
63     real_pt = (real_x, real_y)
64
65     # display beacons
66     patches = []
67     for b in beacons:
68         patches += [ Circle((b[0], b[1]), 40, alpha=0.4) ]
69     pcol.append(PatchCollection(patches, facecolor="yellow", alpha = 1))
70
71     patches = [ Circle((real_x, real_y), 20, alpha=0.4, facecolor="red") ]
72     pcol.append(PatchCollection(patches, facecolor="red", alpha = 1))
73
74     # process angles from robot position
75     a0,ea0 = get_angle((real_x, real_y), beacons[0])
76     a1,ea1 = get_angle((real_x, real_y), beacons[1])
77     a0 -=  real_a
78     a1 -=  real_a
79     text =  "real_pt = %2.2f, %2.2f, %2.2f\n"%(real_x, real_y, real_a)
80     text += "a0 = %2.2f (%+2.2f deg)\n"%(a0, ea0*(180./math.pi))
81     text += "a1 = %2.2f (%+2.2f deg)\n"%(a1, ea1*(180./math.pi))
82     d0,ed0 = get_distance((real_x, real_y), beacons[0])
83     d1,ed1 = get_distance((real_x, real_y), beacons[1])
84     text += "d0 = %2.2f (%+2.2f %%)\n"%(d0, ed0)
85     text += "d1 = %2.2f (%+2.2f %%)\n"%(d1, ed1)
86
87     cmd = "./main ad2pos %f %f %f %f"%(a0, a1, d0, d1)
88     print cmd
89     o,i = popen2.popen2(cmd)
90     i.close()
91     s = o.read(1000000)
92     o.close()
93
94     open(filename + ".txt", "w").write(s)
95
96     if len(s) == 1000000:
97         gloupix()
98
99     fig = plt.figure()
100     ax = fig.add_subplot(111)
101     title = "Erreur de position en mm, qd l'erreur "
102     title += "d'angle aleatoire est comprise\n"
103     title += "erreur -%1.1f et %1.1f deg "%(RANDOM_ERROR_A, RANDOM_ERROR_A)
104     title += "et de distance de +/- %2.2f %%"%(RANDOM_ERROR_D)
105     ax.set_title(title)
106
107     # area
108     x,y = build_poly([(0,0), (3000,0), (3000,2100), (0,2100)])
109     ax.plot(x, y, 'g-')
110
111     result_pt = (-1, -1)
112     result_x, result_y, result_a = -1, -1, -1
113     patches = []
114     for l in s.split("\n"):
115         m = re.match("circle: x=%s y=%s r=%s"%(FLOAT, FLOAT, FLOAT), l)
116         if m:
117             x,y,r = (float(m.groups()[0]), float(m.groups()[1]), float(m.groups()[2]))
118             print x,y,r
119             patches += [ Circle((x, y), r, facecolor="none") ]
120         m = re.match("p%s: x=%s y=%s a=%s"%(INT, FLOAT, FLOAT, FLOAT), l)
121         if m:
122             n,x,y,a = (float(m.groups()[0]), float(m.groups()[1]),
123                         float(m.groups()[2]), float(m.groups()[3]))
124             if (n == 0):
125                 patches += [ Circle((x, y), 20, alpha=0.4, facecolor="yellow") ]
126                 result_x, result_y = (x, y)
127                 result_pt = (x,y)
128                 result_a = a
129             text += l + "\n"
130
131     pcol.append(PatchCollection(patches, facecolor="none", alpha = 0.6))
132     pcol.append(PatchCollection(patches, facecolor="blue", alpha = 0.2))
133
134     patches = [ Circle(result_pt, 20, alpha=0.4, facecolor="green") ]
135     pcol.append(PatchCollection(patches, cmap=matplotlib.cm.jet, alpha = 1))
136
137     # text area, far from the point
138     l = [(800., 1800.), (800., 500.), (1500., 1800.), (1500., 500.),
139          (2200., 1800.), (2200., 500.)]
140     l.sort(cmp=lambda p1,p2: (dist(p1,real_pt)<dist(p2,real_pt)) and 1 or -1)
141     x,y = l[0]
142     text += "result_pt: x=%2.2f, y=%2.2f, a=%2.2f\n"%(result_x, result_y, result_a)
143     error_dist = dist(real_pt, result_pt)
144     error_a = result_a - real_a
145     if error_a > math.pi:
146         error_a -= 2*math.pi
147     if error_a < -math.pi:
148         error_a += 2*math.pi
149     text += "error = %2.2f mm, %2.2f deg"%(error_dist, error_a * 180. / math.pi)
150     matplotlib.pyplot.text(x, y, text, size=8,
151              ha="center", va="center",
152              bbox = dict(boxstyle="round",
153                          ec=(1., 0.5, 0.5),
154                          fc=(1., 0.8, 0.8),
155                          alpha=0.6,
156                          ),
157              alpha=0.8
158              )
159     for p in pcol:
160         ax.add_collection(p)
161
162     ax.grid()
163     ax.set_xlim(-100, 3100)
164     ax.set_ylim(-100, 2200)
165     #ax.set_title('spline paths')
166     #plt.show()
167     fig.savefig(filename)
168
169 # graph position from angles
170 def graph(filename, real_x, real_y, real_a):
171     pcol = []
172     print "real_pt = %s"%(str((real_x, real_y, real_a)))
173     real_pt = (real_x, real_y)
174
175     # display beacons
176     patches = []
177     for b in beacons:
178         patches += [ Circle((b[0], b[1]), 40, alpha=0.4) ]
179     pcol.append(PatchCollection(patches, facecolor="yellow", alpha = 1))
180
181     patches = [ Circle((real_x, real_y), 20, alpha=0.4, facecolor="red") ]
182     pcol.append(PatchCollection(patches, facecolor="red", alpha = 1))
183
184     # process angles from robot position
185     a0,ea0 = get_angle((real_x, real_y), beacons[0])
186     a1,ea1 = get_angle((real_x, real_y), beacons[1])
187     a2,ea2 = get_angle((real_x, real_y), beacons[2])
188     a0 -=  real_a
189     a1 -=  real_a
190     a2 -=  real_a
191     text  = "a0 = %2.2f (%+2.2f deg)\n"%(a0, ea0*(180./math.pi))
192     text += "a1 = %2.2f (%+2.2f deg)\n"%(a1, ea1*(180./math.pi))
193     text += "a2 = %2.2f (%+2.2f deg)\n"%(a2, ea2*(180./math.pi))
194
195     cmd = "./main angle2pos %f %f %f"%(a0, a1, a2)
196     print cmd
197     o,i = popen2.popen2(cmd)
198     i.close()
199     s = o.read(1000000)
200     o.close()
201
202     open(filename + ".txt", "w").write(s)
203
204     if len(s) == 1000000:
205         gloupix()
206
207     fig = plt.figure()
208     ax = fig.add_subplot(111)
209     ax.set_title("Erreur de position en mm lorsqu'on ajoute une erreur de mesure\n"
210                  "d'angle aleatoire comprise entre - %1.1f et %1.1f deg"%(RANDOM_ERROR_A,
211                                                                           RANDOM_ERROR_A))
212
213     # area
214     x,y = build_poly([(0,0), (3000,0), (3000,2100), (0,2100)])
215     ax.plot(x, y, 'g-')
216
217     result_pt = (-1, -1)
218     patches = []
219     for l in s.split("\n"):
220         m = re.match("circle: x=%s y=%s r=%s"%(FLOAT, FLOAT, FLOAT), l)
221         if m:
222             x,y,r = (float(m.groups()[0]), float(m.groups()[1]), float(m.groups()[2]))
223             print x,y,r
224             patches += [ Circle((x, y), r, facecolor="none") ]
225         m = re.match("p%s: x=%s y=%s"%(INT, FLOAT, FLOAT), l)
226         if m:
227             n,x,y = (float(m.groups()[0]), float(m.groups()[1]), float(m.groups()[2]))
228             if (n == 0):
229                 result_pt = (x, y)
230             text += l + "\n"
231
232     pcol.append(PatchCollection(patches, facecolor="none", alpha = 0.6))
233     pcol.append(PatchCollection(patches, facecolor="blue", alpha = 0.2))
234
235     patches = [ Circle(result_pt, 20, alpha=0.4, facecolor="green") ]
236     pcol.append(PatchCollection(patches, cmap=matplotlib.cm.jet, alpha = 1))
237
238     # text area, far from the point
239     l = [(800., 1800.), (800., 500.), (1500., 1800.), (1500., 500.),
240          (2200., 1800.), (2200., 500.)]
241     l.sort(cmp=lambda p1,p2: (dist(p1,real_pt)<dist(p2,real_pt)) and 1 or -1)
242     x,y = l[0]
243     text += "real_pt: x=%2.2f, y=%2.2f\n"%(real_x, real_y)
244     text += "error = %2.2f mm"%(dist(real_pt, result_pt))
245     matplotlib.pyplot.text(x, y, text, size=8,
246              ha="center", va="center",
247              bbox = dict(boxstyle="round",
248                          ec=(1., 0.5, 0.5),
249                          fc=(1., 0.8, 0.8),
250                          alpha=0.6,
251                          ),
252              alpha=0.8
253              )
254     for p in pcol:
255         ax.add_collection(p)
256
257     ax.grid()
258     ax.set_xlim(-100, 3100)
259     ax.set_ylim(-100, 2200)
260     #ax.set_title('spline paths')
261     #plt.show()
262     fig.savefig(filename)
263
264 def do_random_test():
265     random.seed(0)
266     for i in range(21):
267         print "---- random %d"%i
268         x = random.randint(0, 3000)
269         y = random.randint(0, 2100)
270         a = random.random()*2*math.pi - math.pi
271         graph("test%d.png"%i, x, y, a)
272         graph_da("test_da%d.png"%i, x, y, a)
273
274 def do_graph_2d(data, filename, title):
275     # Make plot with vertical (default) colorbar
276     fig = plt.figure()
277     ax = fig.add_subplot(111)
278
279     cax = ax.imshow(data)
280     ax.set_title(title)
281
282     # Add colorbar, make sure to specify tick locations to match desired ticklabels
283     cbar = fig.colorbar(cax, ticks=[0, 50])
284     cbar.ax.set_yticklabels(['0', '50 and more'])# vertically oriented colorbar
285     fig.savefig(filename)
286
287 def get_data(cmd, sat=0):
288     data = np.array([[50.]*210]*300)
289     oo,ii = popen2.popen2(cmd)
290     ii.close()
291     while True:
292         l = oo.readline()
293         if l == "":
294             break
295         try:
296             x,y,e = l[:-1].split(" ")
297         except:
298             print "Fail: %s"%(l)
299             continue
300         x = int(x)
301         y = int(y)
302         e = float(e)
303         if sat:
304             if e < sat:
305                 e = 0
306             else:
307                 e = sat
308         data[x,y] = e
309     oo.close()
310     return data
311
312 def do_graph_2d_simple_error():
313     for i in range(4):
314         for j in ["0.1", "0.5", "1.0"]:
315             print "do_graph_2d_simple_error %s %s"%(i, j)
316             data = get_data("./main simple_error %d %s"%(i,j))
317             if i != 3:
318                 title  = 'Erreur de position en mm, pour une erreur\n'
319                 title += 'de mesure de %s deg sur la balise %d'%(j,i)
320             else:
321                 title  = 'Erreur de position en mm, pour une erreur\n'
322                 title += 'de mesure de %s deg sur les 3 balises'%(j)
323             do_graph_2d(data, "error_a%d_%s.png"%(i,j), title)
324
325 def do_graph_2d_ad_error():
326     for d in ["0.0", "0.1", "0.5", "1.0"]:
327         for a in ["0.0", "0.1", "0.5", "1.0"]:
328             for i in ["0", "1", "2"]:
329                 print "do_graph_2d_ad_error %s %s %s"%(i, d, a)
330                 data = get_data("./main da_error %s %s -%s"%(i, d, a))
331                 title  = 'Erreur de position en mm, pour une erreur\n'
332                 title += "d'angle de %s deg et dist de %s %% (algo %s)"%(a, d, i)
333                 do_graph_2d(data, "error_da_%s_%s_%s.png"%(i, d, a), title)
334
335 def do_graph_2d_ad_error_mm():
336     for d in ["5", "10", "20"]:
337         for a in ["0.0", "0.1", "0.5", "1.0"]:
338             print "do_graph_2d_ad_error_mm 0 %s %s"%(d, a)
339             data = get_data("./main da_error_mm 0 %s -%s"%(d, a))
340             title  = 'Erreur de position en mm, pour une erreur\n'
341             title += "d'angle de %s deg et dist de %s mm"%(a, d)
342             do_graph_2d(data, "error_da_%smm_%s.png"%(d, a), title)
343
344 def do_graph_2d_move_error():
345     i = 0
346     for period in [ 20, 40 ]:
347         for speed in [ 0.3, 0.7, 1. ]:
348             angle_deg = 0
349             print "do_graph_2d_move_error %s %s"%(period, speed)
350             while angle_deg < 360:
351                 angle_rad = angle_deg * (math.pi/180.)
352                 data = get_data("./main move_error %f %f %f"%(speed, period, angle_rad))
353                 do_graph_2d(data, "error_move_error_%d.png"%(i),
354                             'Erreur de mesure si le robot se deplace a %2.2f m/s\n'
355                             'vers %d deg (periode tourelle = %d ms)'%(speed, angle_deg, period))
356                 angle_deg += 45
357                 i += 1
358     period = 20
359     speed = 1.
360     angle_deg = 45
361     angle_rad = angle_deg * (math.pi/180.)
362     data = get_data("./main move_error %f %f %f"%(speed, period, angle_rad), sat=20)
363     do_graph_2d(data, "error_move_error_%d.png"%(i),
364                 "En rouge, l'erreur de mesure est > 2cm (pour un deplacement\n"
365                 'a %2.2f m/s vers %d deg et une periode tourelle = %d ms)'%(speed, angle_deg, period))
366
367 #do_random_test()
368 #do_graph_2d_simple_error()
369 #do_graph_2d_move_error()
370 #do_graph_2d_ad_error()
371 do_graph_2d_ad_error_mm()