Statistics
| Branch: | Revision:

scoutos / scout / scoutsim / GUI.py @ 1e52c76b

History | View | Annotate | Download (16.2 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
roslib.load_manifest("scoutsim")
23

    
24
# scoutsim imports
25
from scoutsim.srv import *
26

    
27
from collections import defaultdict
28

    
29

    
30
class Behaviors(object):
31
    def __init__(self):
32
        print "Warning: You should not need to create an instance of Behaviors"
33

    
34
    # this takes advantage of the fact that our behavior list start at 1    
35
    behaviors = ["Pause", "CW Circle", "CCW Circle", "Odometry",
36
                          "Navigation Map", "Scheduler", "Warehouse",
37
                          "Line Follow", "WL Test", "Maze Solve"]
38
    @classmethod
39
    def getNumber(self, behaviorName):
40
        if (behaviorName in Behaviors.behaviors):
41
            return Behaviors.behaviors.index(behaviorName)
42
    
43
    @classmethod
44
    def getName(self, index):
45
        if (0 <= index < len(self.Behavior)):
46
            return Behaviors.behaviors[index]
47
        else:
48
            return -1
49
    
50
    @classmethod
51
    def getBehaviors(self):
52
        # "Pause" is not considered a behavior in GUI
53
        return Behaviors.behaviors[1:]
54

    
55
# each scout is represented by this class
56
class Scout(object):
57
    numOfScouts = 0 # class variable keeping track of scouts
58
    
59
    def __init__(self, x=0, y=0, theta=0, name=u"",
60
                       Scouts=None, oninit = False):
61
        Scout.numOfScouts += 1 # reverted if not successful
62
        if len(name) == 0:
63
            # automatically give name according to numOfScouts
64
            name = self.autoNameGen(Scouts)
65
        # set vars
66
        self.name = name
67
        self.behavior = "chilling"
68
        self.process =  None# used to keep track of all processes
69
                        # related to this scout  
70
        self.paused = False
71
        self.valid = False
72
        # these vars are right now just the original position
73
        # but when ODOMETRY is done, we could actually use them
74
        self.x, self.y = x, y 
75
        self.theta = theta
76
        
77
        # spawn the scout if it is not scout1 (automatically spawned)
78
        if (not oninit): 
79
            if (self.SpawnInSimulator()):
80
                # spawned correctly
81
                self.valid = True
82
            else:
83
                Scout.numOfScouts -= 1;
84
        
85

    
86
    def autoNameGen(self, Scouts):
87
        i = 1
88
        while (True):
89
            if not (("scout%d"%i) in Scouts):
90
                return "scout%d"%i
91
            i+=1
92

    
93

    
94
        # update the classvariable
95
        
96

    
97
    def SpawnInSimulator(self):
98
        try:
99
            # ros spawn routine
100
            rospy.wait_for_service('/spawn');
101
            service = rospy.ServiceProxy('/spawn', Spawn);
102
            response = service(self.x, self.y, self.theta, self.name)
103
            return True
104
        except rospy.ServiceException as Error:
105
            return False
106
   
107
    def terminateOldBehavior(self):
108
        if self.process != None:
109
            # now terminate the process
110
            self.process.terminate() #TODO: this kills the process
111
                                     #      but maybe not the rosnode
112
                                     #      change to "rosnode kill" instead
113
            # make sure old behavior is terminated before we run the new one
114
            self.process.wait()
115

    
116
            # the following lines causes lag and does not solve real issues
117
            # first unregister the node
118
            cmd = "rosnode kill /%sBehavior"%self.name
119
            temp = subprocess.Popen(cmd, shell=True)
120
            temp.wait() 
121
            # this node loses contact but still is not cleaned up in rosnode
122
            # however, this should not affect usage at all.
123
        
124
    def changeBehavior(self, behavior):
125
        self.behavior = behavior
126
        if (behavior == "pause"):
127
            self.paused = True
128
        else:
129
            self.paused = False
130
        self.terminateOldBehavior()
131
        # do rosprocess calls for new behavior
132
        roscommand = ("rosrun libscout libscout %s %s"%
133
                            (Behaviors.getNumber(self.behavior), self.name))
134
        self.process = subprocess.Popen(roscommand, shell=True)
135

    
136
    def teleop(self):
137
        # teleop involved only one command \
138
        self.terminateOldBehavior()
139
        self.process = None
140
        cmd = "rosservice call /set_teleop %s"%self.name
141
        try:
142
            subprocess.Popen(shlex.split(cmd), shell=False)
143
        except:
144
            #not sure why this is happening....
145
            # seems to be a ros thing
146
            print "warning, socket error, ignored"
147
    
148
    def sonar_viz(self, on_off):
149
        # teleop involved only one command \
150
        cmd = "rosservice call /set_sonar_viz %s %s"%(on_off, self.name)
151
        try:
152
            subprocess.Popen(shlex.split(cmd), shell=False)
153
        except:
154
            #not sure why this is happening....
155
            # seems to be a ros thing
156
            print "warning, socket error, ignored"
157

    
158
    def killSelf(self):
159
        # terminate its current behavior
160
        self.terminateOldBehavior()
161
        
162
        # ros call to kill scout in simulator
163
        try:
164
            rospy.wait_for_service("/kill")
165
            service = rospy.ServiceProxy("/kill", Kill)
166
            response = service(self.name)
167
            Scout.numOfScouts -= 1
168
            self.valid = False
169
        except rospy.ServiceException, e:
170
            print "warning: kill scout unsuccessful" 
171
            self.valid = True
172
        # remove from the class
173

    
174

    
175
# a wrapper for wx Frame
176
class Window(wx.App):
177
    def __init__(self, title=""):
178
        super(Window, self).__init__()
179
        self.initUIFrame()
180

    
181
    def initUIFrame(self):
182
        self.GUIFrame = GUI(None, "hallo!")
183

    
184

    
185
# actual GUI frame
186
class GUI(wx.Frame):
187
    def __init__(self, parent, title):
188
        super(GUI, self).__init__(parent, title=title,
189
            style=wx.DEFAULT_FRAME_STYLE ^ wx.RESIZE_BORDER, size=(600, 600))
190
        
191
        # open up scoutsim race
192
        if len(sys.argv) > 1:
193
            map_name = sys.argv[1]
194
        else:
195
            map_name = "race"
196
        command = shlex.split("rosrun scoutsim scoutsim_node " + map_name)
197
        self.simWindowProcess = subprocess.Popen(command, shell=False)
198
        #rospy.wait_for_service('/spawn') #don't know why this is here
199
        
200
        # register call back for on close cleanup
201
        self.Bind(wx.EVT_CLOSE, self.onClose) 
202

    
203

    
204
        self.initData()
205
        self.initUI()
206
        self.Show(True)     
207
    
208
    # do clean up after user close the window
209
    def onClose(self, event):
210
        print "Cleaning up all Processes"
211
        
212
        # kill all the behavior processes
213
        for scout in self.scouts:
214
            process = self.scouts[scout].process
215
            if (process != None):
216
                process.terminate()
217

    
218
        # kill the scout sim window
219
        self.simWindowProcess.terminate()
220
        print "done"
221
        self.Destroy()
222

    
223
    def initData(self):
224
        self.scouts = {}
225
        #FIXME: these arguments are not right...fix them!!!
226
        self.scouts["scout1"] = Scout(x=0, y=0, theta=0, name="scout1",
227
                                      Scouts = self.scouts, oninit=True)
228

    
229
    # addButton callback
230
    def addScout(self, x_wx, y_wx, theta_wx, name_wx):
231
        # x, y, theta, name are given as wx Inputs
232
        x = x_wx.GetValue()
233
        y = y_wx.GetValue()
234
        theta = theta_wx.GetValue()
235
        name = name_wx.GetValue()
236
        newSc = Scout(x, y, theta, name, self.scouts)
237
        if (newSc.valid):
238
            # successful
239
            self.scouts[newSc.name] = newSc
240
            self.addScoutBox(newSc.name)
241
            # alert user spawn successful
242
            wx.MessageBox(u"Scout '%s' created successfully at (%d,%d,%d\u00B0)"
243
                            %(newSc.name, newSc.x, newSc.y, newSc.theta), 
244
                            "Scout Created", wx.OK | wx.ICON_INFORMATION)
245

    
246
        else:
247
            #failed to create scout
248
            wx.MessageBox("Scout Creation failed :( Check for duplicate name",
249
                            "Add Scout failed", wx.OK | wx.ICON_ERROR)
250

    
251
    def removeScout(self, name):
252
        sct = self.scouts[name]
253
        sct.killSelf()
254
        if (sct.valid == False):
255
            # successful, remove from the dictionary
256
            del self.scouts[name]
257
            # delete the refresh the display
258
            self.mainArea.Hide(self.sizer[name]) #required by wx before remove
259
            self.mainArea.Remove(self.sizer[name])
260
            del self.sizer[name]
261
            self.window.Layout()
262
            self.window.Refresh()
263
            self.window.Update()
264
        else:
265
            raise Exception
266
    
267
    # runPressed
268
    # change UI display and invoke change Behavior in scout
269
    def changeBehavior(self, name, currBehaviorLabel, 
270
                        pauseButton, newBehavior):
271
        
272
        # to handle user pressing "Run" during pause
273
        if (newBehavior != "Pause" and self.scouts[name].paused == True):
274
            self.correctPauseButton(name, 
275
                                        pauseButton, pauseToResume=True)
276
        
277
        currBehaviorLabel.SetLabel(" | Current Behavior: %s"%newBehavior)
278
        scout = self.scouts[name]
279
        scout.changeBehavior(newBehavior)
280
        self.correctTeleopButton(None)
281

    
282

    
283
    def buttonDown(button):
284
        return button.GetValue()
285

    
286
    def pauseResumeScout(self, name, pauseButton, currBehaviorLabel, dropMenu):
287
        if (pauseButton.GetValue() == True): # 
288
            # change behavior to Pause
289
            self.changeBehavior(name, currBehaviorLabel, pauseButton, "Pause")    
290
            # change button to "Resume" & label to "Pause"
291
            currBehaviorLabel.SetLabel("Pause")
292
            self.correctPauseButton(name, 
293
                                        pauseButton, pauseToResume=False)
294
        else:
295
            # resume
296
            # change behavior to whatever is in the drop down menu
297
            self.changeBehavior(name, currBehaviorLabel, pauseButton, 
298
                        dropMenu.GetStringSelection())
299
            self.correctPauseButton(name, 
300
                                        pauseButton, pauseToResume=True)
301

    
302
    def teleop(self, name):
303
        self.correctTeleopButton(name)
304
        self.scouts[name].teleop()
305

    
306
    def sonar_viz(self, name, sonar_vizButton):
307
        if (sonar_vizButton.GetValue() == True):
308
            # turn on the sonar viz
309
            self.scouts[name].sonar_viz("on")
310
        else:
311
            # turn off sonar viz
312
            self.scouts[name].sonar_viz("off")
313

    
314
    ############################ UI stuff here ###########################
315
    
316
    def initUI(self):
317
        self.allTeleopButtons = {}
318
        self.initAddScoutArea()
319
    
320
    def correctTeleopButton(self, teleopingName):
321
        for name in self.allTeleopButtons:
322
            if name == teleopingName:
323
                self.allTeleopButtons[name].SetValue(True)
324
            else:
325
                self.allTeleopButtons[name].SetValue(False)
326
                
327

    
328
    def correctPauseButton(self, name, pauseButton, pauseToResume):
329
        if (pauseToResume):
330
            print "correcting button!"
331
            self.scouts[name].paused = False
332
            pauseButton.SetValue(False) # unpress it
333
            pauseButton.SetLabel("Pause")
334
        else:
335
            self.scouts[name].paused = True
336
            pauseButton.SetValue(True) # press it
337
            pauseButton.SetLabel("Resume")
338

    
339
    # the labels and input boxes for adding a scout through GUI
340
    def initAddScoutArea(self):
341
        # all the layout stuff copied over: using grid layout
342
        # button callbacks are changed
343
        self.totalCols = 8
344
        self.window = wx.ScrolledWindow(self, style=wx.VSCROLL)
345
        self.mainArea = wx.GridSizer(cols=1)
346
        sizer = wx.FlexGridSizer(rows=4, cols=self.totalCols, hgap=5, vgap=5)
347

    
348
        # Labels
349
        blankText = wx.StaticText(self.window, label="")
350
        newScout = wx.StaticText(self.window, label="New Scout")
351
        newScoutName = wx.StaticText(self.window, label="Name:")
352
        startXTitle = wx.StaticText(self.window, label="X:")
353
        startYTitle = wx.StaticText(self.window, label="Y:")
354
        startThetaTitle = wx.StaticText(self.window, label="Rad:")
355

    
356
        # Inputs
357
        newScoutInput = wx.TextCtrl(self.window)
358
        startX = wx.lib.agw.floatspin.FloatSpin(self.window, min_val=0, digits=5)
359
        startY = wx.lib.agw.floatspin.FloatSpin(self.window, min_val=0, digits=5)
360
        startTheta = wx.lib.agw.floatspin.FloatSpin(self.window, min_val=0, digits=5)
361
        addButton = wx.Button(self.window, id=wx.ID_ADD)
362

    
363
        # Pretty Stuff
364
        hLine = wx.StaticLine(self.window, size=(600, 5))
365
        bottomHLine = wx.StaticLine(self.window, size=(600, 5))
366
        
367
        # Row 0: just the label add scout
368
        sizer.Add(newScout)
369
        for i in range(7):
370
            sizer.AddStretchSpacer(1)
371
        # Row 1: input(name), x coord, y coord, rad, 
372
        sizer.AddMany([newScoutName, (newScoutInput, 0, wx.EXPAND), startXTitle,
373
            startX, startYTitle, startY, startThetaTitle, startTheta])
374
        # Row 2: just the add button to the right
375
        for i in range(7):
376
            sizer.AddStretchSpacer(1)
377
        sizer.Add(addButton)
378
        # Row 3
379
        
380
        # Events
381
        addButton.Bind(wx.EVT_BUTTON, lambda event: self.addScout(
382
            startX, startY, startTheta, newScoutInput))
383

    
384
        sizer.AddGrowableCol(idx=1)
385
        self.mainArea.Add(sizer, proportion=1, flag=wx.ALL|wx.EXPAND, border=10)
386
        self.window.SetSizer(self.mainArea)
387
        self.sizer = defaultdict()
388
        self.window.Layout()
389
        
390
        # make the scout1's controller
391
        self.addScoutBox("scout1");
392
    
393
    
394

    
395
    # copied over from old GUI
396
    def addScoutBox(self, name):
397
        self.sizer[name] = wx.FlexGridSizer(rows=2, cols=5, hgap=5, vgap=5)
398
        # Labels
399
        scoutName = wx.StaticText(self.window, label="Scout: %s"%name)
400
        behaviorLabel = wx.StaticText(self.window, label="Behavior: ")
401
        currBehaviorLabel = wx.StaticText(self.window,
402
            label="  |  Current Behavior: Slacking off")
403

    
404
        # Inputs
405
        # drop down menue
406
        scoutChoices = wx.Choice(self.window, 
407
                                    choices=Behaviors.getBehaviors())
408
        #   buttons
409
        pauseButton = wx.ToggleButton(self.window, label="Pause")
410
        runButton = wx.Button(self.window, label="Run")
411
        killButton = wx.Button(self.window, label="Kill")
412
        teleopButton = wx.ToggleButton(self.window, label="Teleop")
413
        sonar_vizButton = wx.ToggleButton(self.window, label="sonar viz")
414
        self.allTeleopButtons[name] = teleopButton
415
        # row 0
416
        self.sizer[name].Add(scoutName)
417
        self.sizer[name].Add(currBehaviorLabel, wx.EXPAND | wx.ALIGN_RIGHT)
418
        self.sizer[name].AddStretchSpacer(1)
419
        self.sizer[name].Add(killButton, wx.ALIGN_RIGHT)
420
        self.sizer[name].Add(sonar_vizButton)
421

    
422
        # row 1
423
        self.sizer[name].Add(behaviorLabel)
424
        self.sizer[name].Add(scoutChoices, wx.EXPAND)
425
        self.sizer[name].Add(runButton)
426
        self.sizer[name].Add(pauseButton, wx.ALIGN_RIGHT)
427
        self.sizer[name].Add(teleopButton) 
428
        # Events
429
        killButton.Bind(wx.EVT_BUTTON, lambda event: self.removeScout(name))
430
        runButton.Bind(wx.EVT_BUTTON,
431
                lambda event: self.changeBehavior(name, currBehaviorLabel,
432
                    pauseButton, # needed to handle press "run" during pause
433
                    scoutChoices.GetStringSelection()))
434
        pauseButton.Bind(wx.EVT_TOGGLEBUTTON, 
435
            lambda event: self.pauseResumeScout(name, pauseButton,
436
                                        currBehaviorLabel, scoutChoices))
437
        teleopButton.Bind(wx.EVT_TOGGLEBUTTON, 
438
                                lambda event: self.teleop(name))
439
        sonar_vizButton.Bind(wx.EVT_TOGGLEBUTTON, 
440
                                lambda event: 
441
                                    self.sonar_viz(name, sonar_vizButton))
442
        self.mainArea.Add(self.sizer[name], proportion=1,
443
            flag=wx.ALL | wx.EXPAND, border=10)
444
        self.window.Layout()
445
        return True
446

    
447

    
448

    
449
if __name__ == '__main__':
450
    # open up GUI
451
    window = Window(title="Colony Scout Manager")
452
    window.MainLoop()