Project

General

Profile

Revision 9a4d8398

ID9a4d83982992ee7a284d9603303041ed4d8ea7ea

Added by Yuyang Guo about 11 years ago

New GUI should be working now! (run, pause resume should all work
pretty robustly). Teleop is not working yet...

View differences:

scout/scoutsim/GUI.py
25 25

  
26 26
from collections import defaultdict
27 27

  
28

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

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

  
28 54
# each scout is represented by this class
29 55
class Scout(object):
30 56
    numOfScouts = 0 # class variable keeping track of scouts
31 57
    
32 58
    def __init__(self, x=0, y=0, theta=0, name=u"",
33 59
                       Scouts=None, oninit = False):
34
        print (len(name))
35 60
        Scout.numOfScouts += 1 # reverted if not successful
36 61
        if len(name) == 0:
37 62
            # automatically give name according to numOfScouts
......
41 66
        self.behavior = "chilling"
42 67
        self.process =  None# used to keep track of all processes
43 68
                        # related to this scout  
69
        self.paused = False
44 70
        self.valid = False
45 71
        # these vars are right now just the original position
46 72
        # but when ODOMETRY is done, we could actually use them
......
68 94
        
69 95

  
70 96
    def SpawnInSimulator(self):
71
        print "trying to spawn scout!!!"
72 97
        try:
73 98
            # ros spawn routine
74 99
            rospy.wait_for_service('/spawn');
......
76 101
            response = service(self.x, self.y, self.theta, self.name)
77 102
            return True
78 103
        except rospy.ServiceException as Error:
79
            print "things are bad!!!"
80
            print Error
81 104
            return False
82
    
83
    def changeBehavior(self, behavior):
84
        self.behavior = behavior
105
   
106
    def terminateOldBehavior(self):
107
        if self.process != None:
108
            # now terminate the process
109
            self.process.terminate() #TODO: this kills the process
110
                                     #      but maybe not the rosnode
111
                                     #      change to "rosnode kill" instead
112
            # make sure old behavior is terminated before we run the new one
113
            self.process.wait()
85 114

  
86
        # WARNING: Check this but ROS kills the old behavior for us
87
            # kill the old behavior
88
#        if self.process != None:
89
#            self.process.terminate() #TODO: this kills the process
90
#                                     #      but maybe not the rosnode
91
#                                     #      change to "rosnode kill" instead
92
#        # make sure the old behavior is terminated before we run the new one
93
#        self.process.wait()
115
            # the following lines causes lag and does not solve real issues
116
            # first unregister the node
117
            #cmd = "rosnode kill /%s_behavior"%self.name
118
            #temp = subprocess.Popen(cmd, shell=True)
119
            #temp.wait() 
120
            # this node loses contact but still is not cleaned up in rosnode
121
            # however, this should not affect usage at all.
94 122
        
123
    def changeBehavior(self, behavior):
124
        self.behavior = behavior
125
        if (behavior == "pause"):
126
            self.paused = True
127
        else:
128
            self.paused = False
129
        self.terminateOldBehavior()
95 130
        # do rosprocess calls for new behavior
96 131
        roscommand = shlex.split("rosrun libscout libscout %s %s"%
97
                                            (self.name, self.behavior))
98
        subprocess.Popen(roscommand, shell=False)
132
                            (self.name, Behaviors.getNumber(self.behavior)))
133
        self.process = subprocess.Popen(roscommand, shell=False)
99 134

  
100 135
    def killSelf(self):
101 136
        # terminate its current behavior
102
        if self.process != None:
103
            self.process.terminate() # TODO: same here, try "rosnode kill"
104
            self.process.wait()
137
        self.terminateOldBehavior()
138
        
105 139
        # ros call to kill scout in simulator
106 140
        try:
107 141
            rospy.wait_for_service("/kill")
......
161 195

  
162 196
    def initData(self):
163 197
        self.scouts = {}
198
        #FIXME: these arguments are not right...fix them!!!
164 199
        self.scouts["scout1"] = Scout(x=0, y=0, theta=0, name="scout1",
165 200
                                      Scouts = self.scouts, oninit=True)
166
        #TODO: these arguments are not right...fix them!!!
167 201

  
168 202
    # addButton callback
169 203
    def addScout(self, x_wx, y_wx, theta_wx, name_wx):
......
172 206
        y = y_wx.GetValue()
173 207
        theta = theta_wx.GetValue()
174 208
        name = name_wx.GetValue()
175
        print "in GUI trying to add scout!"
176 209
        newSc = Scout(x, y, theta, name, self.scouts)
177 210
        if (newSc.valid):
178 211
            # successful
......
190 223

  
191 224
    def removeScout(self, name):
192 225
        sct = self.scouts[name]
193
        print "removing scout %s"%name
194 226
        sct.killSelf()
195 227
        if (sct.valid == False):
196 228
            # successful, remove from the dictionary
197 229
            del self.scouts[name]
198
            # refresh the display
199
            print self.mainArea.Hide(self.sizer[name])
200
            print self.mainArea.Remove(self.sizer[name])
230
            # delete the refresh the display
231
            self.mainArea.Hide(self.sizer[name]) #required by wx before remove
232
            self.mainArea.Remove(self.sizer[name])
201 233
            del self.sizer[name]
202 234
            self.window.Layout()
203 235
            self.window.Refresh()
......
205 237
        else:
206 238
            raise Exception
207 239

  
240
    # change UI display and invoke change Behavior in scout
241
    def changeBehavior(self, name, currBehaviorLabel, 
242
                        pauseButton, newBehavior):
243
        
244
        # to handle user pressing "Run" during pause
245
        if (newBehavior != "Pause" and self.scouts[name].paused == True):
246
            print "trying to do stuff!!!"
247
            self.correctPauseButton(name, 
248
                                        pauseButton, pauseToResume=True)
249
        
250
        currBehaviorLabel.SetLabel(" | Current Behavior: %s"%newBehavior)
251
        scout = self.scouts[name]
252
        scout.changeBehavior(newBehavior)
253

  
254

  
255
    def correctPauseButton(self, name, pauseButton, pauseToResume):
256
        if (pauseToResume):
257
            print "correcting button!"
258
            self.scouts[name].paused = False
259
            pauseButton.SetValue(False) # unpress it
260
            pauseButton.SetLabel("Pause")
261
        else:
262
            self.scouts[name].paused = True
263
            pauseButton.SetValue(True) # press it
264
            pauseButton.SetLabel("Resume")
265

  
266
    def buttonDown(button):
267
        return button.GetValue()
268

  
269
    def pauseResumeScout(self, name, pauseButton, currBehaviorLabel, dropMenu):
270
        if (pauseButton.GetValue() == True): # 
271
            # change behavior to Pause
272
            self.changeBehavior(name, currBehaviorLabel, pauseButton, "Pause")    
273
            # change button to "Resume" & label to "Pause"
274
            currBehaviorLabel.SetLabel("Pause")
275
            self.correctPauseButton(name, 
276
                                        pauseButton, pauseToResume=False)
277
        else:
278
            # resume
279
            # change behavior to whatever is in the drop down menu
280
            self.changeBehavior(name, currBehaviorLabel, pauseButton, 
281
                        dropMenu.GetStringSelection())
282
            self.correctPauseButton(name, 
283
                                        pauseButton, pauseToResume=True)
284
            
285

  
208 286
    ############################ UI stuff here ###########################
209 287
    
210 288
    def initUI(self):
......
268 346
    def addScoutBox(self, name):
269 347
        self.sizer[name] = wx.FlexGridSizer(rows=2, cols=5, hgap=5, vgap=5)
270 348
        # Labels
271
        scoutName = wx.StaticText(self.window, label="Scout: " + name)
349
        scoutName = wx.StaticText(self.window, label="Scout: %s"%name)
272 350
        behaviorLabel = wx.StaticText(self.window, label="Behavior: ")
273 351
        currBehaviorLabel = wx.StaticText(self.window,
274 352
            label="  |  Current Behavior: Slacking off")
275 353

  
276 354
        # Inputs
277 355
        # drop down menue
278
        scoutChoices = wx.Choice(self.window) #choices=GetBehaviors())
356
        scoutChoices = wx.Choice(self.window, 
357
                                    choices=Behaviors.getBehaviors())
279 358
        #   buttons
280 359
        pauseButton = wx.ToggleButton(self.window, label="Pause")
281 360
        runButton = wx.Button(self.window, label="Run")
......
297 376
        self.sizer[name].Add(teleopButton) 
298 377
        # Events
299 378
        killButton.Bind(wx.EVT_BUTTON, lambda event: self.removeScout(name))
379
        runButton.Bind(wx.EVT_BUTTON,
380
                lambda event: self.changeBehavior(name, currBehaviorLabel,
381
                    pauseButton, # needed to handle press "run" during pause
382
                    scoutChoices.GetStringSelection()))
383
        pauseButton.Bind(wx.EVT_TOGGLEBUTTON, 
384
            lambda event: self.pauseResumeScout(name, pauseButton,
385
                                        currBehaviorLabel, scoutChoices))
300 386

  
301 387
        self.mainArea.Add(self.sizer[name], proportion=1,
302 388
            flag=wx.ALL | wx.EXPAND, border=10)

Also available in: Unified diff