Statistics
| Branch: | Revision:

root / scout / scoutsim / BehaviorGUI.py @ 8fa2bd2e

History | View | Annotate | Download (8.64 KB)

1
#!/usr/bin/python
2

    
3
import wx
4
import wx.lib.intctrl 
5
import wx.lib.agw.floatspin
6
import string
7
import subprocess
8
import signal
9
from collections import defaultdict
10
# ROS imports
11
import roslib; roslib.load_manifest("scoutsim")
12
import rospy
13
from scoutsim.srv import *
14

    
15
# Everyone global variables (PLEASE REFACTOR ME)
16
scouts = defaultdict(str) # Dict in the form [scout_name: current_behavior]
17
processes = defaultdict() # Dict in the form [scout_name: process_handler]
18

    
19
#
20
# ROS functions
21
#
22
def AddToSimulator(name, x, y, theta):
23
    if name in scouts:
24
        return False
25
    scouts[name] = ""
26
    print "Calling ROS function to create scout "+name
27
    try:
28
        rospy.wait_for_service('/spawn');
29
        service = rospy.ServiceProxy('/spawn', Spawn);
30
        resp = service(x, y, theta, name)
31
        return True
32
    except rospy.ServiceException, e:
33
        return False
34

    
35
def GetBehaviors():
36
    return ["CW Circle", "CCW Circle", "Odometry", "Navigation Map",
37
        "Scheduler", "Warehouse", "Line Follow", "WL Test"]
38

    
39
def SetBehavior(name, behaviorLabel, behaviorButton=False, behavior="Pause",
40
        selectedBehavior="Pause"):
41
    if behavior == "Pause":
42
        if name not in processes:
43
            return False
44
        if not behaviorButton.GetValue():
45
            processes[name] = subprocess.Popen("rosrun libscout libscout "
46
                + name + " "
47
                + str(GetBehaviors().index(selectedBehavior) + 1) + "",
48
                shell=True)
49
            print "Scout " + name + " was resumed."
50
            behaviorButton.SetLabel("Pause")
51
            behaviorLabel.SetLabel("  |  Current Behavior: " + scouts[name])
52
        else:
53
                        processes[name].terminate()
54
                        del processes[name]
55
                        subprocess.Popen("rosrun libscout libscout " + name + " 0",
56
                                shell=True) 
57
                        print "Scout "+name+" was paused."
58
                        behaviorButton.SetLabel("Resume")
59
                        behaviorLabel.SetLabel("  |  Current Behavior: Paused")
60
        return True
61
    if name in processes:
62
        processes[name].terminate()
63
        # TODO Running a new behavior while paused does not do anything and
64
        # Gives lots of errors.
65
                #
66
                # -- Matt 11/30 --
67
                # Might be fixed, can't run it to tell.
68
    processes[name] = subprocess.Popen("rosrun libscout libscout " + name + " "
69
        + str(GetBehaviors().index(behavior) + 1) + "",
70
        shell=True) 
71
    behaviorButton.SetValue(False)
72
    behaviorButton.SetLabel("Pause")
73
    print "Scout " + name + " was set to behavior: " + behavior
74
    behaviorLabel.SetLabel("  |  Current Behavior: " + behavior)
75
    scouts[name] = behavior
76

    
77
def KillScout(name):
78
        try:
79
                rospy.wait_for_service('/kill')
80
                service = rospy.ServiceProxy('/kill', Kill)
81
                if name in processes:
82
                        # TODO BUG: Pausing a scout and then killing it crashes the program.
83
                        #
84
                        # -- Matt 11/30 --
85
                        # Should be fixed
86
                        processes[name].terminate()
87
                        del processes[name]
88
                resp = service(name)
89
                if name in scouts:
90
                        del scouts[name]
91
                return True
92
        except rospy.ServiceException, e:
93
                return False
94

    
95
def PauseScout(name, behaviorLabel, behaviorButton, selectedBehavior):
96
    return SetBehavior(name, behaviorLabel, behaviorButton,
97
        "Pause", selectedBehavior)
98
#
99
# End ROS functions
100
#
101

    
102
class GUI(wx.Frame):
103
    def __init__(self, parent, title):
104
        super(GUI, self).__init__(parent, title=title,
105
            style=wx.DEFAULT_FRAME_STYLE ^ wx.RESIZE_BORDER, size=(600, 600))
106
        self.InitUI()
107
        self.Show()     
108
        self.AddScoutBox("scout1")     
109

    
110
    def InitUI(self):
111
        self.window = wx.ScrolledWindow(self, style=wx.VSCROLL)
112
        self.mainArea = wx.GridSizer(cols=1)
113
        sizer = wx.FlexGridSizer(rows=4, cols=8, hgap=5, vgap=5)
114

    
115
        # Labels
116
        blankText = wx.StaticText(self.window, label="")
117
        newScout = wx.StaticText(self.window, label="New Scout")
118
        newScoutName = wx.StaticText(self.window, label="Name:")
119
        startXTitle = wx.StaticText(self.window, label="X:")
120
        startYTitle = wx.StaticText(self.window, label="Y:")
121
        startThetaTitle = wx.StaticText(self.window, label="Rad:")
122

    
123
        # Inputs
124
        newScoutInput = wx.TextCtrl(self.window)
125
        startX = wx.lib.agw.floatspin.FloatSpin(self.window, min_val=0, digits=5)
126
        startY = wx.lib.agw.floatspin.FloatSpin(self.window, min_val=0, digits=5)
127
        startTheta = wx.lib.agw.floatspin.FloatSpin(self.window, min_val=0, digits=5)
128
        addButton = wx.Button(self.window, id=wx.ID_ADD)
129

    
130
        # Pretty Stuff
131
        hLine = wx.StaticLine(self.window, size=(600, 5))
132
        bottomHLine = wx.StaticLine(self.window, size=(600, 5))
133

    
134
        # Row 0
135
        sizer.Add(newScout)
136
        for i in range(7):
137
            sizer.AddStretchSpacer(1)
138
        # Row 1
139
        sizer.AddMany([newScoutName, (newScoutInput, 0, wx.EXPAND), startXTitle,
140
            startX, startYTitle, startY, startThetaTitle, startTheta])
141
        # Row 2
142
        for i in range(7):
143
            sizer.AddStretchSpacer(1)
144
        sizer.Add(addButton)
145
        # Row 3
146

    
147
        # Events
148
        addButton.Bind(wx.EVT_BUTTON, lambda event: self.NewScout(event,
149
            newScoutInput, startX, startY, startTheta))
150

    
151
        sizer.AddGrowableCol(idx=1)
152
        self.mainArea.Add(sizer, proportion=1, flag=wx.ALL|wx.EXPAND, border=10)
153
        self.window.SetSizer(self.mainArea)
154
        self.sizer = defaultdict()
155

    
156
    def NewScout(self, e, name, x, y, theta):
157
        if (name):
158
            scoutCreated = self.AddScout(name.GetValue(), x.GetValue(),
159
                y.GetValue(), theta.GetValue())
160
            if (scoutCreated):
161
                wx.MessageBox("Scout '" + name.GetValue()
162
                    + "' created successfully at (" + str(x.GetValue()) + ", "
163
                    + str(y.GetValue()) + ") facing " + str(theta.GetValue())
164
                    + u"\u00B0.", 'Scout Created', wx.OK | wx.ICON_INFORMATION)
165
                name.SetValue("")
166
                x.SetValue(0)
167
                y.SetValue(0)
168
                theta.SetValue(0)
169
                return True
170
        wx.MessageBox('Error creating scout.  Please check that all information'
171
            'was entered properly and there are no duplicate scout names.',
172
            'Info', wx.OK | wx.ICON_ERROR)
173
    
174
    def AddScoutBox(self, name):
175
        self.sizer[name] = wx.FlexGridSizer(rows=2, cols=4, hgap=5, vgap=5)
176
        # Labels
177
        scoutName = wx.StaticText(self.window, label="Scout: " + name)
178
        behaviorLabel = wx.StaticText(self.window, label="Behavior: ")
179
        currBehaviorLabel = wx.StaticText(self.window,
180
            label="  |  Current Behavior: Slacking off")
181

    
182
        # Inputs
183
        scoutChoices = wx.Choice(self.window, choices=GetBehaviors())
184
        pauseButton = wx.ToggleButton(self.window, label="Pause")
185
        runButton = wx.Button(self.window, label="Run")
186
        killButton = wx.Button(self.window, label="Kill")
187

    
188
        self.sizer[name].Add(scoutName)
189
        self.sizer[name].Add(currBehaviorLabel, wx.EXPAND | wx.ALIGN_RIGHT)
190
        self.sizer[name].AddStretchSpacer(1)
191
        self.sizer[name].Add(killButton, wx.ALIGN_RIGHT)
192

    
193
        self.sizer[name].Add(behaviorLabel)
194
        self.sizer[name].Add(scoutChoices, wx.EXPAND)
195
        self.sizer[name].Add(runButton)
196
        self.sizer[name].Add(pauseButton, wx.ALIGN_RIGHT)
197
        
198
        # Events
199
        pauseButton.Bind(wx.EVT_TOGGLEBUTTON,
200
            lambda event: PauseScout(name, currBehaviorLabel, pauseButton,
201
                scoutChoices.GetStringSelection()))
202
        runButton.Bind(wx.EVT_BUTTON,
203
            lambda event: SetBehavior(name, currBehaviorLabel, pauseButton,
204
                scoutChoices.GetStringSelection()))
205
        killButton.Bind(wx.EVT_BUTTON, lambda event: self.RemoveScout(name))
206

    
207
        self.mainArea.Add(self.sizer[name], proportion=1,
208
            flag=wx.ALL | wx.EXPAND, border=10)
209
        self.window.Layout()
210
        return True
211

    
212
    def AddScout(self, name, x, y, theta):
213
        if AddToSimulator(name, x, y, theta):
214
            return self.AddScoutBox(name)
215
        wx.MessageBox("ROS call failed!", 'ROS Error', wx.OK | wx.ICON_ERROR)
216
        return False
217

    
218
    def RemoveScout(self, name):
219
        print("Hidden scoutLayout for " + name + ": "
220
            + str(self.mainArea.Hide(self.sizer[name], True)))
221
        print("Removed scoutLayout for " + name + ": "
222
            + str(self.mainArea.Remove(self.sizer[name])) + " (Maybe...)")
223
        del self.sizer[name]
224
        if KillScout(name):
225
            self.window.Layout()
226
            self.window.Refresh()
227
            self.window.Update()
228
        return
229

    
230
if __name__ == '__main__':
231
    subprocess.Popen("rosrun scoutsim scoutsim_node race", shell=True)
232
    rospy.wait_for_service('/spawn')
233
    subprocess.Popen("rosservice call /spawn 1.4 0.275 0 scout1", shell=True)
234
    app = wx.App()
235
    GUI(None, title='Colony Scout Manager')
236
    app.MainLoop()