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 = 3.375
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 == 'staticpatrol':
217 self.updateBackgroundHooks.append(self.basicPatrolHook)
218
219 self.updateBackgroundHooks.append(self.drawObstacleHook)
220 self.updateBackgroundHooks.append(self.drawRRTHook)
221 elif strategy == 'coordinatedpatrol':
222 self.updateBackgroundHooks.append(self.basicPatrolHook)
223
224 self.updateBackgroundHooks.append(self.drawObstacleHook)
225 self.updateBackgroundHooks.append(self.drawRRTHook)
226 self.updateBackgroundHooks.append(self.onlineHeuristicDivideHook)
227 else:
228 self.updateBackgroundHooks.append(self.noopHook)
229
230 - def setNumStepsToSimulate(self, numSteps):
231 self.numStepsToSimulate = numSteps
232
233 - def OnRunClick(self, event):
234 "Callback for the run button, starts the simulation"
235
236
237 if self.simEnv == None:
238 print 'World Environment is not initialized, ignoring click'
239 return
240
241
242 if not self._pygameDataInitialized:
243
244 hwnd = self.notebook_1_pane_2.GetHandle()
245 os.environ['SDL_WINDOWID'] = str(hwnd)
246 pygame.init()
247
248 x,y = self.notebook_1_pane_2.GetSizeTuple()
249 self._surface = pygame.display.set_mode((x,y))
250 self._surface.fill(self.bgColor)
251 self.lastScreenX, self.lastScreenY = x, y
252
253
254
255
256 self.ship = pygame.image.load(os.path.join(os.path.dirname( __file__ ), 'images','ship3.png'))
257
258 """
259 self.shipPositions = [self.ship.get_rect().move(600, random.randint(0,400)) for i in range(10)]
260 """
261 self.shipPositions = [self.ship.get_rect().move(self.screen(state.x), self.screen(state.y)) for state in self.simEnv.state.shipExternalStates]
262
263 self.shipRotatedPositions = list(self.shipPositions)
264 """
265 self.angle = -90
266 """
267
268
269
270 self.obstacles = pygame.image.load(os.path.join(os.path.dirname( __file__ ), 'images','obstacles_transparent_bg.png'))
271
272 self.waypoint = pygame.image.load(os.path.join(os.path.dirname( __file__ ), 'images','target.png'))
273
274
275
276 self.StartTimers(event)
277
278 self._pygameDataInitialized = True
279
280 - def OnZoominClick(self, event):
281 "Zoom in callback."
282 self.zoom /= 1.5
283
284 - def OnZoomoutClick(self, event):
285 "Zoom out callback."
286 self.zoom *= 1.5
287
288 - def StartTimers(self, event):
289 "Initialize timers for different events"
290
291
292 self.timer = wx.Timer(self)
293 self.Bind(wx.EVT_TIMER, self.Update, self.timer)
294 self.fps = 60.
295 self.timespacing = 1000.0 / self.fps
296 self.timer.Start(self.timespacing, False)
297 self.i = 0
298
299
300
301 self.backgroundTimer = wx.Timer(self)
302 self.Bind(wx.EVT_TIMER, self.UpdateBackground, self.backgroundTimer)
303 self.backgroundTimer.Start(500)
304 self.UpdateBackground(None)
305
306
307
308 - def Update(self, event):
309 "Call back for the timer event"
310 if self.stepNum >= self.numStepsToSimulate:
311
312 self.Close()
313 self.stepNum += 1
314 self.UpdateScreen()
315
316
317 - def UpdateScreen(self):
318 """
319 Draws the next frame in the animation.
320
321 Uses the "dirty rectangles" method.
322 This method only updates changed parts of the screen,
323 in contrast to updating all the screen each frame.
324 It gives significant performance improvement.
325 """
326
327 self.simEnv.step()
328 newShipStates = self.simEnv.state.shipExternalStates
329
330
331
332 t1 = time.time()
333 for pos in self.shipRotatedPositions:
334 self._surface.fill(self.bgColor, pos)
335 t2 = time.time()
336
337
338
339
340 t1 = time.time()
341 rotate = pygame.transform.rotate
342 oldRotatedPositions = list(self.shipRotatedPositions)
343 self.shipRotatedPositions = []
344 for i,shipState in enumerate(newShipStates):
345
346 """
347 speed = i
348 transformAngle = (self.angle * -1) - 90 # from ^ image coord to "top-left based" coords
349 xTranslation = round( speed * cos(radians(transformAngle)) )
350 yTranslation = round( speed * sin(radians(transformAngle)) )
351 newpos = pos.move(xTranslation, yTranslation)
352 self.shipPositions[i] = newpos
353 """
354 oldX, oldY = self.shipPositions[i].center
355 xDiff = self.screen(shipState.x) - oldX
356 yDiff = self.screen(shipState.y) - oldY
357 newpos = self.shipPositions[i].move(xDiff, yDiff)
358 self.shipPositions[i] = newpos
359
360 center = newpos.center
361
362
363
364 toRotate = -shipState.orientation
365
366 rotated = rotate(self.ship, toRotate)
367 newpos = rotated.get_rect(center=center)
368 self._surface.blit(rotated, newpos)
369
370 self.shipRotatedPositions.append(newpos)
371
372 """
373 self.angle -= 1
374 if self.angle <= -360:
375 self.angle = 0
376 """
377
378
379
380
381 try:
382 flag
383 except:
384
385 flag = 1
386 pygame.display.update()
387 else:
388
389 pygame.display.update(self.shipRotatedPositions + oldRotatedPositions)
390 t2 = time.time()
391
392
393
394
395 self._framecounter += 1
396 self.frame_1_statusbar.SetStatusText("Frame %i" % self._framecounter, 1)
397
398
399
400 - def UpdateBackground(self, event):
401 """
402 Colors the background in self.bgColor.
403 This make sure background is always there,
404 even after switching tabs, moving windows above it,
405 and so on
406 """
407 x,y = self.notebook_1_pane_2.GetSizeTuple()
408 if x != self.lastScreenX or y != self.lastScreenY or self.lastZoom != self.zoom:
409
410 self._surface = pygame.display.set_mode((x,y))
411 self._surface.fill(self.bgColor)
412 self.lastScreenX, self.lastScreenY, self.lastZoom = x, y, self.zoom
413
414
415 for hook in self.updateBackgroundHooks:
416
417 hook()
418
419 pygame.display.update()
420
421
422
423 - def OnQuit(self, event):
424 self.Close()
425
426
427
428
429
430 - def drawRRTHook(self):
431 """This callback is being used only if there is
432 an RRT to be drawn. It is determined in setConfigurationOptions()
433 """
434 for i, agent in enumerate(self.simEnv.shipAgents):
435
436 color = COLORS[i % len(COLORS)]
437 if isinstance(agent.strategy.navigationTactic, AgentRRTTactic):
438
439
440 if self.agent2RRTData[i] == None:
441
442 self.agent2RRTData[i] = copy.deepcopy(agent.strategy.navigationTactic.rrtBuilder)
443
444 self.drawRRT(agent.strategy.navigationTactic.rrtBuilder, color)
445 else:
446
447 if self.agent2RRTData[i] != None:
448 self.drawRRT(self.agent2RRTData[i], self.bgColor)
449 self.agent2RRTData[i] = None
450
451
452
453 - def drawRRT(self, RRTStrategy, color):
454 rrt = RRTStrategy.tree
455 nodes = rrt.nodes
456
457 for node in nodes:
458 nodeState = node.state
459 x1, y1 = self.screen(nodeState.x), self.screen(nodeState.y)
460 if node.parent != None:
461 parentState = node.parent.state
462 x2, y2 = self.screen(parentState.x), self.screen(parentState.y)
463 else:
464 x2, y2 = x1, y1
465 pygame.draw.lines(self._surface, color, False, [(x1,y1), (x2,y2)], 1)
466
467 goal = RRTStrategy.goalPos
468 x, y = self.screen(goal.x), self.screen(goal.y)
469 x, y = int(round(x)), int(round(y))
470 pygame.draw.circle(self._surface, color, (x, y), 4, 0)
471
473 """This hook is used when we want to draw the
474 start and goal states.
475 """
476 for i, agent in enumerate(self.simEnv.shipAgents):
477
478 color = COLORS[i % len(COLORS)]
479
480
481 externalState = self.initialSimEnv.state.shipExternalStates[i]
482 x, y = self.screen(externalState.x), self.screen(externalState.y)
483 pygame.draw.rect(self._surface, color, pygame.Rect(x+2,y+2,4,4))
484
485
486 goal = agent.strategy.goalPos
487 x, y = goal
488 x, y = self.screen(x), self.screen(y)
489 pygame.draw.circle(self._surface, color, (x, y), 4, 0)
490
491 - def drawPathHook(self):
492 """
493 This hook is used when we want to draw the
494 waypoints of each agent's path, for all agents.
495 """
496 for i, agent in enumerate(self.simEnv.shipAgents):
497
498 color = COLORS[i % len(COLORS)]
499
500
501
502
503
504
505
506 for point in agent.strategy.getPath():
507 x, y = point
508 x, y = self.screen(x), self.screen(y)
509
510
511 self._surface.blit(self.waypoint, (x, y))
512
514 """This hook is used for drawing a specific obstacle."""
515
516 color = BLACK
517 for obst in self.simEnv.state.sea.obstacles.getObstaclesList():
518 points = obst.getBorder()
519 scaledPoints = [(self.screen(x), self.screen(y)) for x,y in points]
520
521 pygame.draw.lines(self._surface, color, False, scaledPoints, 1)
522
524 """
525 Draws a red/green background to every ship depending on whether they are ready
526 """
527 shipStates = self.simEnv.state.shipExternalStates
528 agent = self.simEnv.shipAgents[0]
529
530 patroling = (agent.strategy.missionState == agent.strategy.PATROLING)
531
532 if patroling:
533 self.readyToPatrolCounter += 1
534 else:
535 self.readyToPatrolCounter = 0
536
537
538 reachedStart = agent.strategy.reachedStartPoint
539 for i, agent in enumerate(self.simEnv.shipAgents):
540 startPoint = agent.strategy.myStartingPatrolPosition
541 if startPoint != None:
542 x, y = int(round(self.screen(startPoint.x))), int(round(self.screen(startPoint.y)))
543 if patroling:
544
545 if self.readyToPatrolCounter < 5:
546 pygame.draw.circle(self._surface, GREEN3, (x, y), 12, 0)
547 if self.readyToPatrolCounter == 5:
548 pygame.draw.circle(self._surface, self.bgColor, (x, y), 12, 0)
549 elif i in reachedStart:
550
551 pygame.draw.circle(self._surface, GREEN3, (x, y), 12, 0)
552 else:
553
554 pygame.draw.circle(self._surface, RED, (x, y), 12, 0)
555
556
557
558
559
560
561
562
563
564
565 - def basicPatrolHook(self):
566 """Just calls other hooks."""
567 self.drawPathHook()
568
569
570 - def noopHook(self):
572
573 - def screen(self, coord):
574 return coord / self.zoom
575
576
577
578
579
580
581
582
583 -def runGui(env, shipType, worldModel, strategy, numSteps):
584 app = wx.PySimpleApp(0)
585 wx.InitAllImageHandlers()
586 frame_1 = MainGUIWindow(None, -1, "")
587 frame_1.initData(env, shipType, worldModel, strategy, numSteps)
588 app.SetTopWindow(frame_1)
589 frame_1.Show()
590 app.MainLoop()
591
592 """
593 if __name__ == "__main__":
594 runGui(None) # TODO: None only because runGui expects an argument, just in the meantime
595 """
596