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