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(21):
 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("test%d.png"%i, x, y, a)
 262         graph_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, "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, "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, "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()