python display hostsim

diff --git a/projects/microb2010/tests/hostsim/display.py b/projects/microb2010/tests/hostsim/display.py
index fac65cf514edf95cbb3633dcfe905bc1bcb9a7ee..239b2d6a9f0aebd901e4e5a3a0a8946d5f108da2 100644 (file)
--- a/projects/microb2010/tests/hostsim/display.py
+++ b/projects/microb2010/tests/hostsim/display.py
@@ -1,4 +1,4 @@
-import math, sys, time, os
+import math, sys, time, os, random
 from visual import *
 
 AREA_X = 3000.
@@ -14,7 +14,7 @@ scene.autoscale=0
 save_pos = []
 
 robot = box(pos = (0, 0, 150),
-            size = (300,300,300),
+            size = (250,320,350),
             color = (1, 0, 0) )
 
 last_pos = robot.pos.x, robot.pos.y, robot.pos.z
@@ -39,14 +39,166 @@ robot_trail = curve()
 robot_trail_list = []
 max_trail = 500
 
+area_objects = []
+
+OFFSET_CORN_X=150
+OFFSET_CORN_Y=222
+STEP_CORN_X=225
+STEP_CORN_Y=250
+
+WAYPOINTS_NBX = 13
+WAYPOINTS_NBY = 8
+
+TYPE_WAYPOINT=0
+TYPE_DANGEROUS=1
+TYPE_WHITE_CORN=2
+TYPE_BLACK_CORN=3
+TYPE_OBSTACLE=4
+TYPE_NEIGH=5
+
+col = [TYPE_WAYPOINT] * WAYPOINTS_NBY
+waypoints = [col[:] for i in range(WAYPOINTS_NBX)]
+corn_table = [TYPE_WHITE_CORN]*18
+
+corn_side_confs = [
+    [ 1, 4 ],
+    [ 0, 4 ],
+    [ 2, 4 ],
+    [ 2, 3 ],
+    [ 0, 3 ],
+    [ 1, 3 ],
+    [ 1, 6 ],
+    [ 0, 6 ],
+    [ 2, 6 ],
+    ]
+corn_center_confs = [
+    [ 5, 8 ],
+    [ 7, 8 ],
+    [ 5, 9 ],
+    [ 7, 8 ],
+    ]
+
+def pt2corn(i,j):
+    if i == 0 and j == 2: return 0
+    if i == 0 and j == 4: return 1
+    if i == 0 and j == 6: return 2
+    if i == 2 and j == 3: return 3
+    if i == 2 and j == 5: return 4
+    if i == 2 and j == 7: return 5
+    if i == 4 and j == 4: return 6
+    if i == 4 and j == 6: return 7
+    if i == 6 and j == 5: return 8
+    if i == 6 and j == 7: return 9
+    if i == 8 and j == 4: return 10
+    if i == 8 and j == 6: return 11
+    if i == 10 and j == 3: return 12
+    if i == 10 and j == 5: return 13
+    if i == 10 and j == 7: return 14
+    if i == 12 and j == 2: return 15
+    if i == 12 and j == 4: return 16
+    if i == 12 and j == 6: return 17
+    return -1
+
+def corn_get_sym(i):
+    sym = [15, 16, 17, 12, 13, 14, 10, 11, 8, 9, 6, 7, 3, 4, 5, 0, 1, 2]
+    return sym[i]
+
+def init_corn_table(conf_side, conf_center):
+    global corn_table, corn_side_confs, corn_center_confs
+    print "confs = %d, %d"%(conf_side, conf_center)
+    for i in range(18):
+        if i in corn_side_confs[conf_side]:
+            corn_table[i] = TYPE_BLACK_CORN
+            continue
+        if corn_get_sym(i) in corn_side_confs[conf_side]:
+            corn_table[i] = TYPE_BLACK_CORN
+            continue
+        if i in corn_center_confs[conf_center]:
+            corn_table[i] = TYPE_BLACK_CORN
+            continue
+        if corn_get_sym(i) in corn_center_confs[conf_center]:
+            corn_table[i] = TYPE_BLACK_CORN
+            continue
+        corn_table[i] = TYPE_WHITE_CORN
+
+def init_waypoints():
+    global waypoints, corn_table
+
+    for i in range(WAYPOINTS_NBX):
+        for j in range(WAYPOINTS_NBY):
+            # corn
+            c = pt2corn(i, j)
+            if c >= 0:
+                waypoints[i][j] = corn_table[c]
+                continue
+            # too close of border
+            if (i & 1) == 1 and j == WAYPOINTS_NBY -1:
+                waypoints[i][j] = TYPE_OBSTACLE
+                continue
+            # hill
+            if i >= 2 and i < WAYPOINTS_NBX - 2 and j < 2:
+                waypoints[i][j] = TYPE_OBSTACLE
+                continue
+            # dangerous points
+            if i == 0 or i == WAYPOINTS_NBX-1:
+                waypoints[i][j] = TYPE_DANGEROUS
+                continue
+            if (i&1) == 0 and j == WAYPOINTS_NBY-1:
+                waypoints[i][j] = TYPE_DANGEROUS
+                continue
+
+            waypoints[i][j] = TYPE_WAYPOINT
+
+        print i, waypoints[i]
+    return waypoints
+
+
+def toggle_obj_disp():
+    global area_objects
+
+    if area_objects == []:
+        i = 0
+        j = 0
+        x = OFFSET_CORN_X
+        while x < 3000:
+            if (i & 1) == 0:
+                y = OFFSET_CORN_Y
+            else:
+                y = OFFSET_CORN_Y + STEP_CORN_Y/2
+            j = 0
+            while y < 2100:
+                print x,y
+                if waypoints[i][j] == TYPE_WHITE_CORN:
+                    c = cylinder(axis=(0,0,1), length=150,
+                                 radius=25, color=(0.8,0.8,0.8),
+                                 pos=(x-AREA_X/2,y-AREA_Y/2,75))
+                    area_objects.append(c)
+                elif waypoints[i][j] == TYPE_BLACK_CORN:
+                    c = cylinder(axis=(0,0,1), length=150,
+                                 radius=25, color=(0.2,0.2,0.2),
+                                 pos=(x-AREA_X/2,y-AREA_Y/2,75))
+                    area_objects.append(c)
+                j += 1
+                y += STEP_CORN_Y
+            i += 1
+            x += STEP_CORN_X
+    else:
+        for o in area_objects:
+            if o.visible:
+                o.visible = 0
+            else:
+                o.visible = 1
+
+
 def set_robot(x, y, a):
     global robot, last_pos, robot_trail, robot_trail_list
     global save_pos
 
     robot.pos = (x - AREA_X/2, y - AREA_Y/2, 150)
-    robot.axis = (math.cos(a*math.pi/180) * 300,
-                  math.sin(a*math.pi/180) * 300,
+    robot.axis = (math.cos(a*math.pi/180),
+                  math.sin(a*math.pi/180),
                   0)
+    robot.size = (250, 320, 350)
 
     # save position
     save_pos.append((robot.pos.x, robot.pos, a))
@@ -75,6 +227,10 @@ def silent_mkfifo(f):
     except:
         pass
 
+init_corn_table(random.randint(0,8), random.randint(0,3))
+waypoints = init_waypoints()
+toggle_obj_disp()
+
 while True:
     silent_mkfifo("/tmp/.robot_sim2dis")
     silent_mkfifo("/tmp/.robot_dis2sim")
@@ -83,8 +239,6 @@ while True:
         fw = open("/tmp/.robot_dis2sim", "w", 0)
         while True:
             l = fr.readline()
-            if l == "":
-                break
             try:
                 x,y,a = map(lambda x:int(x), l[:-1].split(" "))
                 set_robot(x,y,a)
@@ -116,6 +270,12 @@ while True:
                 graph()
             elif k == "s":
                 save()
+            elif k == "h":
+                toggle_obj_disp()
+
+            # EOF
+            if l == "":
+                break
 
         fr.close()
         fw.close()