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