1 import math, sys, time, os, random
10 area = [ (0.0, 0.0, -0.2), (3000.0, 2100.0, 0.2) ]
11 areasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , area)
12 area_box = box(size=areasize, color=(0.0, 1.0, 0.0))
16 # all positions of robot every 5ms
19 robot = box(pos = (0, 0, ROBOT_HEIGHT/2),
20 size = (250,320,ROBOT_HEIGHT),
21 color = (0.3, 0.3, 0.3) )
23 last_pos = robot.pos.x, robot.pos.y, robot.pos.z
24 hcenter_line = curve()
25 hcenter_line.pos = [(-AREA_X/2, 0., 0.3), (AREA_X/2, 0., 0.3)]
26 vcenter_line = curve()
27 vcenter_line.pos = [(0., -AREA_Y/2, 0.3), (0., AREA_Y/2, 0.3)]
29 yellowarea = [ (0.0, 0.0, -0.5), (500.0, 500.0, 0.5) ]
30 yellowareasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , yellowarea)
31 yellowarea_box = box(pos=(-AREA_X/2+250,-AREA_Y/2+250,0), size=yellowareasize, color=(1.0, 1.0, 0.0))
33 bluearea = [ (0.0, 0.0, -0.5), (500.0, 500.0, 0.5) ]
34 blueareasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , bluearea)
35 bluearea_box = box(pos=(AREA_X/2-250,-AREA_Y/2+250,0), size=blueareasize, color=(0.0, 0.0, 1.0))
37 greyarea = [ (0.0, 0.0, -0.5), (1520.0, 500.0, 0.5) ]
38 greyareasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , greyarea)
39 greyarea_box = box(pos=(0,-AREA_Y/2+250,0), size=greyareasize, color=(0.3, 0.6, 0.3))
43 sq.pos = [(-sz, -sz, 0.3),
75 col = [TYPE_WAYPOINT] * WAYPOINTS_NBY
76 waypoints = [col[:] for i in range(WAYPOINTS_NBX)]
77 corn_table = [TYPE_WHITE_CORN]*18
98 if i == 0 and j == 2: return 0
99 if i == 0 and j == 4: return 1
100 if i == 0 and j == 6: return 2
101 if i == 2 and j == 3: return 3
102 if i == 2 and j == 5: return 4
103 if i == 2 and j == 7: return 5
104 if i == 4 and j == 4: return 6
105 if i == 4 and j == 6: return 7
106 if i == 6 and j == 5: return 8
107 if i == 6 and j == 7: return 9
108 if i == 8 and j == 4: return 10
109 if i == 8 and j == 6: return 11
110 if i == 10 and j == 3: return 12
111 if i == 10 and j == 5: return 13
112 if i == 10 and j == 7: return 14
113 if i == 12 and j == 2: return 15
114 if i == 12 and j == 4: return 16
115 if i == 12 and j == 6: return 17
119 sym = [15, 16, 17, 12, 13, 14, 10, 11, 8, 9, 6, 7, 3, 4, 5, 0, 1, 2]
122 def init_corn_table(conf_side, conf_center):
123 global corn_table, corn_side_confs, corn_center_confs
124 print "confs = %d, %d"%(conf_side, conf_center)
126 if i in corn_side_confs[conf_side]:
127 corn_table[i] = TYPE_BLACK_CORN
129 if corn_get_sym(i) in corn_side_confs[conf_side]:
130 corn_table[i] = TYPE_BLACK_CORN
132 if i in corn_center_confs[conf_center]:
133 corn_table[i] = TYPE_BLACK_CORN
135 if corn_get_sym(i) in corn_center_confs[conf_center]:
136 corn_table[i] = TYPE_BLACK_CORN
138 corn_table[i] = TYPE_WHITE_CORN
140 def init_waypoints():
141 global waypoints, corn_table
143 for i in range(WAYPOINTS_NBX):
144 for j in range(WAYPOINTS_NBY):
148 waypoints[i][j] = corn_table[c]
152 if (i & 1) == 0 and j > 3:
153 waypoints[i][j] = TYPE_BALL
155 if (i == 0 or i == WAYPOINTS_NBX-1) and j > 2:
156 waypoints[i][j] = TYPE_BALL
159 # too close of border
160 if (i & 1) == 1 and j == WAYPOINTS_NBY -1:
161 waypoints[i][j] = TYPE_OBSTACLE
164 if i >= 2 and i < WAYPOINTS_NBX - 2 and j < 2:
165 waypoints[i][j] = TYPE_OBSTACLE
168 if i == 0 or i == WAYPOINTS_NBX-1:
169 waypoints[i][j] = TYPE_DANGEROUS
171 if (i&1) == 0 and j == WAYPOINTS_NBY-1:
172 waypoints[i][j] = TYPE_DANGEROUS
175 waypoints[i][j] = TYPE_WAYPOINT
177 print i, waypoints[i]
181 def toggle_obj_disp():
184 if area_objects == []:
192 y = OFFSET_CORN_Y + STEP_CORN_Y/2
196 if waypoints[i][j] == TYPE_WHITE_CORN:
197 c = cylinder(axis=(0,0,1), length=CORN_HEIGHT,
198 radius=25, color=(0.8,0.8,0.8),
199 pos=(x-AREA_X/2,y-AREA_Y/2,CORN_HEIGHT/2))
200 area_objects.append(c)
201 elif waypoints[i][j] == TYPE_BLACK_CORN:
202 c = cylinder(axis=(0,0,1), length=CORN_HEIGHT,
203 radius=25, color=(0.2,0.2,0.2),
204 pos=(x-AREA_X/2,y-AREA_Y/2,CORN_HEIGHT/2))
205 area_objects.append(c)
206 elif waypoints[i][j] == TYPE_BALL:
207 c = sphere(radius=50, color=(1., 0.,0.),
208 pos=(x-AREA_X/2,y-AREA_Y/2,50))
209 area_objects.append(c)
211 c = sphere(radius=5, color=(0., 0.,1.),
212 pos=(x-AREA_X/2,y-AREA_Y/2,5))
213 area_objects.append(c)
219 for o in area_objects:
226 def set_robot(x, y, a):
227 global robot, last_pos, robot_trail, robot_trail_list
230 robot.pos = (x - AREA_X/2, y - AREA_Y/2, ROBOT_HEIGHT/2)
231 robot.axis = (math.cos(a*math.pi/180),
232 math.sin(a*math.pi/180),
234 robot.size = (250, 320, ROBOT_HEIGHT)
237 save_pos.append((robot.pos.x, robot.pos, a))
239 pos = robot.pos.x, robot.pos.y, 0.3
241 robot_trail_list.append(pos)
243 robot_trail_l = len(robot_trail_list)
244 if robot_trail_l > max_trail:
245 robot_trail_list = robot_trail_list[robot_trail_l - max_trail:]
246 robot_trail.pos = robot_trail_list
252 f = open("/tmp/robot_save", "w")
254 f.write("%f %f %f\n"%(p[0], p[1], p[2]))
257 def silent_mkfifo(f):
263 init_corn_table(random.randint(0,8), random.randint(0,3))
264 waypoints = init_waypoints()
268 silent_mkfifo("/tmp/.robot_sim2dis")
269 silent_mkfifo("/tmp/.robot_dis2sim")
271 fr = open("/tmp/.robot_sim2dis", "r")
272 fw = open("/tmp/.robot_dis2sim", "w", 0)
276 x,y,a = map(lambda x:int(x), l[:-1].split(" "))
281 if scene.kb.keys == 0:
284 k = scene.kb.getkey()
287 scene.center = x-10,y,z
289 scene.center = x+10,y,z
291 scene.center = x,y+10,z
293 scene.center = x,y-10,z
299 robot_trail_list = []