1 import math, sys, time, os, random, re
4 FLOAT = "([-+]?[0-9]*\.?[0-9]+)"
5 INT = "([-+]?[0-9][0-9]*)"
13 area = [ (0.0, 0.0, -0.2), (3000.0, 2100.0, 0.2) ]
14 areasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , area)
15 area_box = box(size=areasize, color=(0.0, 1.0, 0.0))
19 # all positions of robot every 5ms
22 robot = box(color=(0.4, 0.4, 0.4))
23 lspickle = box(color=(0.4, 0.4, 0.4))
24 rspickle = box(color=(0.4, 0.4, 0.4))
27 hcenter_line = curve()
28 hcenter_line.pos = [(-AREA_X/2, 0., 0.3), (AREA_X/2, 0., 0.3)]
29 vcenter_line = curve()
30 vcenter_line.pos = [(0., -AREA_Y/2, 0.3), (0., AREA_Y/2, 0.3)]
32 yellowarea = [ (0.0, 0.0, -0.5), (500.0, 500.0, 0.5) ]
33 yellowareasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , yellowarea)
34 yellowarea_box = box(pos=(-AREA_X/2+250,-AREA_Y/2+250,0), size=yellowareasize, color=(1.0, 1.0, 0.0))
36 bluearea = [ (0.0, 0.0, -0.5), (500.0, 500.0, 0.5) ]
37 blueareasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , bluearea)
38 bluearea_box = box(pos=(AREA_X/2-250,-AREA_Y/2+250,0), size=blueareasize, color=(0.0, 0.0, 1.0))
40 greyarea = [ (0.0, 0.0, -0.5), (1520.0, 500.0, 0.5) ]
41 greyareasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , greyarea)
42 greyarea_box = box(pos=(0,-AREA_Y/2+250,0), size=greyareasize, color=(0.3, 0.6, 0.3))
46 sq.pos = [(-sz, -sz, 0.3),
83 col = [TYPE_WAYPOINT] * WAYPOINTS_NBY
84 waypoints = [col[:] for i in range(WAYPOINTS_NBX)]
85 corn_table = [TYPE_WHITE_CORN]*18
106 if i == 0 and j == 2: return 0
107 if i == 0 and j == 4: return 1
108 if i == 0 and j == 6: return 2
109 if i == 2 and j == 3: return 3
110 if i == 2 and j == 5: return 4
111 if i == 2 and j == 7: return 5
112 if i == 4 and j == 4: return 6
113 if i == 4 and j == 6: return 7
114 if i == 6 and j == 5: return 8
115 if i == 6 and j == 7: return 9
116 if i == 8 and j == 4: return 10
117 if i == 8 and j == 6: return 11
118 if i == 10 and j == 3: return 12
119 if i == 10 and j == 5: return 13
120 if i == 10 and j == 7: return 14
121 if i == 12 and j == 2: return 15
122 if i == 12 and j == 4: return 16
123 if i == 12 and j == 6: return 17
127 sym = [15, 16, 17, 12, 13, 14, 10, 11, 8, 9, 6, 7, 3, 4, 5, 0, 1, 2]
130 def init_corn_table(conf_side, conf_center):
131 global corn_table, corn_side_confs, corn_center_confs
132 print "confs = %d, %d"%(conf_side, conf_center)
134 if i in corn_side_confs[conf_side]:
135 corn_table[i] = TYPE_BLACK_CORN
137 if corn_get_sym(i) in corn_side_confs[conf_side]:
138 corn_table[i] = TYPE_BLACK_CORN
140 if i in corn_center_confs[conf_center]:
141 corn_table[i] = TYPE_BLACK_CORN
143 if corn_get_sym(i) in corn_center_confs[conf_center]:
144 corn_table[i] = TYPE_BLACK_CORN
146 corn_table[i] = TYPE_WHITE_CORN
148 def init_waypoints():
149 global waypoints, corn_table
151 for i in range(WAYPOINTS_NBX):
152 for j in range(WAYPOINTS_NBY):
156 waypoints[i][j] = corn_table[c]
160 if (i & 1) == 0 and j > 3:
161 waypoints[i][j] = TYPE_BALL
163 if (i == 0 or i == WAYPOINTS_NBX-1) and j > 2:
164 waypoints[i][j] = TYPE_BALL
167 # too close of border
168 if (i & 1) == 1 and j == WAYPOINTS_NBY -1:
169 waypoints[i][j] = TYPE_OBSTACLE
172 if i >= 2 and i < WAYPOINTS_NBX - 2 and j < 2:
173 waypoints[i][j] = TYPE_OBSTACLE
176 if i == 0 or i == WAYPOINTS_NBX-1:
177 waypoints[i][j] = TYPE_DANGEROUS
179 if (i&1) == 0 and j == WAYPOINTS_NBY-1:
180 waypoints[i][j] = TYPE_DANGEROUS
183 waypoints[i][j] = TYPE_WAYPOINT
185 print i, waypoints[i]
189 def toggle_obj_disp():
193 if area_objects == []:
194 c = sphere(radius=5, color=(0., 0.,1.),
195 pos=(1238.-AREA_X/2, 1313.-AREA_Y/2, 5))
196 area_objects.append(c)
197 c = sphere(radius=5, color=(0., 0.,1.),
198 pos=(1364.-AREA_X/2, 1097.-AREA_Y/2, 5))
199 area_objects.append(c)
200 c = sphere(radius=5, color=(0., 0.,1.),
201 pos=(1453.-AREA_X/2, 1176.-AREA_Y/2, 5))
202 area_objects.append(c)
203 c = sphere(radius=5, color=(0., 0.,1.),
204 pos=(1109.-AREA_X/2, 1050.-AREA_Y/2, 5))
205 area_objects.append(c)
207 if area_objects == []:
215 y = OFFSET_CORN_Y + STEP_CORN_Y/2
219 if waypoints[i][j] == TYPE_WHITE_CORN:
220 c = cylinder(axis=(0,0,1), length=CORN_HEIGHT,
221 radius=25, color=(0.8,0.8,0.8),
222 pos=(x-AREA_X/2,y-AREA_Y/2,CORN_HEIGHT/2))
223 area_objects.append(c)
224 elif waypoints[i][j] == TYPE_BLACK_CORN:
225 c = cylinder(axis=(0,0,1), length=CORN_HEIGHT,
226 radius=25, color=(0.2,0.2,0.2),
227 pos=(x-AREA_X/2,y-AREA_Y/2,CORN_HEIGHT/2))
228 area_objects.append(c)
229 elif waypoints[i][j] == TYPE_BALL:
230 c = sphere(radius=50, color=(1., 0.,0.),
231 pos=(x-AREA_X/2,y-AREA_Y/2,50))
232 area_objects.append(c)
234 c = sphere(radius=5, color=(0., 0.,1.),
235 pos=(x-AREA_X/2,y-AREA_Y/2,5))
236 area_objects.append(c)
242 for o in area_objects:
250 global robot, last_pos, robot_trail, robot_trail_list
251 global save_pos, robot_x, robot_y, robot_a
253 axis = (math.cos(robot_a*math.pi/180),
254 math.sin(robot_a*math.pi/180),
257 robot.pos = (robot_x - AREA_X/2, robot_y - AREA_Y/2, ROBOT_HEIGHT/2)
259 robot.size = (250, 320, ROBOT_HEIGHT)
261 lspickle.pos = (robot_x - AREA_X/2 + (robot_lspickle*60) * math.cos((robot_a+90)*math.pi/180),
262 robot_y - AREA_Y/2 + (robot_lspickle*60) * math.sin((robot_a+90)*math.pi/180),
265 lspickle.size = (20, 320, 5)
267 rspickle.pos = (robot_x - AREA_X/2 + (robot_rspickle*60) * math.cos((robot_a-90)*math.pi/180),
268 robot_y - AREA_Y/2 + (robot_rspickle*60) * math.sin((robot_a-90)*math.pi/180),
271 rspickle.size = (20, 320, 5)
274 save_pos.append((robot.pos.x, robot.pos, robot_a))
276 pos = robot.pos.x, robot.pos.y, 0.3
278 robot_trail_list.append(pos)
280 robot_trail_l = len(robot_trail_list)
281 if robot_trail_l > max_trail:
282 robot_trail_list = robot_trail_list[robot_trail_l - max_trail:]
283 robot_trail.pos = robot_trail_list
289 f = open("/tmp/robot_save", "w")
291 f.write("%f %f %f\n"%(p[0], p[1], p[2]))
294 def silent_mkfifo(f):
300 #init_corn_table(random.randint(0,8), random.randint(0,3))
301 init_corn_table(0, 0)
302 waypoints = init_waypoints()
306 silent_mkfifo("/tmp/.robot_sim2dis")
307 silent_mkfifo("/tmp/.robot_dis2sim")
309 fr = open("/tmp/.robot_sim2dis", "r")
310 fw = open("/tmp/.robot_dis2sim", "w", 0)
317 m = re.match("pos=%s,%s,%s"%(INT,INT,INT), l)
319 robot_x = int(m.groups()[0])
320 robot_y = int(m.groups()[1])
321 robot_a = int(m.groups()[2])
326 m = re.match("ballboard=%s"%(INT), l)
328 print int(m.groups()[0])
332 m = re.match("cobboard=%s"%(INT), l)
334 mode = int(m.groups()[0])
335 if (mode & 0x01) == 0:
337 elif (mode & 0x02) == 0:
341 if (mode & 0x04) == 0:
343 elif (mode & 0x08) == 0:
348 if scene.kb.keys == 0:
351 k = scene.kb.getkey()
354 scene.center = x-10,y,z
356 scene.center = x+10,y,z
358 scene.center = x,y+10,z
360 scene.center = x,y-10,z
366 robot_trail_list = []