1 import math, sys, time, os, random, re
2 from visual import *
4 FLOAT = "([-+]?[0-9]*\.?[0-9]+)"
5 INT = "([-+]?[0-9][0-9]*)"
7 AREA_X = 3000.
8 AREA_Y = 2100.
10 ROBOT_HEIGHT=5 # 350
11 CORN_HEIGHT=5  # 150
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))
17 scene.autoscale=0
19 # all positions of robot every 5ms
20 save_pos = []
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))
26 last_pos = (0.,0.,0.)
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))
44 YELLOW = 0
45 BLUE = 1
46 color = YELLOW
48 def square(sz):
49     sq = curve()
50     sq.pos = [(-sz, -sz, 0.3),
51               (-sz, sz, 0.3),
52               (sz, sz, 0.3),
53               (sz, -sz, 0.3),
54               (-sz, -sz, 0.3),]
55     return sq
57 sq1 = square(250)
58 sq2 = square(500)
60 robot_x = 0.
61 robot_y = 0.
62 robot_a = 0.
63 robot_lspickle = 0
64 robot_rspickle = 0
65 robot_trail = curve()
66 robot_trail_list = []
67 max_trail = 500
69 area_objects = []
71 OFFSET_CORN_X=150
72 OFFSET_CORN_Y=222
73 STEP_CORN_X=225
74 STEP_CORN_Y=250
76 WAYPOINTS_NBX = 13
77 WAYPOINTS_NBY = 8
79 TYPE_WAYPOINT=0
80 TYPE_DANGEROUS=1
81 TYPE_WHITE_CORN=2
82 TYPE_BLACK_CORN=3
83 TYPE_OBSTACLE=4
84 TYPE_BALL=5
85 TYPE_NEIGH=6
87 col = [TYPE_WAYPOINT] * WAYPOINTS_NBY
88 waypoints = [col[:] for i in range(WAYPOINTS_NBX)]
89 corn_table = [TYPE_WHITE_CORN]*18
91 corn_side_confs = [
92     [ 1, 4 ],
93     [ 0, 4 ],
94     [ 2, 4 ],
95     [ 2, 3 ],
96     [ 0, 3 ],
97     [ 1, 3 ],
98     [ 1, 6 ],
99     [ 0, 6 ],
100     [ 2, 6 ],
101     ]
102 corn_center_confs = [
103     [ 5, 8 ],
104     [ 7, 8 ],
105     [ 5, 9 ],
106     [ 7, 8 ],
107     ]
109 def pt2corn(i,j):
110     if i == 0 and j == 2: return 0
111     if i == 0 and j == 4: return 1
112     if i == 0 and j == 6: return 2
113     if i == 2 and j == 3: return 3
114     if i == 2 and j == 5: return 4
115     if i == 2 and j == 7: return 5
116     if i == 4 and j == 4: return 6
117     if i == 4 and j == 6: return 7
118     if i == 6 and j == 5: return 8
119     if i == 6 and j == 7: return 9
120     if i == 8 and j == 4: return 10
121     if i == 8 and j == 6: return 11
122     if i == 10 and j == 3: return 12
123     if i == 10 and j == 5: return 13
124     if i == 10 and j == 7: return 14
125     if i == 12 and j == 2: return 15
126     if i == 12 and j == 4: return 16
127     if i == 12 and j == 6: return 17
128     return -1
130 def corn_get_sym(i):
131     sym = [15, 16, 17, 12, 13, 14, 10, 11, 8, 9, 6, 7, 3, 4, 5, 0, 1, 2]
132     return sym[i]
134 def init_corn_table(conf_side, conf_center):
135     global corn_table, corn_side_confs, corn_center_confs
136     print "confs = %d, %d"%(conf_side, conf_center)
137     for i in range(18):
138         if i in corn_side_confs[conf_side]:
139             corn_table[i] = TYPE_BLACK_CORN
140             continue
141         if corn_get_sym(i) in corn_side_confs[conf_side]:
142             corn_table[i] = TYPE_BLACK_CORN
143             continue
144         if i in corn_center_confs[conf_center]:
145             corn_table[i] = TYPE_BLACK_CORN
146             continue
147         if corn_get_sym(i) in corn_center_confs[conf_center]:
148             corn_table[i] = TYPE_BLACK_CORN
149             continue
150         corn_table[i] = TYPE_WHITE_CORN
152 def init_waypoints():
153     global waypoints, corn_table
155     for i in range(WAYPOINTS_NBX):
156         for j in range(WAYPOINTS_NBY):
157             # corn
158             c = pt2corn(i, j)
159             if c >= 0:
160                 waypoints[i][j] = corn_table[c]
161                 continue
163             # balls
164             if (i & 1) == 0 and j > 3:
165                 waypoints[i][j] = TYPE_BALL
166                 continue
167             if (i == 0 or i == WAYPOINTS_NBX-1) and j > 2:
168                 waypoints[i][j] = TYPE_BALL
169                 continue
171             # too close of border
172             if (i & 1) == 1 and j == WAYPOINTS_NBY -1:
173                 waypoints[i][j] = TYPE_OBSTACLE
174                 continue
175             # hill
176             if i >= 2 and i < WAYPOINTS_NBX - 2 and j < 2:
177                 waypoints[i][j] = TYPE_OBSTACLE
178                 continue
179             # dangerous points
180             if i == 0 or i == WAYPOINTS_NBX-1:
181                 waypoints[i][j] = TYPE_DANGEROUS
182                 continue
183             if (i&1) == 0 and j == WAYPOINTS_NBY-1:
184                 waypoints[i][j] = TYPE_DANGEROUS
185                 continue
187             waypoints[i][j] = TYPE_WAYPOINT
189         print i, waypoints[i]
190     return waypoints
193 def toggle_obj_disp():
194     global area_objects
196     """
197     if area_objects == []:
198         c = sphere(radius=5, color=(0., 0.,1.),
199                    pos=(1238.-AREA_X/2, 1313.-AREA_Y/2, 5))
200         area_objects.append(c)
201         c = sphere(radius=5, color=(0., 0.,1.),
202                    pos=(1364.-AREA_X/2, 1097.-AREA_Y/2, 5))
203         area_objects.append(c)
204         c = sphere(radius=5, color=(0., 0.,1.),
205                    pos=(1453.-AREA_X/2, 1176.-AREA_Y/2, 5))
206         area_objects.append(c)
207         c = sphere(radius=5, color=(0., 0.,1.),
208                    pos=(1109.-AREA_X/2, 1050.-AREA_Y/2, 5))
209         area_objects.append(c)
210 """
211     if area_objects == []:
212         i = 0
213         j = 0
214         x = OFFSET_CORN_X
215         while x < 3000:
216             if (i & 1) == 0:
217                 y = OFFSET_CORN_Y
218             else:
219                 y = OFFSET_CORN_Y + STEP_CORN_Y/2
220             j = 0
221             while y < 2100:
222                 print x,y
223                 if waypoints[i][j] == TYPE_WHITE_CORN:
224                     c = cylinder(axis=(0,0,1), length=CORN_HEIGHT,
226                                  pos=(x-AREA_X/2,y-AREA_Y/2,CORN_HEIGHT/2))
227                     area_objects.append(c)
228                 elif waypoints[i][j] == TYPE_BLACK_CORN:
229                     c = cylinder(axis=(0,0,1), length=CORN_HEIGHT,
231                                  pos=(x-AREA_X/2,y-AREA_Y/2,CORN_HEIGHT/2))
232                     area_objects.append(c)
233                 elif waypoints[i][j] == TYPE_BALL:
234                     c = sphere(radius=50, color=(1., 0.,0.),
235                                pos=(x-AREA_X/2,y-AREA_Y/2,50))
236                     area_objects.append(c)
237                 else:
238                     c = sphere(radius=5, color=(0., 0.,1.),
239                                pos=(x-AREA_X/2,y-AREA_Y/2,5))
240                     area_objects.append(c)
241                 j += 1
242                 y += STEP_CORN_Y
243             i += 1
244             x += STEP_CORN_X
245     else:
246         for o in area_objects:
247             if o.visible:
248                 o.visible = 0
249             else:
250                 o.visible = 1
252 def toggle_color():
253     global color
254     global BLUE, YELLOW
255     if color == YELLOW:
256         color = BLUE
257     else:
258         color = YELLOW
260 def set_robot():
261     global robot, last_pos, robot_trail, robot_trail_list
262     global save_pos, robot_x, robot_y, robot_a
264     if color == YELLOW:
265         tmp_x = robot_x - AREA_X/2
266         tmp_y = robot_y - AREA_Y/2
267         tmp_a = robot_a
268     else:
269         tmp_x = -robot_x + AREA_X/2
270         tmp_y = -robot_y + AREA_Y/2
271         tmp_a = robot_a
273     robot.pos = (tmp_x, tmp_y, ROBOT_HEIGHT/2)
274     axis = (math.cos(tmp_a*math.pi/180),
275             math.sin(tmp_a*math.pi/180),
276             0)
278     robot.axis = axis
279     robot.size = (250, 320, ROBOT_HEIGHT)
281     lspickle.pos = (tmp_x + (robot_lspickle*60) * math.cos((tmp_a+90)*math.pi/180),
282                     tmp_y + (robot_lspickle*60) * math.sin((tmp_a+90)*math.pi/180),
283                     ROBOT_HEIGHT/2)
284     lspickle.axis = axis
285     lspickle.size = (20, 320, 5)
287     rspickle.pos = (tmp_x + (robot_rspickle*60) * math.cos((tmp_a-90)*math.pi/180),
288                     tmp_y + (robot_rspickle*60) * math.sin((tmp_a-90)*math.pi/180),
289                     ROBOT_HEIGHT/2)
290     rspickle.axis = axis
291     rspickle.size = (20, 320, 5)
293     # save position
294     save_pos.append((robot.pos.x, robot.pos.y, tmp_a))
296     pos = robot.pos.x, robot.pos.y, 0.3
297     if pos != last_pos:
298         robot_trail_list.append(pos)
299         last_pos = pos
300     robot_trail_l = len(robot_trail_list)
301     if robot_trail_l > max_trail:
302         robot_trail_list = robot_trail_list[robot_trail_l - max_trail:]
303     robot_trail.pos = robot_trail_list
305 def graph():
306     pass
308 def save():
309     f = open("/tmp/robot_save", "w")
310     for p in save_pos:
311         f.write("%f %f %f\n"%(p[0], p[1], p[2]))
312     f.close()
314 def silent_mkfifo(f):
315     try:
316         os.mkfifo(f)
317     except:
318         pass
320 #init_corn_table(random.randint(0,8), random.randint(0,3))
321 init_corn_table(0, 0)
322 waypoints = init_waypoints()
323 toggle_obj_disp()
325 while True:
326     silent_mkfifo("/tmp/.robot_sim2dis")
327     silent_mkfifo("/tmp/.robot_dis2sim")
328     while True:
329         fr = open("/tmp/.robot_sim2dis", "r")
330         fw = open("/tmp/.robot_dis2sim", "w", 0)
331         while True:
332             m = None
335             # parse position
336             if not m:
337                 m = re.match("pos=%s,%s,%s"%(INT,INT,INT), l)
338                 if m:
339                     robot_x = int(m.groups()[0])
340                     robot_y = int(m.groups()[1])
341                     robot_a = int(m.groups()[2])
342                     set_robot()
344             # parse ballboard
345             if not m:
346                 m = re.match("ballboard=%s"%(INT), l)
347                 if m:
348                     print int(m.groups()[0])
350             # parse cobboard
351             if not m:
352                 m = re.match("cobboard=%s"%(INT), l)
353                 if m:
354                     mode = int(m.groups()[0])
355                     if (mode & 0x01) == 0:
356                         robot_lspickle = 0
357                     elif (mode & 0x02) == 0:
358                         robot_lspickle = 1
359                     else:
360                         robot_lspickle = 2
361                     if (mode & 0x04) == 0:
362                         robot_rspickle = 0
363                     elif (mode & 0x08) == 0:
364                         robot_rspickle = 1
365                     else:
366                         robot_rspickle = 2
368             if scene.kb.keys == 0:
369                 continue
371             k = scene.kb.getkey()
372             x,y,z = scene.center
373             if k == "left":
374                 scene.center = x-10,y,z
375             elif k == "right":
376                 scene.center = x+10,y,z
377             elif k == "up":
378                 scene.center = x,y+10,z
379             elif k == "down":
380                 scene.center = x,y-10,z
381             elif k == "l":
382                 fw.write("l")
383             elif k == "r":
384                 fw.write("r")
385             elif k == "c":
386                 robot_trail_list = []
387             elif k == "x":
388                 save_pos = []
389             elif k == "g":
390                 graph()
391             elif k == "s":
392                 save()
393             elif k == "h":
394                 toggle_obj_disp()
395             elif k == "i":
396                 toggle_color()
398             # EOF
399             if l == "":
400                 break
402         fr.close()
403         fw.close()