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
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) ]
19 def build_poly(ptlist):
21 polydata.append((Path.MOVETO, (ptlist[0])))
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)
30 def build_path(ptlist):
32 polydata.append((Path.MOVETO, (ptlist[0])))
34 polydata.append((Path.LINETO, (pt)))
35 codes, verts = zip(*polydata)
36 poly = mpath.Path(verts, codes)
37 x, y = zip(*poly.vertices)
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])
47 alpha = math.atan2(a[1]-ref[1], a[0]-ref[0])
48 beta = math.atan2(b[1]-ref[1], b[0]-ref[0])
51 gamma = gamma + 2*math.pi
52 return gamma + error, error
55 return math.sqrt((p1[0]-p2[0])**2+(p1[1]-p2[1])**2)
57 def graph(filename, real_x, real_y):
59 real_pt = (real_x, real_y)
64 patches += [ Circle((b[0], b[1]), 40, alpha=0.4) ]
66 patches += [ Circle((real_x, real_y), 20, alpha=0.4, facecolor="red") ]
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))
86 cmd = "./main angle2pos %f %f %f"%(a01, a12, a20)
87 o,i = popen2.popen2(cmd)
92 open(filename + ".txt", "w").write(s)
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,
104 x,y = build_poly([(0,0), (3000,0), (3000,2100), (0,2100)])
107 for l in s.split("\n"):
108 m = re.match("circle: x=%s y=%s r=%s"%(FLOAT, FLOAT, FLOAT), l)
110 x,y,r = (float(m.groups()[0]), float(m.groups()[1]), float(m.groups()[2]))
112 patches += [ Circle((x, y), r, facecolor="none") ]
113 m = re.match("p%s: x=%s y=%s"%(INT, FLOAT, FLOAT), l)
115 n,x,y = (float(m.groups()[0]), float(m.groups()[1]), float(m.groups()[2]))
117 patches += [ Circle((x, y), 20, alpha=0.4, facecolor="yellow") ]
121 p = PatchCollection(patches, cmap=matplotlib.cm.jet, match_original=True)
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)
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",
142 ax.set_xlim(-100, 3100)
143 ax.set_ylim(-100, 2200)
144 #ax.set_title('spline paths')
146 fig.savefig(filename)
148 def do_random_test():
151 x = random.randint(0, 3000)
152 y = random.randint(0, 2100)
153 graph("test%d.png"%i, x, y)
155 def do_graph_2d(data, filename, title):
156 # Make plot with vertical (default) colorbar
158 ax = fig.add_subplot(111)
160 cax = ax.imshow(data)
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)
168 def get_data(cmd, sat=0):
169 data = np.array([[0.]*210]*300)
170 oo,ii = popen2.popen2(cmd)
177 x,y,e = l[:-1].split(" ")
193 def do_graph_2d_simple_error():
195 for j in ["0.1", "0.5", "1.0"]:
196 data = get_data("./main simple_error %d %s"%(i,j))
198 title = 'Erreur de position en mm, pour une erreur\n'
199 title += 'de mesure de %s deg sur la balise %d'%(j,i)
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)
205 def do_graph_2d_move_error():
207 for period in [ 20, 40 ]:
208 for speed in [ 0.3, 0.7, 1. ]:
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))
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))
228 #do_graph_2d_simple_error()
229 do_graph_2d_move_error()