projects/microb2010/tests/hostsim/display.py

   1 import math, sys, time, os, random
   2 from visual import *
   3
   4 AREA_X = 3000.
   5 AREA_Y = 2100.
   6
   7 area = [ (0.0, 0.0, -0.2), (3000.0, 2100.0, 0.2) ]
   8 areasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , area)
   9 area_box = box(size=areasize, color=(0.0, 1.0, 0.0))
  10
  11 scene.autoscale=0
  12
  13 # all positions of robot every 5ms
  14 save_pos = []
  15
  16 robot = box(pos = (0, 0, 150),
  17             size = (250,320,350),
  18             color = (1, 0, 0) )
  19
  20 last_pos = robot.pos.x, robot.pos.y, robot.pos.z
  21 hcenter_line = curve()
  22 hcenter_line.pos = [(-AREA_X/2, 0., 0.3), (AREA_X/2, 0., 0.3)]
  23 vcenter_line = curve()
  24 vcenter_line.pos = [(0., -AREA_Y/2, 0.3), (0., AREA_Y/2, 0.3)]
  25
  26 def square(sz):
  27     sq = curve()
  28     sq.pos = [(-sz, -sz, 0.3),
  29               (-sz, sz, 0.3),
  30               (sz, sz, 0.3),
  31               (sz, -sz, 0.3),
  32               (-sz, -sz, 0.3),]
  33     return sq
  34
  35 sq1 = square(250)
  36 sq2 = square(500)
  37
  38 robot_trail = curve()
  39 robot_trail_list = []
  40 max_trail = 500
  41
  42 area_objects = []
  43
  44 OFFSET_CORN_X=150
  45 OFFSET_CORN_Y=222
  46 STEP_CORN_X=225
  47 STEP_CORN_Y=250
  48
  49 WAYPOINTS_NBX = 13
  50 WAYPOINTS_NBY = 8
  51
  52 TYPE_WAYPOINT=0
  53 TYPE_DANGEROUS=1
  54 TYPE_WHITE_CORN=2
  55 TYPE_BLACK_CORN=3
  56 TYPE_OBSTACLE=4
  57 TYPE_NEIGH=5
  58
  59 col = [TYPE_WAYPOINT] * WAYPOINTS_NBY
  60 waypoints = [col[:] for i in range(WAYPOINTS_NBX)]
  61 corn_table = [TYPE_WHITE_CORN]*18
  62
  63 corn_side_confs = [
  64     [ 1, 4 ],
  65     [ 0, 4 ],
  66     [ 2, 4 ],
  67     [ 2, 3 ],
  68     [ 0, 3 ],
  69     [ 1, 3 ],
  70     [ 1, 6 ],
  71     [ 0, 6 ],
  72     [ 2, 6 ],
  73     ]
  74 corn_center_confs = [
  75     [ 5, 8 ],
  76     [ 7, 8 ],
  77     [ 5, 9 ],
  78     [ 7, 8 ],
  79     ]
  80
  81 def pt2corn(i,j):
  82     if i == 0 and j == 2: return 0
  83     if i == 0 and j == 4: return 1
  84     if i == 0 and j == 6: return 2
  85     if i == 2 and j == 3: return 3
  86     if i == 2 and j == 5: return 4
  87     if i == 2 and j == 7: return 5
  88     if i == 4 and j == 4: return 6
  89     if i == 4 and j == 6: return 7
  90     if i == 6 and j == 5: return 8
  91     if i == 6 and j == 7: return 9
  92     if i == 8 and j == 4: return 10
  93     if i == 8 and j == 6: return 11
  94     if i == 10 and j == 3: return 12
  95     if i == 10 and j == 5: return 13
  96     if i == 10 and j == 7: return 14
  97     if i == 12 and j == 2: return 15
  98     if i == 12 and j == 4: return 16
  99     if i == 12 and j == 6: return 17
 100     return -1
 101
 102 def corn_get_sym(i):
 103     sym = [15, 16, 17, 12, 13, 14, 10, 11, 8, 9, 6, 7, 3, 4, 5, 0, 1, 2]
 104     return sym[i]
 105
 106 def init_corn_table(conf_side, conf_center):
 107     global corn_table, corn_side_confs, corn_center_confs
 108     print "confs = %d, %d"%(conf_side, conf_center)
 109     for i in range(18):
 110         if i in corn_side_confs[conf_side]:
 111             corn_table[i] = TYPE_BLACK_CORN
 112             continue
 113         if corn_get_sym(i) in corn_side_confs[conf_side]:
 114             corn_table[i] = TYPE_BLACK_CORN
 115             continue
 116         if i in corn_center_confs[conf_center]:
 117             corn_table[i] = TYPE_BLACK_CORN
 118             continue
 119         if corn_get_sym(i) in corn_center_confs[conf_center]:
 120             corn_table[i] = TYPE_BLACK_CORN
 121             continue
 122         corn_table[i] = TYPE_WHITE_CORN
 123
 124 def init_waypoints():
 125     global waypoints, corn_table
 126
 127     for i in range(WAYPOINTS_NBX):
 128         for j in range(WAYPOINTS_NBY):
 129             # corn
 130             c = pt2corn(i, j)
 131             if c >= 0:
 132                 waypoints[i][j] = corn_table[c]
 133                 continue
 134             # too close of border
 135             if (i & 1) == 1 and j == WAYPOINTS_NBY -1:
 136                 waypoints[i][j] = TYPE_OBSTACLE
 137                 continue
 138             # hill
 139             if i >= 2 and i < WAYPOINTS_NBX - 2 and j < 2:
 140                 waypoints[i][j] = TYPE_OBSTACLE
 141                 continue
 142             # dangerous points
 143             if i == 0 or i == WAYPOINTS_NBX-1:
 144                 waypoints[i][j] = TYPE_DANGEROUS
 145                 continue
 146             if (i&1) == 0 and j == WAYPOINTS_NBY-1:
 147                 waypoints[i][j] = TYPE_DANGEROUS
 148                 continue
 149
 150             waypoints[i][j] = TYPE_WAYPOINT
 151
 152         print i, waypoints[i]
 153     return waypoints
 154
 155
 156 def toggle_obj_disp():
 157     global area_objects
 158
 159     if area_objects == []:
 160         i = 0
 161         j = 0
 162         x = OFFSET_CORN_X
 163         while x < 3000:
 164             if (i & 1) == 0:
 165                 y = OFFSET_CORN_Y
 166             else:
 167                 y = OFFSET_CORN_Y + STEP_CORN_Y/2
 168             j = 0
 169             while y < 2100:
 170                 print x,y
 171                 if waypoints[i][j] == TYPE_WHITE_CORN:
 172                     c = cylinder(axis=(0,0,1), length=150,
 173                                  radius=25, color=(0.8,0.8,0.8),
 174                                  pos=(x-AREA_X/2,y-AREA_Y/2,75))
 175                     area_objects.append(c)
 176                 elif waypoints[i][j] == TYPE_BLACK_CORN:
 177                     c = cylinder(axis=(0,0,1), length=150,
 178                                  radius=25, color=(0.2,0.2,0.2),
 179                                  pos=(x-AREA_X/2,y-AREA_Y/2,75))
 180                     area_objects.append(c)
 181                 j += 1
 182                 y += STEP_CORN_Y
 183             i += 1
 184             x += STEP_CORN_X
 185     else:
 186         for o in area_objects:
 187             if o.visible:
 188                 o.visible = 0
 189             else:
 190                 o.visible = 1
 191
 192
 193 def set_robot(x, y, a):
 194     global robot, last_pos, robot_trail, robot_trail_list
 195     global save_pos
 196
 197     robot.pos = (x - AREA_X/2, y - AREA_Y/2, 150)
 198     robot.axis = (math.cos(a*math.pi/180),
 199                   math.sin(a*math.pi/180),
 200                   0)
 201     robot.size = (250, 320, 350)
 202
 203     # save position
 204     save_pos.append((robot.pos.x, robot.pos, a))
 205
 206     pos = robot.pos.x, robot.pos.y, 0.3
 207     if pos != last_pos:
 208         robot_trail_list.append(pos)
 209         last_pos = pos
 210     robot_trail_l = len(robot_trail_list)
 211     if robot_trail_l > max_trail:
 212         robot_trail_list = robot_trail_list[robot_trail_l - max_trail:]
 213     robot_trail.pos = robot_trail_list
 214
 215 def graph():
 216     pass
 217
 218 def save():
 219     f = open("/tmp/robot_save", "w")
 220     for p in save_pos:
 221         f.write("%f %f %f\n"%(p[0], p[1], p[2]))
 222     f.close()
 223
 224 def silent_mkfifo(f):
 225     try:
 226         os.mkfifo(f)
 227     except:
 228         pass
 229
 230 init_corn_table(random.randint(0,8), random.randint(0,3))
 231 waypoints = init_waypoints()
 232 toggle_obj_disp()
 233
 234 while True:
 235     silent_mkfifo("/tmp/.robot_sim2dis")
 236     silent_mkfifo("/tmp/.robot_dis2sim")
 237     while True:
 238         fr = open("/tmp/.robot_sim2dis", "r")
 239         fw = open("/tmp/.robot_dis2sim", "w", 0)
 240         while True:
 241             l = fr.readline()
 242             try:
 243                 x,y,a = map(lambda x:int(x), l[:-1].split(" "))
 244                 set_robot(x,y,a)
 245             except ValueError:
 246                 pass
 247
 248             if scene.kb.keys == 0:
 249                 continue
 250
 251             k = scene.kb.getkey()
 252             x,y,z = scene.center
 253             if k == "left":
 254                 scene.center = x-10,y,z
 255             elif k == "right":
 256                 scene.center = x+10,y,z
 257             elif k == "up":
 258                 scene.center = x,y+10,z
 259             elif k == "down":
 260                 scene.center = x,y-10,z
 261             elif k == "l":
 262                 fw.write("l")
 263             elif k == "r":
 264                 fw.write("r")
 265             elif k == "c":
 266                 robot_trail_list = []
 267             elif k == "x":
 268                 save_pos = []
 269             elif k == "g":
 270                 graph()
 271             elif k == "s":
 272                 save()
 273             elif k == "h":
 274                 toggle_obj_disp()
 275
 276             # EOF
 277             if l == "":
 278                 break
 279
 280         fr.close()
 281         fw.close()
 282