root / scout / scoutsim / GUI.py @ f09d002e
History | View | Annotate | Download (16.9 KB)
1 | 01f09975 | Yuyang Guo | #!/usr/bin/python
|
---|---|---|---|
2 | |||
3 | # Behavior GUI
|
||
4 | |||
5 | # shell subprocess libs
|
||
6 | import subprocess |
||
7 | import shlex #for converting string commands to tokens |
||
8 | 6ee555a3 | Priya | import sys |
9 | 01f09975 | Yuyang Guo | |
10 | # GUI libs
|
||
11 | import wx |
||
12 | import wx.lib.intctrl |
||
13 | import wx.lib.agw.floatspin |
||
14 | |||
15 | # not sure why these 2 are here
|
||
16 | import string |
||
17 | import signal |
||
18 | |||
19 | # ros imports for spawning and killing scout
|
||
20 | import rospy |
||
21 | import roslib |
||
22 | 6350051e | Alex | import os |
23 | 01f09975 | Yuyang Guo | roslib.load_manifest("scoutsim")
|
24 | |||
25 | # scoutsim imports
|
||
26 | from scoutsim.srv import * |
||
27 | |||
28 | from collections import defaultdict |
||
29 | |||
30 | 9a4d8398 | Yuyang Guo | |
31 | class Behaviors(object): |
||
32 | def __init__(self): |
||
33 | print "Warning: You should not need to create an instance of Behaviors" |
||
34 | |||
35 | # this takes advantage of the fact that our behavior list start at 1
|
||
36 | behaviors = ["Pause", "CW Circle", "CCW Circle", "Odometry", |
||
37 | "Navigation Map", "Scheduler", "Warehouse", |
||
38 | 6ee555a3 | Priya | "Line Follow", "WL Test", "Maze Solve"] |
39 | 9a4d8398 | Yuyang Guo | @classmethod
|
40 | 6350051e | Alex | def getListFromDir(self): |
41 | behaviors = [] |
||
42 | for path, dirname, fnames in os.walk('../libscout/src'): |
||
43 | if 'src/behaviors' in path or 'src/test_behaviors' in path: |
||
44 | path_parts = path.split('/')
|
||
45 | |||
46 | for f in sorted(fnames): |
||
47 | # The pause_scout behavior needs to go first!
|
||
48 | if f.endswith('.h') and 'pause_scout' in f: |
||
49 | behaviors.insert(0, f.split('.')[0]) |
||
50 | # Everything else goes in alphabetical order
|
||
51 | elif f.endswith('.h'): |
||
52 | behaviors.append(f.split('.')[0]) |
||
53 | return behaviors
|
||
54 | |||
55 | @classmethod
|
||
56 | 9a4d8398 | Yuyang Guo | def getNumber(self, behaviorName): |
57 | 6350051e | Alex | behaviors = self.getListFromDir()
|
58 | if (behaviorName in behaviors): |
||
59 | return behaviors.index(behaviorName)
|
||
60 | 9a4d8398 | Yuyang Guo | |
61 | @classmethod
|
||
62 | def getName(self, index): |
||
63 | 6350051e | Alex | behaviors = self.getListFromDir()
|
64 | 9a4d8398 | Yuyang Guo | if (0 <= index < len(self.Behavior)): |
65 | 6350051e | Alex | return behaviors[index]
|
66 | 9a4d8398 | Yuyang Guo | else:
|
67 | return -1 |
||
68 | |||
69 | @classmethod
|
||
70 | def getBehaviors(self): |
||
71 | # "Pause" is not considered a behavior in GUI
|
||
72 | 6350051e | Alex | behaviors = self.getListFromDir()
|
73 | return behaviors[1:] |
||
74 | 9a4d8398 | Yuyang Guo | |
75 | 01f09975 | Yuyang Guo | # each scout is represented by this class
|
76 | class Scout(object): |
||
77 | numOfScouts = 0 # class variable keeping track of scouts |
||
78 | |||
79 | def __init__(self, x=0, y=0, theta=0, name=u"", |
||
80 | Scouts=None, oninit = False): |
||
81 | Scout.numOfScouts += 1 # reverted if not successful |
||
82 | if len(name) == 0: |
||
83 | # automatically give name according to numOfScouts
|
||
84 | name = self.autoNameGen(Scouts)
|
||
85 | # set vars
|
||
86 | self.name = name
|
||
87 | self.behavior = "chilling" |
||
88 | self.process = None# used to keep track of all processes |
||
89 | # related to this scout
|
||
90 | 9a4d8398 | Yuyang Guo | self.paused = False |
91 | 01f09975 | Yuyang Guo | self.valid = False |
92 | # these vars are right now just the original position
|
||
93 | # but when ODOMETRY is done, we could actually use them
|
||
94 | self.x, self.y = x, y |
||
95 | self.theta = theta
|
||
96 | |||
97 | # spawn the scout if it is not scout1 (automatically spawned)
|
||
98 | if (not oninit): |
||
99 | if (self.SpawnInSimulator()): |
||
100 | # spawned correctly
|
||
101 | self.valid = True |
||
102 | else:
|
||
103 | Scout.numOfScouts -= 1;
|
||
104 | |||
105 | |||
106 | def autoNameGen(self, Scouts): |
||
107 | i = 1
|
||
108 | while (True): |
||
109 | if not (("scout%d"%i) in Scouts): |
||
110 | return "scout%d"%i |
||
111 | i+=1
|
||
112 | |||
113 | |||
114 | # update the classvariable
|
||
115 | |||
116 | |||
117 | def SpawnInSimulator(self): |
||
118 | try:
|
||
119 | # ros spawn routine
|
||
120 | rospy.wait_for_service('/spawn');
|
||
121 | service = rospy.ServiceProxy('/spawn', Spawn);
|
||
122 | response = service(self.x, self.y, self.theta, self.name) |
||
123 | return True |
||
124 | except rospy.ServiceException as Error: |
||
125 | return False |
||
126 | 9a4d8398 | Yuyang Guo | |
127 | def terminateOldBehavior(self): |
||
128 | if self.process != None: |
||
129 | # now terminate the process
|
||
130 | self.process.terminate() #TODO: this kills the process |
||
131 | # but maybe not the rosnode
|
||
132 | # change to "rosnode kill" instead
|
||
133 | # make sure old behavior is terminated before we run the new one
|
||
134 | self.process.wait()
|
||
135 | 01f09975 | Yuyang Guo | |
136 | 9a4d8398 | Yuyang Guo | # the following lines causes lag and does not solve real issues
|
137 | # first unregister the node
|
||
138 | 1e52c76b | Yuyang Guo | cmd = "rosnode kill /%sBehavior"%self.name |
139 | temp = subprocess.Popen(cmd, shell=True)
|
||
140 | temp.wait() |
||
141 | 9a4d8398 | Yuyang Guo | # this node loses contact but still is not cleaned up in rosnode
|
142 | # however, this should not affect usage at all.
|
||
143 | 01f09975 | Yuyang Guo | |
144 | 9a4d8398 | Yuyang Guo | def changeBehavior(self, behavior): |
145 | self.behavior = behavior
|
||
146 | if (behavior == "pause"): |
||
147 | self.paused = True |
||
148 | else:
|
||
149 | self.paused = False |
||
150 | self.terminateOldBehavior()
|
||
151 | 01f09975 | Yuyang Guo | # do rosprocess calls for new behavior
|
152 | 1e52c76b | Yuyang Guo | roscommand = ("rosrun libscout libscout %s %s"%
|
153 | 9873e344 | Yuyang Guo | (Behaviors.getNumber(self.behavior), self.name)) |
154 | 1e52c76b | Yuyang Guo | self.process = subprocess.Popen(roscommand, shell=True) |
155 | 01f09975 | Yuyang Guo | |
156 | ab8736ac | Yuyang Guo | def teleop(self): |
157 | # teleop involved only one command \
|
||
158 | self.terminateOldBehavior()
|
||
159 | self.process = None |
||
160 | cmd = "rosservice call /set_teleop %s"%self.name |
||
161 | try:
|
||
162 | subprocess.Popen(shlex.split(cmd), shell=False)
|
||
163 | except:
|
||
164 | #not sure why this is happening....
|
||
165 | # seems to be a ros thing
|
||
166 | print "warning, socket error, ignored" |
||
167 | 1e52c76b | Yuyang Guo | |
168 | def sonar_viz(self, on_off): |
||
169 | # teleop involved only one command \
|
||
170 | cmd = "rosservice call /set_sonar_viz %s %s"%(on_off, self.name) |
||
171 | try:
|
||
172 | subprocess.Popen(shlex.split(cmd), shell=False)
|
||
173 | except:
|
||
174 | #not sure why this is happening....
|
||
175 | # seems to be a ros thing
|
||
176 | print "warning, socket error, ignored" |
||
177 | ab8736ac | Yuyang Guo | |
178 | 01f09975 | Yuyang Guo | def killSelf(self): |
179 | # terminate its current behavior
|
||
180 | 9a4d8398 | Yuyang Guo | self.terminateOldBehavior()
|
181 | |||
182 | 01f09975 | Yuyang Guo | # ros call to kill scout in simulator
|
183 | try:
|
||
184 | rospy.wait_for_service("/kill")
|
||
185 | service = rospy.ServiceProxy("/kill", Kill)
|
||
186 | response = service(self.name)
|
||
187 | Scout.numOfScouts -= 1
|
||
188 | self.valid = False |
||
189 | except rospy.ServiceException, e:
|
||
190 | ab8736ac | Yuyang Guo | print "warning: kill scout unsuccessful" |
191 | 01f09975 | Yuyang Guo | self.valid = True |
192 | # remove from the class
|
||
193 | |||
194 | |||
195 | # a wrapper for wx Frame
|
||
196 | class Window(wx.App): |
||
197 | def __init__(self, title=""): |
||
198 | super(Window, self).__init__() |
||
199 | self.initUIFrame()
|
||
200 | |||
201 | def initUIFrame(self): |
||
202 | self.GUIFrame = GUI(None, "hallo!") |
||
203 | |||
204 | |||
205 | # actual GUI frame
|
||
206 | class GUI(wx.Frame): |
||
207 | def __init__(self, parent, title): |
||
208 | super(GUI, self).__init__(parent, title=title, |
||
209 | style=wx.DEFAULT_FRAME_STYLE ^ wx.RESIZE_BORDER, size=(600, 600)) |
||
210 | |||
211 | # open up scoutsim race
|
||
212 | 6ee555a3 | Priya | if len(sys.argv) > 1: |
213 | map_name = sys.argv[1]
|
||
214 | else:
|
||
215 | map_name = "race"
|
||
216 | command = shlex.split("rosrun scoutsim scoutsim_node " + map_name)
|
||
217 | 01f09975 | Yuyang Guo | self.simWindowProcess = subprocess.Popen(command, shell=False) |
218 | #rospy.wait_for_service('/spawn') #don't know why this is here
|
||
219 | |||
220 | # register call back for on close cleanup
|
||
221 | self.Bind(wx.EVT_CLOSE, self.onClose) |
||
222 | |||
223 | |||
224 | self.initData()
|
||
225 | self.initUI()
|
||
226 | self.Show(True) |
||
227 | |||
228 | # do clean up after user close the window
|
||
229 | def onClose(self, event): |
||
230 | print "Cleaning up all Processes" |
||
231 | |||
232 | # kill all the behavior processes
|
||
233 | for scout in self.scouts: |
||
234 | process = self.scouts[scout].process
|
||
235 | if (process != None): |
||
236 | process.terminate() |
||
237 | |||
238 | # kill the scout sim window
|
||
239 | self.simWindowProcess.terminate()
|
||
240 | print "done" |
||
241 | self.Destroy()
|
||
242 | |||
243 | def initData(self): |
||
244 | self.scouts = {}
|
||
245 | 9a4d8398 | Yuyang Guo | #FIXME: these arguments are not right...fix them!!!
|
246 | 01f09975 | Yuyang Guo | self.scouts["scout1"] = Scout(x=0, y=0, theta=0, name="scout1", |
247 | Scouts = self.scouts, oninit=True) |
||
248 | |||
249 | # addButton callback
|
||
250 | def addScout(self, x_wx, y_wx, theta_wx, name_wx): |
||
251 | # x, y, theta, name are given as wx Inputs
|
||
252 | x = x_wx.GetValue() |
||
253 | y = y_wx.GetValue() |
||
254 | theta = theta_wx.GetValue() |
||
255 | name = name_wx.GetValue() |
||
256 | newSc = Scout(x, y, theta, name, self.scouts)
|
||
257 | if (newSc.valid):
|
||
258 | # successful
|
||
259 | self.scouts[newSc.name] = newSc
|
||
260 | self.addScoutBox(newSc.name)
|
||
261 | # alert user spawn successful
|
||
262 | wx.MessageBox(u"Scout '%s' created successfully at (%d,%d,%d\u00B0)"
|
||
263 | %(newSc.name, newSc.x, newSc.y, newSc.theta), |
||
264 | "Scout Created", wx.OK | wx.ICON_INFORMATION)
|
||
265 | |||
266 | else:
|
||
267 | #failed to create scout
|
||
268 | wx.MessageBox("Scout Creation failed :( Check for duplicate name",
|
||
269 | "Add Scout failed", wx.OK | wx.ICON_ERROR)
|
||
270 | |||
271 | def removeScout(self, name): |
||
272 | sct = self.scouts[name]
|
||
273 | sct.killSelf() |
||
274 | if (sct.valid == False): |
||
275 | # successful, remove from the dictionary
|
||
276 | del self.scouts[name] |
||
277 | 9a4d8398 | Yuyang Guo | # delete the refresh the display
|
278 | self.mainArea.Hide(self.sizer[name]) #required by wx before remove |
||
279 | self.mainArea.Remove(self.sizer[name]) |
||
280 | 01f09975 | Yuyang Guo | del self.sizer[name] |
281 | self.window.Layout()
|
||
282 | self.window.Refresh()
|
||
283 | self.window.Update()
|
||
284 | else:
|
||
285 | raise Exception |
||
286 | ab8736ac | Yuyang Guo | |
287 | # runPressed
|
||
288 | 9a4d8398 | Yuyang Guo | # change UI display and invoke change Behavior in scout
|
289 | def changeBehavior(self, name, currBehaviorLabel, |
||
290 | pauseButton, newBehavior): |
||
291 | |||
292 | # to handle user pressing "Run" during pause
|
||
293 | if (newBehavior != "Pause" and self.scouts[name].paused == True): |
||
294 | self.correctPauseButton(name,
|
||
295 | pauseButton, pauseToResume=True)
|
||
296 | |||
297 | currBehaviorLabel.SetLabel(" | Current Behavior: %s"%newBehavior)
|
||
298 | scout = self.scouts[name]
|
||
299 | scout.changeBehavior(newBehavior) |
||
300 | ab8736ac | Yuyang Guo | self.correctTeleopButton(None) |
301 | 9a4d8398 | Yuyang Guo | |
302 | |||
303 | def buttonDown(button): |
||
304 | return button.GetValue()
|
||
305 | |||
306 | def pauseResumeScout(self, name, pauseButton, currBehaviorLabel, dropMenu): |
||
307 | if (pauseButton.GetValue() == True): # |
||
308 | # change behavior to Pause
|
||
309 | self.changeBehavior(name, currBehaviorLabel, pauseButton, "Pause") |
||
310 | # change button to "Resume" & label to "Pause"
|
||
311 | currBehaviorLabel.SetLabel("Pause")
|
||
312 | self.correctPauseButton(name,
|
||
313 | pauseButton, pauseToResume=False)
|
||
314 | else:
|
||
315 | # resume
|
||
316 | # change behavior to whatever is in the drop down menu
|
||
317 | self.changeBehavior(name, currBehaviorLabel, pauseButton,
|
||
318 | dropMenu.GetStringSelection()) |
||
319 | self.correctPauseButton(name,
|
||
320 | pauseButton, pauseToResume=True)
|
||
321 | ab8736ac | Yuyang Guo | |
322 | def teleop(self, name): |
||
323 | self.correctTeleopButton(name)
|
||
324 | self.scouts[name].teleop()
|
||
325 | 9a4d8398 | Yuyang Guo | |
326 | 1e52c76b | Yuyang Guo | def sonar_viz(self, name, sonar_vizButton): |
327 | if (sonar_vizButton.GetValue() == True): |
||
328 | # turn on the sonar viz
|
||
329 | self.scouts[name].sonar_viz("on") |
||
330 | else:
|
||
331 | # turn off sonar viz
|
||
332 | self.scouts[name].sonar_viz("off") |
||
333 | |||
334 | 01f09975 | Yuyang Guo | ############################ UI stuff here ###########################
|
335 | |||
336 | def initUI(self): |
||
337 | ab8736ac | Yuyang Guo | self.allTeleopButtons = {}
|
338 | 01f09975 | Yuyang Guo | self.initAddScoutArea()
|
339 | ab8736ac | Yuyang Guo | |
340 | def correctTeleopButton(self, teleopingName): |
||
341 | for name in self.allTeleopButtons: |
||
342 | if name == teleopingName:
|
||
343 | self.allTeleopButtons[name].SetValue(True) |
||
344 | else:
|
||
345 | self.allTeleopButtons[name].SetValue(False) |
||
346 | |||
347 | |||
348 | def correctPauseButton(self, name, pauseButton, pauseToResume): |
||
349 | if (pauseToResume):
|
||
350 | print "correcting button!" |
||
351 | self.scouts[name].paused = False |
||
352 | pauseButton.SetValue(False) # unpress it |
||
353 | pauseButton.SetLabel("Pause")
|
||
354 | else:
|
||
355 | self.scouts[name].paused = True |
||
356 | pauseButton.SetValue(True) # press it |
||
357 | pauseButton.SetLabel("Resume")
|
||
358 | 01f09975 | Yuyang Guo | |
359 | # the labels and input boxes for adding a scout through GUI
|
||
360 | def initAddScoutArea(self): |
||
361 | # all the layout stuff copied over: using grid layout
|
||
362 | # button callbacks are changed
|
||
363 | self.totalCols = 8 |
||
364 | self.window = wx.ScrolledWindow(self, style=wx.VSCROLL) |
||
365 | self.mainArea = wx.GridSizer(cols=1) |
||
366 | sizer = wx.FlexGridSizer(rows=4, cols=self.totalCols, hgap=5, vgap=5) |
||
367 | |||
368 | # Labels
|
||
369 | blankText = wx.StaticText(self.window, label="") |
||
370 | newScout = wx.StaticText(self.window, label="New Scout") |
||
371 | newScoutName = wx.StaticText(self.window, label="Name:") |
||
372 | startXTitle = wx.StaticText(self.window, label="X:") |
||
373 | startYTitle = wx.StaticText(self.window, label="Y:") |
||
374 | startThetaTitle = wx.StaticText(self.window, label="Rad:") |
||
375 | |||
376 | # Inputs
|
||
377 | newScoutInput = wx.TextCtrl(self.window)
|
||
378 | startX = wx.lib.agw.floatspin.FloatSpin(self.window, min_val=0, digits=5) |
||
379 | startY = wx.lib.agw.floatspin.FloatSpin(self.window, min_val=0, digits=5) |
||
380 | startTheta = wx.lib.agw.floatspin.FloatSpin(self.window, min_val=0, digits=5) |
||
381 | addButton = wx.Button(self.window, id=wx.ID_ADD)
|
||
382 | |||
383 | # Pretty Stuff
|
||
384 | hLine = wx.StaticLine(self.window, size=(600, 5)) |
||
385 | bottomHLine = wx.StaticLine(self.window, size=(600, 5)) |
||
386 | |||
387 | # Row 0: just the label add scout
|
||
388 | sizer.Add(newScout) |
||
389 | for i in range(7): |
||
390 | sizer.AddStretchSpacer(1)
|
||
391 | # Row 1: input(name), x coord, y coord, rad,
|
||
392 | sizer.AddMany([newScoutName, (newScoutInput, 0, wx.EXPAND), startXTitle,
|
||
393 | startX, startYTitle, startY, startThetaTitle, startTheta]) |
||
394 | # Row 2: just the add button to the right
|
||
395 | for i in range(7): |
||
396 | sizer.AddStretchSpacer(1)
|
||
397 | sizer.Add(addButton) |
||
398 | # Row 3
|
||
399 | |||
400 | # Events
|
||
401 | addButton.Bind(wx.EVT_BUTTON, lambda event: self.addScout( |
||
402 | startX, startY, startTheta, newScoutInput)) |
||
403 | |||
404 | sizer.AddGrowableCol(idx=1)
|
||
405 | self.mainArea.Add(sizer, proportion=1, flag=wx.ALL|wx.EXPAND, border=10) |
||
406 | self.window.SetSizer(self.mainArea) |
||
407 | self.sizer = defaultdict()
|
||
408 | self.window.Layout()
|
||
409 | |||
410 | # make the scout1's controller
|
||
411 | self.addScoutBox("scout1"); |
||
412 | 1e52c76b | Yuyang Guo | |
413 | |||
414 | |||
415 | 01f09975 | Yuyang Guo | # copied over from old GUI
|
416 | def addScoutBox(self, name): |
||
417 | self.sizer[name] = wx.FlexGridSizer(rows=2, cols=5, hgap=5, vgap=5) |
||
418 | # Labels
|
||
419 | 9a4d8398 | Yuyang Guo | scoutName = wx.StaticText(self.window, label="Scout: %s"%name) |
420 | 01f09975 | Yuyang Guo | behaviorLabel = wx.StaticText(self.window, label="Behavior: ") |
421 | currBehaviorLabel = wx.StaticText(self.window,
|
||
422 | label=" | Current Behavior: Slacking off")
|
||
423 | |||
424 | # Inputs
|
||
425 | # drop down menue
|
||
426 | 9a4d8398 | Yuyang Guo | scoutChoices = wx.Choice(self.window,
|
427 | choices=Behaviors.getBehaviors()) |
||
428 | 01f09975 | Yuyang Guo | # buttons
|
429 | pauseButton = wx.ToggleButton(self.window, label="Pause") |
||
430 | runButton = wx.Button(self.window, label="Run") |
||
431 | killButton = wx.Button(self.window, label="Kill") |
||
432 | teleopButton = wx.ToggleButton(self.window, label="Teleop") |
||
433 | 1e52c76b | Yuyang Guo | sonar_vizButton = wx.ToggleButton(self.window, label="sonar viz") |
434 | ab8736ac | Yuyang Guo | self.allTeleopButtons[name] = teleopButton
|
435 | 01f09975 | Yuyang Guo | # row 0
|
436 | self.sizer[name].Add(scoutName)
|
||
437 | self.sizer[name].Add(currBehaviorLabel, wx.EXPAND | wx.ALIGN_RIGHT)
|
||
438 | self.sizer[name].AddStretchSpacer(1) |
||
439 | self.sizer[name].Add(killButton, wx.ALIGN_RIGHT)
|
||
440 | 1e52c76b | Yuyang Guo | self.sizer[name].Add(sonar_vizButton)
|
441 | 01f09975 | Yuyang Guo | |
442 | # row 1
|
||
443 | self.sizer[name].Add(behaviorLabel)
|
||
444 | self.sizer[name].Add(scoutChoices, wx.EXPAND)
|
||
445 | self.sizer[name].Add(runButton)
|
||
446 | self.sizer[name].Add(pauseButton, wx.ALIGN_RIGHT)
|
||
447 | self.sizer[name].Add(teleopButton)
|
||
448 | # Events
|
||
449 | killButton.Bind(wx.EVT_BUTTON, lambda event: self.removeScout(name)) |
||
450 | 9a4d8398 | Yuyang Guo | runButton.Bind(wx.EVT_BUTTON, |
451 | lambda event: self.changeBehavior(name, currBehaviorLabel, |
||
452 | pauseButton, # needed to handle press "run" during pause
|
||
453 | scoutChoices.GetStringSelection())) |
||
454 | pauseButton.Bind(wx.EVT_TOGGLEBUTTON, |
||
455 | lambda event: self.pauseResumeScout(name, pauseButton, |
||
456 | currBehaviorLabel, scoutChoices)) |
||
457 | ab8736ac | Yuyang Guo | teleopButton.Bind(wx.EVT_TOGGLEBUTTON, |
458 | lambda event: self.teleop(name)) |
||
459 | 1e52c76b | Yuyang Guo | sonar_vizButton.Bind(wx.EVT_TOGGLEBUTTON, |
460 | lambda event:
|
||
461 | self.sonar_viz(name, sonar_vizButton))
|
||
462 | 01f09975 | Yuyang Guo | self.mainArea.Add(self.sizer[name], proportion=1, |
463 | flag=wx.ALL | wx.EXPAND, border=10)
|
||
464 | self.window.Layout()
|
||
465 | return True |
||
466 | |||
467 | |||
468 | |||
469 | if __name__ == '__main__': |
||
470 | # open up GUI
|
||
471 | window = Window(title="Colony Scout Manager")
|
||
472 | window.MainLoop() |