merge

[aversive.git] / projects / microb2010 / tests / tourel_beacon / graph.py

diff --git a/projects/microb2010/tests/tourel_beacon/graph.py b/projects/microb2010/tests/tourel_beacon/graph.py
index f1e175e..fa87953 100644 (file)
--- a/projects/microb2010/tests/tourel_beacon/graph.py
+++ b/projects/microb2010/tests/tourel_beacon/graph.py
@@ -76,7 +76,8 @@ def graph_da(filename, real_x, real_y, real_a):
     a1,ea1 = get_angle((real_x, real_y), beacons[1])
     a0 -=  real_a
     a1 -=  real_a
-    text  = "a0 = %2.2f (%+2.2f deg)\n"%(a0, ea0*(180./math.pi))
+    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])
@@ -108,6 +109,7 @@ def graph_da(filename, real_x, real_y, real_a):
     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)
@@ -121,7 +123,9 @@ def graph_da(filename, real_x, real_y, real_a):
                         float(m.groups()[2]), float(m.groups()[3]))
             if (n == 0):
                 patches += [ Circle((x, y), 20, alpha=0.4, facecolor="yellow") ]
-                result_pt = (x, y)
+                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))
@@ -135,8 +139,14 @@ def graph_da(filename, real_x, real_y, real_a):
          (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 += "real_pt: x=%2.2f, y=%2.2f\n"%(real_x, real_y)
-    text += "error = %2.2f mm"%(dist(real_pt, result_pt))
+    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",
@@ -253,13 +263,13 @@ def graph(filename, real_x, real_y, real_a):
 
 def do_random_test():
     random.seed(0)
-    for i in range(21):
+    for i in range(100):
         print "---- random %d"%i
         x = random.randint(0, 3000)
         y = random.randint(0, 2100)
-        a = random.random()*2*math.pi
-        graph("test%d.png"%i, x, y, a)
-        graph_da("test_da%d.png"%i, x, y, a)
+        a = random.random()*2*math.pi - math.pi
+        graph("angle/test%d.png"%i, x, y, a)
+        graph_da("da/test_da%d.png"%i, x, y, a)
 
 def do_graph_2d(data, filename, title):
     # Make plot with vertical (default) colorbar
@@ -275,7 +285,7 @@ def do_graph_2d(data, filename, title):
     fig.savefig(filename)
 
 def get_data(cmd, sat=0):
-    data = np.array([[0.]*210]*300)
+    data = np.array([[50.]*210]*300)
     oo,ii = popen2.popen2(cmd)
     ii.close()
     while True:
@@ -310,17 +320,26 @@ def do_graph_2d_simple_error():
             else:
                 title  = 'Erreur de position en mm, pour une erreur\n'
                 title += 'de mesure de %s deg sur les 3 balises'%(j)
-            do_graph_2d(data, "error_a%d_%s.png"%(i,j), title)
+            do_graph_2d(data, "simple_error/error_a%d_%s.png"%(i,j), title)
 
 def do_graph_2d_ad_error():
-    for d in ["0.0", "0.1", "0.5"]:
-        for a in ["0.0", "0.1", "0.5"]:
+    for d in ["0.0", "0.1", "0.5", "1.0"]:
+        for a in ["0.0", "0.1", "0.5", "1.0"]:
             for i in ["0", "1", "2"]:
                 print "do_graph_2d_ad_error %s %s %s"%(i, d, a)
-                data = get_data("./main da_error %s %s %s"%(i, d, a))
+                data = get_data("./main da_error %s %s -%s"%(i, d, a))
                 title  = 'Erreur de position en mm, pour une erreur\n'
                 title += "d'angle de %s deg et dist de %s %% (algo %s)"%(a, d, i)
-                do_graph_2d(data, "error_da_%s_%s_%s.png"%(i, d, a), title)
+                do_graph_2d(data, "da_error/error_da_%s_%s_%s.png"%(i, d, a), title)
+
+def do_graph_2d_ad_error_mm():
+    for d in ["5", "10", "20"]:
+        for a in ["0.0", "0.1", "0.5", "1.0"]:
+            print "do_graph_2d_ad_error_mm 0 %s %s"%(d, a)
+            data = get_data("./main da_error_mm 0 %s -%s"%(d, a))
+            title  = 'Erreur de position en mm, pour une erreur\n'
+            title += "d'angle de %s deg et dist de %s mm"%(a, d)
+            do_graph_2d(data, "da_error_mm/error_da_%smm_%s.png"%(d, a), title)
 
 def do_graph_2d_move_error():
     i = 0
@@ -331,7 +350,7 @@ def do_graph_2d_move_error():
             while angle_deg < 360:
                 angle_rad = angle_deg * (math.pi/180.)
                 data = get_data("./main move_error %f %f %f"%(speed, period, angle_rad))
-                do_graph_2d(data, "error_move_error_%d.png"%(i),
+                do_graph_2d(data, "move_error/error_move_error_%d.png"%(i),
                             'Erreur de mesure si le robot se deplace a %2.2f m/s\n'
                             'vers %d deg (periode tourelle = %d ms)'%(speed, angle_deg, period))
                 angle_deg += 45
@@ -345,7 +364,8 @@ def do_graph_2d_move_error():
                 "En rouge, l'erreur de mesure est > 2cm (pour un deplacement\n"
                 'a %2.2f m/s vers %d deg et une periode tourelle = %d ms)'%(speed, angle_deg, period))
 
-do_random_test()
-do_graph_2d_simple_error()
-do_graph_2d_move_error()
-do_graph_2d_ad_error()
+#do_random_test()
+#do_graph_2d_simple_error()
+#do_graph_2d_move_error()
+#do_graph_2d_ad_error()
+do_graph_2d_ad_error_mm()