Statistics
| Branch: | Revision:

root / scout / scoutsim / BehaviorGUI.py @ da8ace01

History | View | Annotate | Download (8.52 KB)

1 5755691e Matt Bryant
#!/usr/bin/python
2
3
import wx
4
import wx.lib.intctrl 
5 8fa2bd2e Matt Bryant
import wx.lib.agw.floatspin
6 5755691e Matt Bryant
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 27f73b95 Matt Bryant
# Everyone global variables (PLEASE REFACTOR ME)
16 8fa2bd2e Matt Bryant
scouts = defaultdict(str) # Dict in the form [scout_name: current_behavior]
17
processes = defaultdict() # Dict in the form [scout_name: process_handler]
18 5755691e Matt Bryant
19
#
20
# ROS functions
21
#
22
def AddToSimulator(name, x, y, theta):
23 6e8c4ad2 Alex
    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 5755691e Matt Bryant
35
def GetBehaviors():
36 6e8c4ad2 Alex
    return ["CW Circle", "CCW Circle", "Odometry", "Navigation Map",
37
        "Scheduler", "Warehouse", "Line Follow", "WL Test"]
38
39 da8ace01 Matt Bryant
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 6e8c4ad2 Alex
        else:
52 8fa2bd2e Matt Bryant
                        processes[name].terminate()
53
                        del processes[name]
54 da8ace01 Matt Bryant
                        subprocess.Popen("rosrun libscout libscout " + name + " 0", shell=True) 
55 8fa2bd2e Matt Bryant
                        print "Scout "+name+" was paused."
56
                        behaviorButton.SetLabel("Resume")
57
                        behaviorLabel.SetLabel("  |  Current Behavior: Paused")
58 da8ace01 Matt Bryant
                        return True
59
        if name in processes:
60
                processes[name].terminate()
61 73adca60 Alex
        # TODO Running a new behavior while paused does not do anything and
62
        # Gives lots of errors.
63 da8ace01 Matt Bryant
        #
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 5755691e Matt Bryant
75
def KillScout(name):
76 8fa2bd2e Matt Bryant
        try:
77
                rospy.wait_for_service('/kill')
78
                service = rospy.ServiceProxy('/kill', Kill)
79
                if name in processes:
80 da8ace01 Matt Bryant
                        processes[name].kill()
81 8fa2bd2e Matt Bryant
                        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 5755691e Matt Bryant
89 16d4e150 Priya
def PauseScout(name, behaviorLabel, behaviorButton, selectedBehavior):
90 6e8c4ad2 Alex
    return SetBehavior(name, behaviorLabel, behaviorButton,
91
        "Pause", selectedBehavior)
92 5755691e Matt Bryant
#
93
# End ROS functions
94
#
95
96
class GUI(wx.Frame):
97 6e8c4ad2 Alex
    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 8fa2bd2e Matt Bryant
        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 6e8c4ad2 Alex
        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 5755691e Matt Bryant
224
if __name__ == '__main__':
225 da8ace01 Matt Bryant
        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()