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()