1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43 import wx
44 import os
45 import pygame
46 import random
47 import time
48 import copy
49 from math import *
50 from navigationTactics import AgentRRTTactic
51
52
53
54
55
56 RED = (255,0,0)
57 YELLOW = (255, 255, 0)
58 DARKGREEN = (0, 100, 0)
59 DODGERBLUE = (30, 144, 255)
60 PINK = (255, 192, 203)
61 GRAY60 = (153, 153, 153)
62 DARKORANGE = (255, 140, 0)
63 BLUEVIOLET = (138, 43, 226)
64 ORANGE = (255, 165, 0)
65 AQUAMARINE = (127, 255, 212)
66 SADDLEBROWN = (139, 69, 19)
67 CHOCOLATE2 = (238, 118, 33)
68 GREEN3 = (0, 205, 0)
69 DEEPPINK = (255, 20, 147)
70 HOTPINK = (255, 105, 180)
71 WHITE = (255, 255, 255)
72 BLACK = (0, 0, 0)
73 COLORS = [RED, DARKGREEN, DODGERBLUE, PINK, GRAY60, DARKORANGE, BLUEVIOLET, ORANGE, AQUAMARINE, SADDLEBROWN, CHOCOLATE2, GREEN3, DEEPPINK, YELLOW, HOTPINK]
74
75
76
77
78 -class MainGUIWindow(wx.Frame):
79 - def __init__(self, *args, **kwds):
80
81 kwds["style"] = wx.DEFAULT_FRAME_STYLE
82 wx.Frame.__init__(self, *args, **kwds)
83 self.notebook_1 = wx.Notebook(self, -1, style=0)
84
85
86 self._pygameDataInitialized = False
87 self._framecounter = 0
88 self.lastScreenX = 0
89 self.lastScreenY = 0
90
91 self.stepNum = 0
92
93
94 self.zoom = 1.0
95 self.lastZoom = self.zoom
96
97
98 self.simEnv = None
99 self.updateBackgroundHooks = []
100 self.numStepsToSimulate = 0
101 self.agent2RRTData = {}
102
103
104 self.darkBackground = False
105 if self.darkBackground:
106 self.bgColor = (0,0,0)
107 else:
108
109 self.bgColor = (135,206,250)
110
111
112
113 self.frame_1_menubar = wx.MenuBar()
114 self.file = wx.Menu()
115 self.file.Append(wx.NewId(), "&Open", "", wx.ITEM_NORMAL)
116 self.file.Append(wx.NewId(), "&Save", "", wx.ITEM_NORMAL)
117 self.file.AppendSeparator()
118 self.quit = wx.MenuItem(self.file, wx.NewId(), "&Quit\tCtrl+Q", "", wx.ITEM_NORMAL)
119 self.file.AppendItem(self.quit)
120 self.frame_1_menubar.Append(self.file, "&File")
121 wxglade_tmp_menu = wx.Menu()
122 self.frame_1_menubar.Append(wxglade_tmp_menu, "&Edit")
123 wxglade_tmp_menu = wx.Menu()
124 self.frame_1_menubar.Append(wxglade_tmp_menu, "&Help")
125 self.SetMenuBar(self.frame_1_menubar)
126
127
128
129 self.frame_1_statusbar = self.CreateStatusBar(1, 0)
130
131
132
133 self.frame_1_toolbar = wx.ToolBar(self, -1)
134 self.SetToolBar(self.frame_1_toolbar)
135 runId = wx.NewId()
136 self.frame_1_toolbar.AddLabelTool(runId, "Run", wx.Bitmap(os.path.join(os.path.dirname( __file__ ), "images", "player_play.png"), wx.BITMAP_TYPE_ANY), wx.NullBitmap, wx.ITEM_NORMAL, "Run", "Starts the simulation")
137 zoominId = wx.NewId()
138 self.frame_1_toolbar.AddLabelTool(zoominId, "Zoom In", wx.Bitmap(os.path.join(os.path.dirname( __file__ ), "images", "zoom-in.png"), wx.BITMAP_TYPE_ANY), wx.NullBitmap, wx.ITEM_NORMAL, "Zoom In", "Zoom In")
139 zoomoutId = wx.NewId()
140 self.frame_1_toolbar.AddLabelTool(zoomoutId, "Zoom Out", wx.Bitmap(os.path.join(os.path.dirname( __file__ ), "images", "zoom-out.png"), wx.BITMAP_TYPE_ANY), wx.NullBitmap, wx.ITEM_NORMAL, "Zoom Out", "Zoom out")
141
142
143
144
145 self.notebook_1_pane_1 = wx.Panel(self.notebook_1, -1)
146 self.notebook_1_pane_2 = wx.Panel(self.notebook_1, -1)
147 self.notebook_1_pane_3 = wx.Panel(self.notebook_1, -1)
148
149
150 self.__set_properties()
151 self.__do_layout()
152
153
154
155 self.Bind(wx.EVT_TOOL, self.OnRunClick, id=runId)
156 self.Bind(wx.EVT_TOOL, self.OnZoominClick, id=zoominId)
157 self.Bind(wx.EVT_TOOL, self.OnZoomoutClick, id=zoomoutId)
158 self.Bind(wx.EVT_MENU, self.OnQuit, self.quit)
159
160
161
162
164
165 self.SetTitle("UTSeaSim - The University of Texas at Austin")
166 self.SetSize((1300, 800))
167
168 self.frame_1_statusbar.SetFieldsCount(2)
169 self.frame_1_statusbar.SetStatusWidths([-3,-2])
170
171 frame_1_statusbar_fields = ["frame_1_statusbar", ""]
172 for i in range(len(frame_1_statusbar_fields)):
173 self.frame_1_statusbar.SetStatusText(frame_1_statusbar_fields[i], i)
174 self.frame_1_toolbar.Realize()
175
176
177
178
179 - def __do_layout(self):
180
181 sizer_1 = wx.BoxSizer(wx.VERTICAL)
182
183 self.notebook_1.AddPage(self.notebook_1_pane_2, "Simulation")
184 self.notebook_1.AddPage(self.notebook_1_pane_3, "Results")
185 sizer_1.Add(self.notebook_1, 1, wx.EXPAND, 0)
186 self.SetSizer(sizer_1)
187 self.Layout()
188
189
190 - def initData(self, env, shipType, worldModel, strategy, numSteps):
191 """Init application specific data"""
192 self.setSimEnv(env)
193 self.setConfigurationOptions(shipType, worldModel, strategy)
194 self.setNumStepsToSimulate(numSteps)
195
196 for i in range(len(self.simEnv.shipAgents)):
197 self.agent2RRTData[i] = None
198
199 - def setSimEnv(self, env):
200 "The GUI queries the env for its state and display it"
201 self.simEnv = env
202 self.initialSimEnv = copy.deepcopy(env)
203
204
205 - def setConfigurationOptions(self, shipType, worldModel, strategy):
206 """Passing the cmd-line configuration options.
207 Based on what the user chose on the cmd-line, we might have
208 different GUI configurations.
209 """
210
211 if strategy == 'holonomicrrt' or strategy == 'rrt':
212 self.updateBackgroundHooks.append(self.drawRRTHook)
213 self.updateBackgroundHooks.append(self.basicPatrolHook)
214 elif strategy == 'pid' or strategy == 'measuredist':
215 self.updateBackgroundHooks.append(self.drawStartEndHook)
216 elif strategy == 'basicpatrol':
217 self.updateBackgroundHooks.append(self.basicPatrolHook)
218
219 self.updateBackgroundHooks.append(self.drawObstacleHook)
220 self.updateBackgroundHooks.append(self.drawRRTHook)
221 else:
222 self.updateBackgroundHooks.append(self.noopHook)
223
224 - def setNumStepsToSimulate(self, numSteps):
225 self.numStepsToSimulate = numSteps
226
227 - def OnRunClick(self, event):
228 "Callback for the run button, starts the simulation"
229
230
231 if self.simEnv == None:
232 print 'World Environment is not initialized, ignoring click'
233 return
234
235
236 if not self._pygameDataInitialized:
237
238 hwnd = self.notebook_1_pane_2.GetHandle()
239 os.environ['SDL_WINDOWID'] = str(hwnd)
240 pygame.init()
241
242 x,y = self.notebook_1_pane_2.GetSizeTuple()
243 self._surface = pygame.display.set_mode((x,y))
244 self._surface.fill(self.bgColor)
245 self.lastScreenX, self.lastScreenY = x, y
246
247
248
249
250 self.ship = pygame.image.load(os.path.join(os.path.dirname( __file__ ), 'images','ship3.png'))
251
252 """
253 self.shipPositions = [self.ship.get_rect().move(600, random.randint(0,400)) for i in range(10)]
254 """
255 self.shipPositions = [self.ship.get_rect().move(self.screen(state.x), self.screen(state.y)) for state in self.simEnv.state.shipExternalStates]
256
257 self.shipRotatedPositions = list(self.shipPositions)
258 """
259 self.angle = -90
260 """
261
262
263
264 self.obstacles = pygame.image.load(os.path.join(os.path.dirname( __file__ ), 'images','obstacles_transparent_bg.png'))
265
266 self.waypoint = pygame.image.load(os.path.join(os.path.dirname( __file__ ), 'images','target.png'))
267
268
269
270 self.StartTimers(event)
271
272 self._pygameDataInitialized = True
273
274 - def OnZoominClick(self, event):
275 "Zoom in callback."
276 self.zoom /= 1.5
277
278 - def OnZoomoutClick(self, event):
279 "Zoom out callback."
280 self.zoom *= 1.5
281
282 - def StartTimers(self, event):
283 "Initialize timers for different events"
284
285
286 self.timer = wx.Timer(self)
287 self.Bind(wx.EVT_TIMER, self.Update, self.timer)
288 self.fps = 60.
289 self.timespacing = 1000.0 / self.fps
290 self.timer.Start(self.timespacing, False)
291 self.i = 0
292
293
294
295 self.backgroundTimer = wx.Timer(self)
296 self.Bind(wx.EVT_TIMER, self.UpdateBackground, self.backgroundTimer)
297 self.backgroundTimer.Start(500)
298 self.UpdateBackground(None)
299
300
301
302 - def Update(self, event):
303 "Call back for the timer event"
304 if self.stepNum >= self.numStepsToSimulate:
305
306 self.Close()
307 self.stepNum += 1
308 self.UpdateScreen()
309
310
311 - def UpdateScreen(self):
312 """
313 Draws the next frame in the animation.
314
315 Uses the "dirty rectangles" method.
316 This method only updates changed parts of the screen,
317 in contrast to updating all the screen each frame.
318 It gives significant performance improvement.
319 """
320
321 self.simEnv.step()
322 newShipStates = self.simEnv.state.shipExternalStates
323
324
325
326 t1 = time.time()
327 for pos in self.shipRotatedPositions:
328 self._surface.fill(self.bgColor, pos)
329 t2 = time.time()
330
331
332
333
334 t1 = time.time()
335 rotate = pygame.transform.rotate
336 oldRotatedPositions = list(self.shipRotatedPositions)
337 self.shipRotatedPositions = []
338 for i,shipState in enumerate(newShipStates):
339
340 """
341 speed = i
342 transformAngle = (self.angle * -1) - 90 # from ^ image coord to "top-left based" coords
343 xTranslation = round( speed * cos(radians(transformAngle)) )
344 yTranslation = round( speed * sin(radians(transformAngle)) )
345 newpos = pos.move(xTranslation, yTranslation)
346 self.shipPositions[i] = newpos
347 """
348 oldX, oldY = self.shipPositions[i].center
349 xDiff = self.screen(shipState.x) - oldX
350 yDiff = self.screen(shipState.y) - oldY
351 newpos = self.shipPositions[i].move(xDiff, yDiff)
352 self.shipPositions[i] = newpos
353
354 center = newpos.center
355
356
357
358 toRotate = -shipState.orientation
359
360 rotated = rotate(self.ship, toRotate)
361 newpos = rotated.get_rect(center=center)
362 self._surface.blit(rotated, newpos)
363
364 self.shipRotatedPositions.append(newpos)
365
366 """
367 self.angle -= 1
368 if self.angle <= -360:
369 self.angle = 0
370 """
371
372 pygame.display.update(self.shipRotatedPositions + oldRotatedPositions)
373 t2 = time.time()
374
375
376
377
378 self._framecounter += 1
379 self.frame_1_statusbar.SetStatusText("Frame %i" % self._framecounter, 1)
380
381
382
383 - def UpdateBackground(self, event):
384 """
385 Colors the background in black.
386 This make sure background is always there,
387 even after switching tabs, moving windows above it,
388 and so on
389 """
390 x,y = self.notebook_1_pane_2.GetSizeTuple()
391 if x != self.lastScreenX or y != self.lastScreenY or self.lastZoom != self.zoom:
392
393 self._surface = pygame.display.set_mode((x,y))
394 self._surface.fill(self.bgColor)
395 self.lastScreenX, self.lastScreenY, self.lastZoom = x, y, self.zoom
396
397
398 for hook in self.updateBackgroundHooks:
399
400 hook()
401
402 pygame.display.update()
403
404
405
406 - def OnQuit(self, event):
407 self.Close()
408
409
410
411
412
413 - def drawRRTHook(self):
414 """This callback is being used only if there is
415 an RRT to be drawn. It is determined in setConfigurationOptions()
416 """
417 for i, agent in enumerate(self.simEnv.shipAgents):
418
419 color = COLORS[i % len(COLORS)]
420 if isinstance(agent.strategy.navigationTactic, AgentRRTTactic):
421
422
423 if self.agent2RRTData[i] == None:
424
425 self.agent2RRTData[i] = copy.deepcopy(agent.strategy.navigationTactic.rrtBuilder)
426
427 self.drawRRT(agent.strategy.navigationTactic.rrtBuilder, color)
428 else:
429
430 if self.agent2RRTData[i] != None:
431 self.drawRRT(self.agent2RRTData[i], self.bgColor)
432 self.agent2RRTData[i] = None
433
434
435
436 - def drawRRT(self, RRTStrategy, color):
437 rrt = RRTStrategy.tree
438 nodes = rrt.nodes
439
440 for node in nodes:
441 nodeState = node.state
442 x1, y1 = self.screen(nodeState.x), self.screen(nodeState.y)
443 if node.parent != None:
444 parentState = node.parent.state
445 x2, y2 = self.screen(parentState.x), self.screen(parentState.y)
446 else:
447 x2, y2 = x1, y1
448 pygame.draw.lines(self._surface, color, False, [(x1,y1), (x2,y2)], 1)
449
450 goal = RRTStrategy.goalPos
451 x, y = self.screen(goal.x), self.screen(goal.y)
452 x, y = int(round(x)), int(round(y))
453 pygame.draw.circle(self._surface, color, (x, y), 4, 0)
454
456 """This hook is used when we want to draw the
457 start and goal states.
458 """
459 for i, agent in enumerate(self.simEnv.shipAgents):
460
461 color = COLORS[i % len(COLORS)]
462
463
464 externalState = self.initialSimEnv.state.shipExternalStates[i]
465 x, y = self.screen(externalState.x), self.screen(externalState.y)
466 pygame.draw.rect(self._surface, color, pygame.Rect(x+2,y+2,4,4))
467
468
469 goal = agent.strategy.goalPos
470 x, y = goal
471 x, y = self.screen(x), self.screen(y)
472 pygame.draw.circle(self._surface, color, (x, y), 4, 0)
473
474 - def drawPathHook(self):
475 """
476 This hook is used when we want to draw the
477 waypoints of each agent's path, for all agents.
478 """
479 for i, agent in enumerate(self.simEnv.shipAgents):
480
481 color = COLORS[i % len(COLORS)]
482
483
484
485
486
487
488
489 for point in agent.strategy.getPath():
490 x, y = point
491 x, y = self.screen(x), self.screen(y)
492
493
494 self._surface.blit(self.waypoint, (x, y))
495
497 """This hook is used for drawing a specific obstacle."""
498
499 color = BLACK
500 for obst in self.simEnv.state.sea.obstacles.getObstaclesList():
501 points = obst.getBorder()
502 scaledPoints = [(self.screen(x), self.screen(y)) for x,y in points]
503
504 pygame.draw.lines(self._surface, color, False, scaledPoints, 1)
505
506
507
508
509
510
511
512
513 - def basicPatrolHook(self):
514 """Just calls other hooks."""
515 self.drawPathHook()
516
517
518 - def noopHook(self):
520
521 - def screen(self, coord):
522 return coord / self.zoom
523
524
525
526
527
528
529
530
531 -def runGui(env, shipType, worldModel, strategy, numSteps):
532 app = wx.PySimpleApp(0)
533 wx.InitAllImageHandlers()
534 frame_1 = MainGUIWindow(None, -1, "")
535 frame_1.initData(env, shipType, worldModel, strategy, numSteps)
536 app.SetTopWindow(frame_1)
537 frame_1.Show()
538 app.MainLoop()
539
540 """
541 if __name__ == "__main__":
542 runGui(None) # TODO: None only because runGui expects an argument, just in the meantime
543 """
544