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