+ patches = [ Circle((real_x, real_y), 20, alpha=0.4, facecolor="red") ]
+ pcol.append(PatchCollection(patches, facecolor="red", alpha = 1))
+
+ # process angles from robot position
+ a0,ea0 = get_angle((real_x, real_y), beacons[0])
+ a1,ea1 = get_angle((real_x, real_y), beacons[1])
+ a0 -= real_a
+ a1 -= real_a
+ text = "real_pt = %2.2f, %2.2f, %2.2f\n"%(real_x, real_y, real_a)
+ text += "a0 = %2.2f (%+2.2f deg)\n"%(a0, ea0*(180./math.pi))
+ text += "a1 = %2.2f (%+2.2f deg)\n"%(a1, ea1*(180./math.pi))
+ d0,ed0 = get_distance((real_x, real_y), beacons[0])
+ d1,ed1 = get_distance((real_x, real_y), beacons[1])
+ text += "d0 = %2.2f (%+2.2f %%)\n"%(d0, ed0)
+ text += "d1 = %2.2f (%+2.2f %%)\n"%(d1, ed1)
+
+ cmd = "./main ad2pos %f %f %f %f"%(a0, a1, d0, d1)
+ print cmd
+ o,i = popen2.popen2(cmd)
+ i.close()
+ s = o.read(1000000)
+ o.close()
+
+ open(filename + ".txt", "w").write(s)
+
+ if len(s) == 1000000:
+ gloupix()
+
+ fig = plt.figure()
+ ax = fig.add_subplot(111)
+ title = "Erreur de position en mm, qd l'erreur "
+ title += "d'angle aleatoire est comprise\n"
+ title += "erreur -%1.1f et %1.1f deg "%(RANDOM_ERROR_A, RANDOM_ERROR_A)
+ title += "et de distance de +/- %2.2f %%"%(RANDOM_ERROR_D)
+ ax.set_title(title)
+
+ # area
+ x,y = build_poly([(0,0), (3000,0), (3000,2100), (0,2100)])
+ ax.plot(x, y, 'g-')
+
+ result_pt = (-1, -1)
+ result_x, result_y, result_a = -1, -1, -1
+ patches = []
+ for l in s.split("\n"):
+ m = re.match("circle: x=%s y=%s r=%s"%(FLOAT, FLOAT, FLOAT), l)
+ if m:
+ x,y,r = (float(m.groups()[0]), float(m.groups()[1]), float(m.groups()[2]))
+ print x,y,r
+ patches += [ Circle((x, y), r, facecolor="none") ]
+ m = re.match("p%s: x=%s y=%s a=%s"%(INT, FLOAT, FLOAT, FLOAT), l)
+ if m:
+ n,x,y,a = (float(m.groups()[0]), float(m.groups()[1]),
+ float(m.groups()[2]), float(m.groups()[3]))
+ if (n == 0):
+ patches += [ Circle((x, y), 20, alpha=0.4, facecolor="yellow") ]
+ result_x, result_y = (x, y)
+ result_pt = (x,y)
+ result_a = a
+ text += l + "\n"
+
+ pcol.append(PatchCollection(patches, facecolor="none", alpha = 0.6))
+ pcol.append(PatchCollection(patches, facecolor="blue", alpha = 0.2))
+
+ patches = [ Circle(result_pt, 20, alpha=0.4, facecolor="green") ]
+ pcol.append(PatchCollection(patches, cmap=matplotlib.cm.jet, alpha = 1))
+
+ # text area, far from the point
+ l = [(800., 1800.), (800., 500.), (1500., 1800.), (1500., 500.),
+ (2200., 1800.), (2200., 500.)]
+ l.sort(cmp=lambda p1,p2: (dist(p1,real_pt)<dist(p2,real_pt)) and 1 or -1)
+ x,y = l[0]
+ text += "result_pt: x=%2.2f, y=%2.2f, a=%2.2f\n"%(result_x, result_y, result_a)
+ error_dist = dist(real_pt, result_pt)
+ error_a = result_a - real_a
+ if error_a > math.pi:
+ error_a -= 2*math.pi
+ if error_a < -math.pi:
+ error_a += 2*math.pi
+ text += "error = %2.2f mm, %2.2f deg"%(error_dist, error_a * 180. / math.pi)
+ matplotlib.pyplot.text(x, y, text, size=8,
+ ha="center", va="center",
+ bbox = dict(boxstyle="round",
+ ec=(1., 0.5, 0.5),
+ fc=(1., 0.8, 0.8),
+ alpha=0.6,
+ ),
+ alpha=0.8
+ )
+ for p in pcol:
+ ax.add_collection(p)
+
+ ax.grid()
+ ax.set_xlim(-100, 3100)
+ ax.set_ylim(-100, 2200)
+ #ax.set_title('spline paths')
+ #plt.show()
+ fig.savefig(filename)
+
+# graph position from angles
+def graph(filename, real_x, real_y, real_a):
+ pcol = []
+ print "real_pt = %s"%(str((real_x, real_y, real_a)))
+ real_pt = (real_x, real_y)
+
+ # display beacons
+ patches = []
+ for b in beacons:
+ patches += [ Circle((b[0], b[1]), 40, alpha=0.4) ]
+ pcol.append(PatchCollection(patches, facecolor="yellow", alpha = 1))
+
+ patches = [ Circle((real_x, real_y), 20, alpha=0.4, facecolor="red") ]
+ pcol.append(PatchCollection(patches, facecolor="red", alpha = 1))