projects/microb2010/tests/hostsim/display.py

   1 import math, sys, time, os
   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 = (300,300,300),
  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 sq1 = square(500)
  37
  38 robot_trail = curve()
  39 robot_trail_list = []
  40 max_trail = 500
  41
  42 def set_robot(x, y, a):
  43     global robot, last_pos, robot_trail, robot_trail_list
  44     global save_pos
  45
  46     robot.pos = (x - AREA_X/2, y - AREA_Y/2, 150)
  47     robot.axis = (math.cos(a*math.pi/180) * 300,
  48                   math.sin(a*math.pi/180) * 300,
  49                   0)
  50
  51     # save position
  52     save_pos.append(robot.pos.x, robot.pos, a)
  53
  54     pos = robot.pos.x, robot.pos.y, 0.3
  55     if pos != last_pos:
  56         robot_trail_list.append(pos)
  57         last_pos = pos
  58     robot_trail_l = len(robot_trail_list)
  59     if robot_trail_l > max_trail:
  60         robot_trail_list = robot_trail_list[robot_trail_l - max_trail:]
  61     robot_trail.pos = robot_trail_list
  62
  63 def graph():
  64     pass
  65
  66 def save():
  67     f = open("/tmp/robot_save", "w")
  68     for p in save_pos:
  69         f.write("%f %f %f\n"%(p[0], p[1], p[2]))
  70     f.close()
  71
  72 def silent_mkfifo(f):
  73     try:
  74         os.mkfifo(f)
  75     except:
  76         pass
  77
  78 while True:
  79     silent_mkfifo("/tmp/.robot_sim2dis")
  80     silent_mkfifo("/tmp/.robot_dis2sim")
  81     while True:
  82         fr = open("/tmp/.robot_sim2dis", "r")
  83         fw = open("/tmp/.robot_dis2sim", "w", 0)
  84         while True:
  85             l = fr.readline()
  86             if l == "":
  87                 break
  88             try:
  89                 x,y,a = map(lambda x:int(x), l[:-1].split(" "))
  90                 set_robot(x,y,a)
  91             except ValueError:
  92                 pass
  93
  94             if scene.kb.keys == 0:
  95                 continue
  96
  97             k = scene.kb.getkey()
  98             x,y,z = scene.center
  99             if k == "left":
 100                 scene.center = x-10,y,z
 101             elif k == "right":
 102                 scene.center = x+10,y,z
 103             elif k == "up":
 104                 scene.center = x,y+10,z
 105             elif k == "down":
 106                 scene.center = x,y-10,z
 107             elif k == "l":
 108                 fw.write("l")
 109             elif k == "r":
 110                 fw.write("r")
 111             elif k == "c":
 112                 robot_trail_list = []
 113             elif k == "x":
 114                 save_pos = []
 115             elif k == "g":
 116                 graph()
 117             elif k == "s":
 118                 save()
 119
 120         fr.close()
 121         fw.close()
 122