Statistics
| Branch: | Revision:

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

History | View | Annotate | Download (8.64 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
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 8fa2bd2e Matt Bryant
                        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 6e8c4ad2 Alex
        return True
61
    if name in processes:
62 8fa2bd2e Matt Bryant
        processes[name].terminate()
63 73adca60 Alex
        # TODO Running a new behavior while paused does not do anything and
64
        # Gives lots of errors.
65 8fa2bd2e Matt Bryant
                #
66
                # -- Matt 11/30 --
67
                # Might be fixed, can't run it to tell.
68 6e8c4ad2 Alex
    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 5755691e Matt Bryant
77
def KillScout(name):
78 8fa2bd2e Matt Bryant
        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 5755691e Matt Bryant
95 16d4e150 Priya
def PauseScout(name, behaviorLabel, behaviorButton, selectedBehavior):
96 6e8c4ad2 Alex
    return SetBehavior(name, behaviorLabel, behaviorButton,
97
        "Pause", selectedBehavior)
98 5755691e Matt Bryant
#
99
# End ROS functions
100
#
101
102
class GUI(wx.Frame):
103 6e8c4ad2 Alex
    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 8fa2bd2e Matt Bryant
        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 6e8c4ad2 Alex
        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 5755691e Matt Bryant
230
if __name__ == '__main__':
231 6e8c4ad2 Alex
    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()