1 import math, sys, time, os, random, re
4 FLOAT = "([-+]?[0-9]*\.?[0-9]+)"
5 INT = "([-+]?[0-9][0-9]*)"
17 area = [ (0.0, 0.0, -0.2), (3000.0, 2100.0, 0.2) ]
18 areasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , area)
19 area_box = box(size=areasize, color=(0.0, 1.0, 0.0))
23 # all positions of robot every 5ms
26 robot = box(color=(0.4, 0.4, 0.4))
27 lspickle = box(color=(0.4, 0.4, 0.4))
28 rspickle = box(color=(0.4, 0.4, 0.4))
30 opp = box(color=(0.7, 0.2, 0.2))
33 hcenter_line = curve()
34 hcenter_line.pos = [(-AREA_X/2, 0., 0.3), (AREA_X/2, 0., 0.3)]
35 vcenter_line = curve()
36 vcenter_line.pos = [(0., -AREA_Y/2, 0.3), (0., AREA_Y/2, 0.3)]
38 yellowarea = [ (0.0, 0.0, -0.5), (500.0, 500.0, 0.5) ]
39 yellowareasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , yellowarea)
40 yellowarea_box = box(pos=(-AREA_X/2+250,-AREA_Y/2+250,0), size=yellowareasize, color=(1.0, 1.0, 0.0))
42 bluearea = [ (0.0, 0.0, -0.5), (500.0, 500.0, 0.5) ]
43 blueareasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , bluearea)
44 bluearea_box = box(pos=(AREA_X/2-250,-AREA_Y/2+250,0), size=blueareasize, color=(0.0, 0.0, 1.0))
46 greyarea = [ (0.0, 0.0, -0.5), (1520.0, 500.0, 0.5) ]
47 greyareasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , greyarea)
48 greyarea_box = box(pos=(0,-AREA_Y/2+250,0), size=greyareasize, color=(0.3, 0.6, 0.3))
56 sq.pos = [(-sz, -sz, 0.3),
69 robot_lspickle_deployed = 0
70 robot_rspickle_deployed = 0
71 robot_lspickle_autoharvest = 0
72 robot_rspickle_autoharvest = 0
95 col = [TYPE_WAYPOINT] * WAYPOINTS_NBY
96 waypoints = [col[:] for i in range(WAYPOINTS_NBX)]
97 corn_table = [TYPE_WHITE_CORN]*18
110 corn_center_confs = [
118 if i == 0 and j == 2: return 0
119 if i == 0 and j == 4: return 1
120 if i == 0 and j == 6: return 2
121 if i == 2 and j == 3: return 3
122 if i == 2 and j == 5: return 4
123 if i == 2 and j == 7: return 5
124 if i == 4 and j == 4: return 6
125 if i == 4 and j == 6: return 7
126 if i == 6 and j == 5: return 8
127 if i == 6 and j == 7: return 9
128 if i == 8 and j == 4: return 10
129 if i == 8 and j == 6: return 11
130 if i == 10 and j == 3: return 12
131 if i == 10 and j == 5: return 13
132 if i == 10 and j == 7: return 14
133 if i == 12 and j == 2: return 15
134 if i == 12 and j == 4: return 16
135 if i == 12 and j == 6: return 17
139 sym = [15, 16, 17, 12, 13, 14, 10, 11, 8, 9, 6, 7, 3, 4, 5, 0, 1, 2]
142 def init_corn_table(conf_side, conf_center):
143 global corn_table, corn_side_confs, corn_center_confs
144 print "confs = %d, %d"%(conf_side, conf_center)
146 if i in corn_side_confs[conf_side]:
147 corn_table[i] = TYPE_BLACK_CORN
149 if corn_get_sym(i) in corn_side_confs[conf_side]:
150 corn_table[i] = TYPE_BLACK_CORN
152 if i in corn_center_confs[conf_center]:
153 corn_table[i] = TYPE_BLACK_CORN
155 if corn_get_sym(i) in corn_center_confs[conf_center]:
156 corn_table[i] = TYPE_BLACK_CORN
158 corn_table[i] = TYPE_WHITE_CORN
160 def init_waypoints():
161 global waypoints, corn_table
163 for i in range(WAYPOINTS_NBX):
164 for j in range(WAYPOINTS_NBY):
168 waypoints[i][j] = corn_table[c]
172 if (i & 1) == 0 and j > 3 and \
173 (not (i == 0 and j == WAYPOINTS_NBY-1)) and \
174 (not (i == WAYPOINTS_NBX-1 and j == WAYPOINTS_NBY-1)):
175 waypoints[i][j] = TYPE_BALL
177 if (i == 0 or i == WAYPOINTS_NBX-1) and j > 2 and \
178 (not (i == 0 and j == WAYPOINTS_NBY-1)) and \
179 (not (i == WAYPOINTS_NBX-1 and j == WAYPOINTS_NBY-1)):
180 waypoints[i][j] = TYPE_BALL
183 # too close of border
184 if (i & 1) == 1 and j == WAYPOINTS_NBY -1:
185 waypoints[i][j] = TYPE_OBSTACLE
188 if i >= 2 and i < WAYPOINTS_NBX - 2 and j < 2:
189 waypoints[i][j] = TYPE_OBSTACLE
192 if i == 0 or i == WAYPOINTS_NBX-1:
193 waypoints[i][j] = TYPE_DANGEROUS
195 if (i&1) == 0 and j == WAYPOINTS_NBY-1:
196 waypoints[i][j] = TYPE_DANGEROUS
199 waypoints[i][j] = TYPE_WAYPOINT
201 print i, waypoints[i]
205 def toggle_obj_disp():
209 if area_objects == []:
210 c = sphere(radius=5, color=(0., 0.,1.),
211 pos=(1238.-AREA_X/2, 1313.-AREA_Y/2, 5))
212 area_objects.append(c)
213 c = sphere(radius=5, color=(0., 0.,1.),
214 pos=(1364.-AREA_X/2, 1097.-AREA_Y/2, 5))
215 area_objects.append(c)
216 c = sphere(radius=5, color=(0., 0.,1.),
217 pos=(1453.-AREA_X/2, 1176.-AREA_Y/2, 5))
218 area_objects.append(c)
219 c = sphere(radius=5, color=(0., 0.,1.),
220 pos=(1109.-AREA_X/2, 1050.-AREA_Y/2, 5))
221 area_objects.append(c)
223 if area_objects == []:
231 y = OFFSET_CORN_Y + STEP_CORN_Y/2
235 if waypoints[i][j] == TYPE_WHITE_CORN:
236 c = cylinder(axis=(0,0,1), length=CORN_HEIGHT,
237 radius=25, color=(0.8,0.8,0.8),
238 pos=(x-AREA_X/2,y-AREA_Y/2,CORN_HEIGHT/2))
239 area_objects.append(c)
240 elif waypoints[i][j] == TYPE_BLACK_CORN:
241 c = cylinder(axis=(0,0,1), length=CORN_HEIGHT,
242 radius=25, color=(0.2,0.2,0.2),
243 pos=(x-AREA_X/2,y-AREA_Y/2,CORN_HEIGHT/2))
244 area_objects.append(c)
245 elif waypoints[i][j] == TYPE_BALL:
246 if BALL_CYLINDER == 1:
247 c = cylinder(axis=(0,0,1), radius=50,
250 pos=(x-AREA_X/2,y-AREA_Y/2,CORN_HEIGHT/2))
252 c = sphere(radius=50, color=(1., 0.,0.),
253 pos=(x-AREA_X/2,y-AREA_Y/2,50))
255 area_objects.append(c)
257 c = sphere(radius=5, color=(0., 0.,1.),
258 pos=(x-AREA_X/2,y-AREA_Y/2,5))
259 area_objects.append(c)
265 for o in area_objects:
280 opp.size = (300, 300, ROBOT_HEIGHT)
281 opp.pos = (x, y, ROBOT_HEIGHT/2)
284 global robot, last_pos, robot_trail, robot_trail_list
285 global save_pos, robot_x, robot_y, robot_a
288 tmp_x = robot_x - AREA_X/2
289 tmp_y = robot_y - AREA_Y/2
292 tmp_x = -robot_x + AREA_X/2
293 tmp_y = -robot_y + AREA_Y/2
296 robot.pos = (tmp_x, tmp_y, ROBOT_HEIGHT/2)
297 axis = (math.cos(tmp_a*math.pi/180),
298 math.sin(tmp_a*math.pi/180),
302 robot.size = (ROBOT_LENGTH, ROBOT_WIDTH, ROBOT_HEIGHT)
304 lspickle.pos = (tmp_x + (robot_lspickle_deployed*60) * math.cos((tmp_a+90)*math.pi/180),
305 tmp_y + (robot_lspickle_deployed*60) * math.sin((tmp_a+90)*math.pi/180),
308 lspickle.size = (20, ROBOT_WIDTH, 5)
309 if robot_lspickle_autoharvest:
310 lspickle.color = (1, 0, 0)
312 lspickle.color = (0.4, 0.4, 0.4)
314 rspickle.pos = (tmp_x + (robot_rspickle_deployed*60) * math.cos((tmp_a-90)*math.pi/180),
315 tmp_y + (robot_rspickle_deployed*60) * math.sin((tmp_a-90)*math.pi/180),
318 rspickle.size = (20, ROBOT_WIDTH, 5)
319 if robot_rspickle_autoharvest:
320 rspickle.color = (1, 0, 0)
322 rspickle.color = (0.4, 0.4, 0.4)
325 save_pos.append((robot.pos.x, robot.pos.y, tmp_a))
327 pos = robot.pos.x, robot.pos.y, 0.3
329 robot_trail_list.append(pos)
331 robot_trail_l = len(robot_trail_list)
332 if robot_trail_l > max_trail:
333 robot_trail_list = robot_trail_list[robot_trail_l - max_trail:]
334 robot_trail.pos = robot_trail_list
340 f = open("/tmp/robot_save", "w")
342 f.write("%f %f %f\n"%(p[0], p[1], p[2]))
345 def silent_mkfifo(f):
351 #init_corn_table(random.randint(0,8), random.randint(0,3))
352 init_corn_table(0, 0)
353 waypoints = init_waypoints()
357 silent_mkfifo("/tmp/.robot_sim2dis")
358 silent_mkfifo("/tmp/.robot_dis2sim")
360 fr = open("/tmp/.robot_sim2dis", "r")
361 fw = open("/tmp/.robot_dis2sim", "w", 0)
368 m = re.match("pos=%s,%s,%s"%(INT,INT,INT), l)
370 robot_x = int(m.groups()[0])
371 robot_y = int(m.groups()[1])
372 robot_a = int(m.groups()[2])
377 m = re.match("ballboard=%s"%(INT), l)
379 print "ballboard: %d"%(int(m.groups()[0]))
383 m = re.match("cobboard=%s,%s"%(INT,INT), l)
385 #print "cobboard: %x,%x"%(int(m.groups()[0]),int(m.groups()[1]))
386 side = int(m.groups()[0])
387 flags = int(m.groups()[1])
389 robot_lspickle_deployed = ((flags & 1) * 2)
390 robot_lspickle_autoharvest = ((flags & 2) != 0)
392 robot_rspickle_deployed = ((flags & 1) * 2)
393 robot_rspickle_autoharvest = ((flags & 2) != 0)
395 if scene.mouse.events != 0:
396 oppx, oppy, oppz = scene.mouse.getevent().project(normal=(0,0,1))
400 fw.write("opp %d %d"%(int(oppx + 1500), int(oppy + 1050)))
402 fw.write("opp %d %d"%(int(1500 - oppx), int(1050 - oppy)))
404 print "not connected"
406 if scene.kb.keys == 0:
409 k = scene.kb.getkey()
412 scene.center = x-10,y,z
414 scene.center = x+10,y,z
416 scene.center = x,y+10,z
418 scene.center = x,y-10,z
424 robot_trail_list = []