1 import math, sys, time, os
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))
13 # all positions of robot every 5ms
16 robot = box(pos = (0, 0, 150),
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)]
28 sq.pos = [(-sz, -sz, 0.3),
42 def set_robot(x, y, a):
43 global robot, last_pos, robot_trail, robot_trail_list
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,
52 save_pos.append((robot.pos.x, robot.pos, a))
54 pos = robot.pos.x, robot.pos.y, 0.3
56 robot_trail_list.append(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
67 f = open("/tmp/robot_save", "w")
69 f.write("%f %f %f\n"%(p[0], p[1], p[2]))
79 silent_mkfifo("/tmp/.robot_sim2dis")
80 silent_mkfifo("/tmp/.robot_dis2sim")
82 fr = open("/tmp/.robot_sim2dis", "r")
83 fw = open("/tmp/.robot_dis2sim", "w", 0)
89 x,y,a = map(lambda x:int(x), l[:-1].split(" "))
94 if scene.kb.keys == 0:
100 scene.center = x-10,y,z
102 scene.center = x+10,y,z
104 scene.center = x,y+10,z
106 scene.center = x,y-10,z
112 robot_trail_list = []