Statistics
| Branch: | Revision:

root / scout / scoutsim / BehaviorGUI.py @ da8ace01

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

    
75
def KillScout(name):
76
        try:
77
                rospy.wait_for_service('/kill')
78
                service = rospy.ServiceProxy('/kill', Kill)
79
                if name in processes:
80
                        processes[name].kill()
81
                        del processes[name]
82
                resp = service(name)
83
                if name in scouts:
84
                        del scouts[name]
85
                return True
86
        except rospy.ServiceException, e:
87
                return False
88

    
89
def PauseScout(name, behaviorLabel, behaviorButton, selectedBehavior):
90
    return SetBehavior(name, behaviorLabel, behaviorButton,
91
        "Pause", selectedBehavior)
92
#
93
# End ROS functions
94
#
95

    
96
class GUI(wx.Frame):
97
    def __init__(self, parent, title):
98
        super(GUI, self).__init__(parent, title=title,
99
            style=wx.DEFAULT_FRAME_STYLE ^ wx.RESIZE_BORDER, size=(600, 600))
100
        self.InitUI()
101
        self.Show()     
102
        self.AddScoutBox("scout1")     
103

    
104
    def InitUI(self):
105
        self.window = wx.ScrolledWindow(self, style=wx.VSCROLL)
106
        self.mainArea = wx.GridSizer(cols=1)
107
        sizer = wx.FlexGridSizer(rows=4, cols=8, hgap=5, vgap=5)
108

    
109
        # Labels
110
        blankText = wx.StaticText(self.window, label="")
111
        newScout = wx.StaticText(self.window, label="New Scout")
112
        newScoutName = wx.StaticText(self.window, label="Name:")
113
        startXTitle = wx.StaticText(self.window, label="X:")
114
        startYTitle = wx.StaticText(self.window, label="Y:")
115
        startThetaTitle = wx.StaticText(self.window, label="Rad:")
116

    
117
        # Inputs
118
        newScoutInput = wx.TextCtrl(self.window)
119
        startX = wx.lib.agw.floatspin.FloatSpin(self.window, min_val=0, digits=5)
120
        startY = wx.lib.agw.floatspin.FloatSpin(self.window, min_val=0, digits=5)
121
        startTheta = wx.lib.agw.floatspin.FloatSpin(self.window, min_val=0, digits=5)
122
        addButton = wx.Button(self.window, id=wx.ID_ADD)
123

    
124
        # Pretty Stuff
125
        hLine = wx.StaticLine(self.window, size=(600, 5))
126
        bottomHLine = wx.StaticLine(self.window, size=(600, 5))
127

    
128
        # Row 0
129
        sizer.Add(newScout)
130
        for i in range(7):
131
            sizer.AddStretchSpacer(1)
132
        # Row 1
133
        sizer.AddMany([newScoutName, (newScoutInput, 0, wx.EXPAND), startXTitle,
134
            startX, startYTitle, startY, startThetaTitle, startTheta])
135
        # Row 2
136
        for i in range(7):
137
            sizer.AddStretchSpacer(1)
138
        sizer.Add(addButton)
139
        # Row 3
140

    
141
        # Events
142
        addButton.Bind(wx.EVT_BUTTON, lambda event: self.NewScout(event,
143
            newScoutInput, startX, startY, startTheta))
144

    
145
        sizer.AddGrowableCol(idx=1)
146
        self.mainArea.Add(sizer, proportion=1, flag=wx.ALL|wx.EXPAND, border=10)
147
        self.window.SetSizer(self.mainArea)
148
        self.sizer = defaultdict()
149

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

    
176
        # Inputs
177
        scoutChoices = wx.Choice(self.window, choices=GetBehaviors())
178
        pauseButton = wx.ToggleButton(self.window, label="Pause")
179
        runButton = wx.Button(self.window, label="Run")
180
        killButton = wx.Button(self.window, label="Kill")
181

    
182
        self.sizer[name].Add(scoutName)
183
        self.sizer[name].Add(currBehaviorLabel, wx.EXPAND | wx.ALIGN_RIGHT)
184
        self.sizer[name].AddStretchSpacer(1)
185
        self.sizer[name].Add(killButton, wx.ALIGN_RIGHT)
186

    
187
        self.sizer[name].Add(behaviorLabel)
188
        self.sizer[name].Add(scoutChoices, wx.EXPAND)
189
        self.sizer[name].Add(runButton)
190
        self.sizer[name].Add(pauseButton, wx.ALIGN_RIGHT)
191
        
192
        # Events
193
        pauseButton.Bind(wx.EVT_TOGGLEBUTTON,
194
            lambda event: PauseScout(name, currBehaviorLabel, pauseButton,
195
                scoutChoices.GetStringSelection()))
196
        runButton.Bind(wx.EVT_BUTTON,
197
            lambda event: SetBehavior(name, currBehaviorLabel, pauseButton,
198
                scoutChoices.GetStringSelection()))
199
        killButton.Bind(wx.EVT_BUTTON, lambda event: self.RemoveScout(name))
200

    
201
        self.mainArea.Add(self.sizer[name], proportion=1,
202
            flag=wx.ALL | wx.EXPAND, border=10)
203
        self.window.Layout()
204
        return True
205

    
206
    def AddScout(self, name, x, y, theta):
207
        if AddToSimulator(name, x, y, theta):
208
            return self.AddScoutBox(name)
209
        wx.MessageBox("ROS call failed!", 'ROS Error', wx.OK | wx.ICON_ERROR)
210
        return False
211

    
212
    def RemoveScout(self, name):
213
        print("Hidden scoutLayout for " + name + ": "
214
            + str(self.mainArea.Hide(self.sizer[name], True)))
215
        print("Removed scoutLayout for " + name + ": "
216
            + str(self.mainArea.Remove(self.sizer[name])) + " (Maybe...)")
217
        del self.sizer[name]
218
        if KillScout(name):
219
            self.window.Layout()
220
            self.window.Refresh()
221
            self.window.Update()
222
        return
223

    
224
if __name__ == '__main__':
225
        ScoutSimGui = subprocess.Popen("rosrun scoutsim scoutsim_node race", shell=True)
226
        rospy.wait_for_service('/spawn')
227
        #subprocess.Popen("rosservice call /spawn 1.4 0.275 0 scout1", shell=True)  # why was this here?
228
        app = wx.App()
229
        GUI(None, title='Colony Scout Manager')
230
        app.MainLoop()
231
        # This doesn't run.  I'm sure there is a reason why.
232
        for process in processes:
233
                process.kill()
234
        ScoutSimGui.kill()