af8edf8f04dabd1a884ecfc212e95b72bc59d928
[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  = "a0 = %2.2f (%+2.2f deg)\n"%(a0, ea0*(180./math.pi))
80     text += "a1 = %2.2f (%+2.2f deg)\n"%(a1, ea1*(180./math.pi))
81     d0,ed0 = get_distance((real_x, real_y), beacons[0])
82     d1,ed1 = get_distance((real_x, real_y), beacons[1])
83     text += "d0 = %2.2f (%+2.2f %%)\n"%(d0, ed0)
84     text += "d1 = %2.2f (%+2.2f %%)\n"%(d1, ed1)
85
86     cmd = "./main ad2pos %f %f %f %f"%(a0, a1, d0, d1)
87     print cmd
88     o,i = popen2.popen2(cmd)
89     i.close()
90     s = o.read(1000000)
91     o.close()
92
93     open(filename + ".txt", "w").write(s)
94
95     if len(s) == 1000000:
96         gloupix()
97
98     fig = plt.figure()
99     ax = fig.add_subplot(111)
100     title = "Erreur de position en mm, qd l'erreur "
101     title += "d'angle aleatoire est comprise\n"
102     title += "erreur -%1.1f et %1.1f deg "%(RANDOM_ERROR_A, RANDOM_ERROR_A)
103     title += "et de distance de +/- %2.2f %%"%(RANDOM_ERROR_D)
104     ax.set_title(title)
105
106     # area
107     x,y = build_poly([(0,0), (3000,0), (3000,2100), (0,2100)])
108     ax.plot(x, y, 'g-')
109
110     result_pt = (-1, -1)
111     patches = []
112     for l in s.split("\n"):
113         m = re.match("circle: x=%s y=%s r=%s"%(FLOAT, FLOAT, FLOAT), l)
114         if m:
115             x,y,r = (float(m.groups()[0]), float(m.groups()[1]), float(m.groups()[2]))
116             print x,y,r
117             patches += [ Circle((x, y), r, facecolor="none") ]
118         m = re.match("p%s: x=%s y=%s a=%s"%(INT, FLOAT, FLOAT, FLOAT), l)
119         if m:
120             n,x,y,a = (float(m.groups()[0]), float(m.groups()[1]),
121                         float(m.groups()[2]), float(m.groups()[3]))
122             if (n == 0):
123                 patches += [ Circle((x, y), 20, alpha=0.4, facecolor="yellow") ]
124                 result_pt = (x, y)
125             text += l + "\n"
126
127     pcol.append(PatchCollection(patches, facecolor="none", alpha = 0.6))
128     pcol.append(PatchCollection(patches, facecolor="blue", alpha = 0.2))
129
130     patches = [ Circle(result_pt, 20, alpha=0.4, facecolor="green") ]
131     pcol.append(PatchCollection(patches, cmap=matplotlib.cm.jet, alpha = 1))
132
133     # text area, far from the point
134     l = [(800., 1800.), (800., 500.), (1500., 1800.), (1500., 500.),
135          (2200., 1800.), (2200., 500.)]
136     l.sort(cmp=lambda p1,p2: (dist(p1,real_pt)<dist(p2,real_pt)) and 1 or -1)
137     x,y = l[0]
138     text += "real_pt: x=%2.2f, y=%2.2f\n"%(real_x, real_y)
139     text += "error = %2.2f mm"%(dist(real_pt, result_pt))
140     matplotlib.pyplot.text(x, y, text, size=8,
141              ha="center", va="center",
142              bbox = dict(boxstyle="round",
143                          ec=(1., 0.5, 0.5),
144                          fc=(1., 0.8, 0.8),
145                          alpha=0.6,
146                          ),
147              alpha=0.8
148              )
149     for p in pcol:
150         ax.add_collection(p)
151
152     ax.grid()
153     ax.set_xlim(-100, 3100)
154     ax.set_ylim(-100, 2200)
155     #ax.set_title('spline paths')
156     #plt.show()
157     fig.savefig(filename)
158
159 # graph position from angles
160 def graph(filename, real_x, real_y, real_a):
161     pcol = []
162     print "real_pt = %s"%(str((real_x, real_y, real_a)))
163     real_pt = (real_x, real_y)
164
165     # display beacons
166     patches = []
167     for b in beacons:
168         patches += [ Circle((b[0], b[1]), 40, alpha=0.4) ]
169     pcol.append(PatchCollection(patches, facecolor="yellow", alpha = 1))
170
171     patches = [ Circle((real_x, real_y), 20, alpha=0.4, facecolor="red") ]
172     pcol.append(PatchCollection(patches, facecolor="red", alpha = 1))
173
174     # process angles from robot position
175     a0,ea0 = get_angle((real_x, real_y), beacons[0])
176     a1,ea1 = get_angle((real_x, real_y), beacons[1])
177     a2,ea2 = get_angle((real_x, real_y), beacons[2])
178     a0 -=  real_a
179     a1 -=  real_a
180     a2 -=  real_a
181     text  = "a0 = %2.2f (%+2.2f deg)\n"%(a0, ea0*(180./math.pi))
182     text += "a1 = %2.2f (%+2.2f deg)\n"%(a1, ea1*(180./math.pi))
183     text += "a2 = %2.2f (%+2.2f deg)\n"%(a2, ea2*(180./math.pi))
184
185     cmd = "./main angle2pos %f %f %f"%(a0, a1, a2)
186     print cmd
187     o,i = popen2.popen2(cmd)
188     i.close()
189     s = o.read(1000000)
190     o.close()
191
192     open(filename + ".txt", "w").write(s)
193
194     if len(s) == 1000000:
195         gloupix()
196
197     fig = plt.figure()
198     ax = fig.add_subplot(111)
199     ax.set_title("Erreur de position en mm lorsqu'on ajoute une erreur de mesure\n"
200                  "d'angle aleatoire comprise entre - %1.1f et %1.1f deg"%(RANDOM_ERROR_A,
201                                                                           RANDOM_ERROR_A))
202
203     # area
204     x,y = build_poly([(0,0), (3000,0), (3000,2100), (0,2100)])
205     ax.plot(x, y, 'g-')
206
207     result_pt = (-1, -1)
208     patches = []
209     for l in s.split("\n"):
210         m = re.match("circle: x=%s y=%s r=%s"%(FLOAT, FLOAT, FLOAT), l)
211         if m:
212             x,y,r = (float(m.groups()[0]), float(m.groups()[1]), float(m.groups()[2]))
213             print x,y,r
214             patches += [ Circle((x, y), r, facecolor="none") ]
215         m = re.match("p%s: x=%s y=%s"%(INT, FLOAT, FLOAT), l)
216         if m:
217             n,x,y = (float(m.groups()[0]), float(m.groups()[1]), float(m.groups()[2]))
218             if (n == 0):
219                 result_pt = (x, y)
220             text += l + "\n"
221
222     pcol.append(PatchCollection(patches, facecolor="none", alpha = 0.6))
223     pcol.append(PatchCollection(patches, facecolor="blue", alpha = 0.2))
224
225     patches = [ Circle(result_pt, 20, alpha=0.4, facecolor="green") ]
226     pcol.append(PatchCollection(patches, cmap=matplotlib.cm.jet, alpha = 1))
227
228     # text area, far from the point
229     l = [(800., 1800.), (800., 500.), (1500., 1800.), (1500., 500.),
230          (2200., 1800.), (2200., 500.)]
231     l.sort(cmp=lambda p1,p2: (dist(p1,real_pt)<dist(p2,real_pt)) and 1 or -1)
232     x,y = l[0]
233     text += "real_pt: x=%2.2f, y=%2.2f\n"%(real_x, real_y)
234     text += "error = %2.2f mm"%(dist(real_pt, result_pt))
235     matplotlib.pyplot.text(x, y, text, size=8,
236              ha="center", va="center",
237              bbox = dict(boxstyle="round",
238                          ec=(1., 0.5, 0.5),
239                          fc=(1., 0.8, 0.8),
240                          alpha=0.6,
241                          ),
242              alpha=0.8
243              )
244     for p in pcol:
245         ax.add_collection(p)
246
247     ax.grid()
248     ax.set_xlim(-100, 3100)
249     ax.set_ylim(-100, 2200)
250     #ax.set_title('spline paths')
251     #plt.show()
252     fig.savefig(filename)
253
254 def do_random_test():
255     random.seed(0)
256     for i in range(100):
257         print "---- random %d"%i
258         x = random.randint(0, 3000)
259         y = random.randint(0, 2100)
260         a = random.random()*2*math.pi
261         graph("angle/test%d.png"%i, x, y, a)
262         graph_da("da/test_da%d.png"%i, x, y, a)
263
264 def do_graph_2d(data, filename, title):
265     # Make plot with vertical (default) colorbar
266     fig = plt.figure()
267     ax = fig.add_subplot(111)
268
269     cax = ax.imshow(data)
270     ax.set_title(title)
271
272     # Add colorbar, make sure to specify tick locations to match desired ticklabels
273     cbar = fig.colorbar(cax, ticks=[0, 50])
274     cbar.ax.set_yticklabels(['0', '50 and more'])# vertically oriented colorbar
275     fig.savefig(filename)
276
277 def get_data(cmd, sat=0):
278     data = np.array([[0.]*210]*300)
279     oo,ii = popen2.popen2(cmd)
280     ii.close()
281     while True:
282         l = oo.readline()
283         if l == "":
284             break
285         try:
286             x,y,e = l[:-1].split(" ")
287         except:
288             print "Fail: %s"%(l)
289             continue
290         x = int(x)
291         y = int(y)
292         e = float(e)
293         if sat:
294             if e < sat:
295                 e = 0
296             else:
297                 e = sat
298         data[x,y] = e
299     oo.close()
300     return data
301
302 def do_graph_2d_simple_error():
303     for i in range(4):
304         for j in ["0.1", "0.5", "1.0"]:
305             print "do_graph_2d_simple_error %s %s"%(i, j)
306             data = get_data("./main simple_error %d %s"%(i,j))
307             if i != 3:
308                 title  = 'Erreur de position en mm, pour une erreur\n'
309                 title += 'de mesure de %s deg sur la balise %d'%(j,i)
310             else:
311                 title  = 'Erreur de position en mm, pour une erreur\n'
312                 title += 'de mesure de %s deg sur les 3 balises'%(j)
313             do_graph_2d(data, "simple_error/error_a%d_%s.png"%(i,j), title)
314
315 def do_graph_2d_ad_error():
316     for d in ["0.0", "0.1", "0.5"]:
317         for a in ["0.0", "0.1", "0.5"]:
318             for i in ["0", "1", "2"]:
319                 print "do_graph_2d_ad_error %s %s %s"%(i, d, a)
320                 data = get_data("./main da_error %s %s %s"%(i, d, a))
321                 title  = 'Erreur de position en mm, pour une erreur\n'
322                 title += "d'angle de %s deg et dist de %s %% (algo %s)"%(a, d, i)
323                 do_graph_2d(data, "da_error/error_da_%s_%s_%s.png"%(i, d, a), title)
324
325 def do_graph_2d_move_error():
326     i = 0
327     for period in [ 20, 40 ]:
328         for speed in [ 0.3, 0.7, 1. ]:
329             angle_deg = 0
330             print "do_graph_2d_move_error %s %s"%(period, speed)
331             while angle_deg < 360:
332                 angle_rad = angle_deg * (math.pi/180.)
333                 data = get_data("./main move_error %f %f %f"%(speed, period, angle_rad))
334                 do_graph_2d(data, "move_error/error_move_error_%d.png"%(i),
335                             'Erreur de mesure si le robot se deplace a %2.2f m/s\n'
336                             'vers %d deg (periode tourelle = %d ms)'%(speed, angle_deg, period))
337                 angle_deg += 45
338                 i += 1
339     period = 20
340     speed = 1.
341     angle_deg = 45
342     angle_rad = angle_deg * (math.pi/180.)
343     data = get_data("./main move_error %f %f %f"%(speed, period, angle_rad), sat=20)
344     do_graph_2d(data, "error_move_error_%d.png"%(i),
345                 "En rouge, l'erreur de mesure est > 2cm (pour un deplacement\n"
346                 'a %2.2f m/s vers %d deg et une periode tourelle = %d ms)'%(speed, angle_deg, period))
347
348 do_random_test()
349 do_graph_2d_simple_error()
350 do_graph_2d_move_error()
351 do_graph_2d_ad_error()