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 def square(sz):
45     sq = curve()
46     sq.pos = [(-sz, -sz, 0.3),
47               (-sz, sz, 0.3),
48               (sz, sz, 0.3),
49               (sz, -sz, 0.3),
50               (-sz, -sz, 0.3),]
51     return sq
53 sq1 = square(250)
54 sq2 = square(500)
56 robot_x = 0.
57 robot_y = 0.
58 robot_a = 0.
59 robot_lspickle = 0
60 robot_rspickle = 0
61 robot_trail = curve()
62 robot_trail_list = []
63 max_trail = 500
65 area_objects = []
67 OFFSET_CORN_X=150
68 OFFSET_CORN_Y=222
69 STEP_CORN_X=225
70 STEP_CORN_Y=250
72 WAYPOINTS_NBX = 13
73 WAYPOINTS_NBY = 8
75 TYPE_WAYPOINT=0
76 TYPE_DANGEROUS=1
77 TYPE_WHITE_CORN=2
78 TYPE_BLACK_CORN=3
79 TYPE_OBSTACLE=4
80 TYPE_BALL=5
81 TYPE_NEIGH=6
83 col = [TYPE_WAYPOINT] * WAYPOINTS_NBY
84 waypoints = [col[:] for i in range(WAYPOINTS_NBX)]
85 corn_table = [TYPE_WHITE_CORN]*18
87 corn_side_confs = [
88     [ 1, 4 ],
89     [ 0, 4 ],
90     [ 2, 4 ],
91     [ 2, 3 ],
92     [ 0, 3 ],
93     [ 1, 3 ],
94     [ 1, 6 ],
95     [ 0, 6 ],
96     [ 2, 6 ],
97     ]
98 corn_center_confs = [
99     [ 5, 8 ],
100     [ 7, 8 ],
101     [ 5, 9 ],
102     [ 7, 8 ],
103     ]
105 def pt2corn(i,j):
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
124     return -1
126 def corn_get_sym(i):
127     sym = [15, 16, 17, 12, 13, 14, 10, 11, 8, 9, 6, 7, 3, 4, 5, 0, 1, 2]
128     return sym[i]
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)
133     for i in range(18):
134         if i in corn_side_confs[conf_side]:
135             corn_table[i] = TYPE_BLACK_CORN
136             continue
137         if corn_get_sym(i) in corn_side_confs[conf_side]:
138             corn_table[i] = TYPE_BLACK_CORN
139             continue
140         if i in corn_center_confs[conf_center]:
141             corn_table[i] = TYPE_BLACK_CORN
142             continue
143         if corn_get_sym(i) in corn_center_confs[conf_center]:
144             corn_table[i] = TYPE_BLACK_CORN
145             continue
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):
153             # corn
154             c = pt2corn(i, j)
155             if c >= 0:
156                 waypoints[i][j] = corn_table[c]
157                 continue
159             # balls
160             if (i & 1) == 0 and j > 3:
161                 waypoints[i][j] = TYPE_BALL
162                 continue
163             if (i == 0 or i == WAYPOINTS_NBX-1) and j > 2:
164                 waypoints[i][j] = TYPE_BALL
165                 continue
167             # too close of border
168             if (i & 1) == 1 and j == WAYPOINTS_NBY -1:
169                 waypoints[i][j] = TYPE_OBSTACLE
170                 continue
171             # hill
172             if i >= 2 and i < WAYPOINTS_NBX - 2 and j < 2:
173                 waypoints[i][j] = TYPE_OBSTACLE
174                 continue
175             # dangerous points
176             if i == 0 or i == WAYPOINTS_NBX-1:
177                 waypoints[i][j] = TYPE_DANGEROUS
178                 continue
179             if (i&1) == 0 and j == WAYPOINTS_NBY-1:
180                 waypoints[i][j] = TYPE_DANGEROUS
181                 continue
183             waypoints[i][j] = TYPE_WAYPOINT
185         print i, waypoints[i]
186     return waypoints
189 def toggle_obj_disp():
190     global area_objects
192     """
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)
206 """
207     if area_objects == []:
208         i = 0
209         j = 0
210         x = OFFSET_CORN_X
211         while x < 3000:
212             if (i & 1) == 0:
213                 y = OFFSET_CORN_Y
214             else:
215                 y = OFFSET_CORN_Y + STEP_CORN_Y/2
216             j = 0
217             while y < 2100:
218                 print x,y
219                 if waypoints[i][j] == TYPE_WHITE_CORN:
220                     c = cylinder(axis=(0,0,1), length=CORN_HEIGHT,
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,
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)
233                 else:
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)
237                 j += 1
238                 y += STEP_CORN_Y
239             i += 1
240             x += STEP_CORN_X
241     else:
242         for o in area_objects:
243             if o.visible:
244                 o.visible = 0
245             else:
246                 o.visible = 1
249 def set_robot():
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),
255             0)
257     robot.pos = (robot_x - AREA_X/2, robot_y - AREA_Y/2, ROBOT_HEIGHT/2)
258     robot.axis = axis
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),
263                     ROBOT_HEIGHT/2)
264     lspickle.axis = axis
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),
269                     ROBOT_HEIGHT/2)
270     rspickle.axis = axis
271     rspickle.size = (20, 320, 5)
273     # save position
274     save_pos.append((robot.pos.x, robot.pos, robot_a))
276     pos = robot.pos.x, robot.pos.y, 0.3
277     if pos != last_pos:
278         robot_trail_list.append(pos)
279         last_pos = 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
285 def graph():
286     pass
288 def save():
289     f = open("/tmp/robot_save", "w")
290     for p in save_pos:
291         f.write("%f %f %f\n"%(p[0], p[1], p[2]))
292     f.close()
294 def silent_mkfifo(f):
295     try:
296         os.mkfifo(f)
297     except:
298         pass
300 #init_corn_table(random.randint(0,8), random.randint(0,3))
301 init_corn_table(0, 0)
302 waypoints = init_waypoints()
303 toggle_obj_disp()
305 while True:
306     silent_mkfifo("/tmp/.robot_sim2dis")
307     silent_mkfifo("/tmp/.robot_dis2sim")
308     while True:
309         fr = open("/tmp/.robot_sim2dis", "r")
310         fw = open("/tmp/.robot_dis2sim", "w", 0)
311         while True:
312             m = None
315             # parse position
316             if not m:
317                 m = re.match("pos=%s,%s,%s"%(INT,INT,INT), l)
318                 if m:
319                     robot_x = int(m.groups()[0])
320                     robot_y = int(m.groups()[1])
321                     robot_a = int(m.groups()[2])
322                     set_robot()
324             # parse ballboard
325             if not m:
326                 m = re.match("ballboard=%s"%(INT), l)
327                 if m:
328                     print int(m.groups()[0])
330             # parse cobboard
331             if not m:
332                 m = re.match("cobboard=%s"%(INT), l)
333                 if m:
334                     mode = int(m.groups()[0])
335                     if (mode & 0x01) == 0:
336                         robot_lspickle = 0
337                     elif (mode & 0x02) == 0:
338                         robot_lspickle = 1
339                     else:
340                         robot_lspickle = 2
341                     if (mode & 0x04) == 0:
342                         robot_rspickle = 0
343                     elif (mode & 0x08) == 0:
344                         robot_rspickle = 1
345                     else:
346                         robot_rspickle = 2
348             if scene.kb.keys == 0:
349                 continue
351             k = scene.kb.getkey()
352             x,y,z = scene.center
353             if k == "left":
354                 scene.center = x-10,y,z
355             elif k == "right":
356                 scene.center = x+10,y,z
357             elif k == "up":
358                 scene.center = x,y+10,z
359             elif k == "down":
360                 scene.center = x,y-10,z
361             elif k == "l":
362                 fw.write("l")
363             elif k == "r":
364                 fw.write("r")
365             elif k == "c":
366                 robot_trail_list = []
367             elif k == "x":
368                 save_pos = []
369             elif k == "g":
370                 graph()
371             elif k == "s":
372                 save()
373             elif k == "h":
374                 toggle_obj_disp()
376             # EOF
377             if l == "":
378                 break
380         fr.close()
381         fw.close()