restore first circle algo + hostsim work
[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 = 0.3 # deg
17 beacons = [ (0.0, 1050.0), (3000.0, 0.0), (3000.0, 2100.0) ]
18
19 def build_poly(ptlist):
20     polydata = []
21     polydata.append((Path.MOVETO, (ptlist[0])))
22     for pt in ptlist[1:]:
23         polydata.append((Path.LINETO, (pt)))
24     polydata.append((Path.CLOSEPOLY, (ptlist[0])))
25     codes, verts = zip(*polydata)
26     poly = mpath.Path(verts, codes)
27     x, y = zip(*poly.vertices)
28     return x,y
29
30 def build_path(ptlist):
31     polydata = []
32     polydata.append((Path.MOVETO, (ptlist[0])))
33     for pt in ptlist[1:]:
34         polydata.append((Path.LINETO, (pt)))
35     codes, verts = zip(*polydata)
36     poly = mpath.Path(verts, codes)
37     x, y = zip(*poly.vertices)
38     return x,y
39
40 def get_angle(ref, b):
41     """get angle from robot point of view (ref) of beacon 'b'"""
42     a = math.atan2(b[1]-ref[1], b[0]-ref[0])
43     ea = (math.pi/180.) * RANDOM_ERROR * random.random()
44     ea = random.choice([ea, -ea])
45     return a + ea, ea
46
47     alpha = math.atan2(a[1]-ref[1], a[0]-ref[0])
48     beta = math.atan2(b[1]-ref[1], b[0]-ref[0])
49     gamma = beta-alpha
50     if gamma < 0:
51         gamma = gamma + 2*math.pi
52     return gamma + error, error
53
54 def dist(p1, p2):
55     return math.sqrt((p1[0]-p2[0])**2+(p1[1]-p2[1])**2)
56
57 def graph(filename, real_x, real_y):
58
59     real_pt = (real_x, real_y)
60
61     # display beacons
62     patches = []
63     for b in beacons:
64         patches += [ Circle((b[0], b[1]), 40, alpha=0.4) ]
65
66     patches += [ Circle((real_x, real_y), 20, alpha=0.4, facecolor="red") ]
67
68     # process angles from robot position
69     a0,ea0 = get_angle((real_x, real_y), beacons[0])
70     a1,ea1 = get_angle((real_x, real_y), beacons[1])
71     a2,ea2 = get_angle((real_x, real_y), beacons[2])
72     text  = "a0 = %2.2f (%+2.2f deg)\n"%(a0, ea0*(180./math.pi))
73     text += "a1 = %2.2f (%+2.2f deg)\n"%(a1, ea1*(180./math.pi))
74     text += "a2 = %2.2f (%+2.2f deg)\n"%(a2, ea2*(180./math.pi))
75
76     a01 = a1-a0
77     if a01 < 0:
78         a01 += 2*math.pi
79     a12 = a2-a1
80     if a12 < 0:
81         a12 += 2*math.pi
82     a20 = a0-a2
83     if a20 < 0:
84         a20 += 2*math.pi
85
86     cmd = "./main angle2pos %f %f %f"%(a01, a12, a20)
87     o,i = popen2.popen2(cmd)
88     i.close()
89     s = o.read(1000000)
90     o.close()
91
92     open(filename + ".txt", "w").write(s)
93
94     if len(s) == 1000000:
95         gloupix()
96
97     fig = plt.figure()
98     ax = fig.add_subplot(111)
99     ax.set_title("Erreur de position en mm lorsqu'on ajoute une erreur de mesure\n"
100                  "d'angle aleatoire comprise entre - %1.1f et %1.1f deg"%(RANDOM_ERROR,
101                                                                           RANDOM_ERROR))
102
103     # area
104     x,y = build_poly([(0,0), (3000,0), (3000,2100), (0,2100)])
105     ax.plot(x, y, 'g-')
106
107     for l in s.split("\n"):
108         m = re.match("circle: x=%s y=%s r=%s"%(FLOAT, FLOAT, FLOAT), l)
109         if m:
110             x,y,r = (float(m.groups()[0]), float(m.groups()[1]), float(m.groups()[2]))
111             print x,y,r
112             patches += [ Circle((x, y), r, facecolor="none") ]
113         m = re.match("p%s: x=%s y=%s"%(INT, FLOAT, FLOAT), l)
114         if m:
115             n,x,y = (float(m.groups()[0]), float(m.groups()[1]), float(m.groups()[2]))
116             if (n == 0):
117                 patches += [ Circle((x, y), 20, alpha=0.4, facecolor="yellow") ]
118                 result_pt = (x, y)
119             text += l + "\n"
120
121     p = PatchCollection(patches, cmap=matplotlib.cm.jet, match_original=True)
122
123     # text area, far from the point
124     l = [(800., 1800.), (800., 500.), (1500., 1800.), (1500., 500.),
125          (2200., 1800.), (2200., 500.)]
126     l.sort(cmp=lambda p1,p2: (dist(p1,real_pt)<dist(p2,real_pt)) and 1 or -1)
127     x,y = l[0]
128     text += "real_pt: x=%2.2f, y=%2.2f\n"%(real_x, real_y)
129     text += "error = %2.2f mm"%(dist(real_pt, result_pt))
130     matplotlib.pyplot.text(x, y, text, size=8,
131              ha="center", va="center",
132              bbox = dict(boxstyle="round",
133                          ec=(1., 0.5, 0.5),
134                          fc=(1., 0.8, 0.8),
135                          alpha=0.6,
136                          ),
137              alpha=0.8
138              )
139     ax.add_collection(p)
140
141     ax.grid()
142     ax.set_xlim(-100, 3100)
143     ax.set_ylim(-100, 2200)
144     #ax.set_title('spline paths')
145     #plt.show()
146     fig.savefig(filename)
147
148 def do_random_test():
149     random.seed(0)
150     for i in range(100):
151         x = random.randint(0, 3000)
152         y = random.randint(0, 2100)
153         graph("test%d.png"%i, x, y)
154
155 def do_graph_2d(data, filename, title):
156     # Make plot with vertical (default) colorbar
157     fig = plt.figure()
158     ax = fig.add_subplot(111)
159
160     cax = ax.imshow(data)
161     ax.set_title(title)
162
163     # Add colorbar, make sure to specify tick locations to match desired ticklabels
164     cbar = fig.colorbar(cax, ticks=[0, 50])
165     cbar.ax.set_yticklabels(['0', '50 et plus'])# vertically oriented colorbar
166     fig.savefig(filename)
167
168 def get_data(cmd, sat=0):
169     data = np.array([[0.]*210]*300)
170     oo,ii = popen2.popen2(cmd)
171     ii.close()
172     while True:
173         l = oo.readline()
174         if l == "":
175             break
176         try:
177             x,y,e = l[:-1].split(" ")
178         except:
179             print "Fail: %s"%(l)
180             continue
181         x = int(x)
182         y = int(y)
183         e = float(e)
184         if sat:
185             if e < sat:
186                 e = 0
187             else:
188                 e = sat
189         data[x,y] = e
190     oo.close()
191     return data
192
193 def do_graph_2d_simple_error():
194     for i in range(4):
195         for j in ["0.1", "0.5", "1.0"]:
196             data = get_data("./main simple_error %d %s"%(i,j))
197             if i != 3:
198                 title  = 'Erreur de position en mm, pour une erreur\n'
199                 title += 'de mesure de %s deg sur la balise %d'%(j,i)
200             else:
201                 title  = 'Erreur de position en mm, pour une erreur\n'
202                 title += 'de mesure de %s deg sur les 3 balises'%(j)
203             do_graph_2d(data, "error_a%d_%s.png"%(i,j), title)
204
205 def do_graph_2d_move_error():
206     i = 0
207     for period in [ 20, 40 ]:
208         for speed in [ 0.3, 0.7, 1. ]:
209             angle_deg = 0
210             while angle_deg < 360:
211                 angle_rad = angle_deg * (math.pi/180.)
212                 data = get_data("./main move_error %f %f %f"%(speed, period, angle_rad))
213                 do_graph_2d(data, "error_move_error_%d.png"%(i),
214                             'Erreur de mesure si le robot se deplace a %2.2f m/s\n'
215                             'vers %d deg (periode tourelle = %d ms)'%(speed, angle_deg, period))
216                 angle_deg += 45
217                 i += 1
218     period = 20
219     speed = 1.
220     angle_deg = 45
221     angle_rad = angle_deg * (math.pi/180.)
222     data = get_data("./main move_error %f %f %f"%(speed, period, angle_rad), sat=20)
223     do_graph_2d(data, "error_move_error_%d.png"%(i),
224                 "En rouge, l'erreur de mesure est > 2cm (pour un deplacement\n"
225                 'a %2.2f m/s vers %d deg et une periode tourelle = %d ms)'%(speed, angle_deg, period))
226
227 #do_random_test()
228 #do_graph_2d_simple_error()
229 do_graph_2d_move_error()