Statistics
| Branch: | Revision:

root / scout / scoutsim / BehaviorGUI.py @ 6e8c4ad2

History | View | Annotate | Download (8.91 KB)

1 5755691e Matt Bryant
#!/usr/bin/python
2
3
import wx
4
import wx.lib.intctrl 
5
import string
6
import subprocess
7
import signal
8
from collections import defaultdict
9
# ROS imports
10
import roslib; roslib.load_manifest("scoutsim")
11
import rospy
12
from scoutsim.srv import *
13
14 27f73b95 Matt Bryant
# Everyone global variables (PLEASE REFACTOR ME)
15 5755691e Matt Bryant
scouts = defaultdict(str) # Associative array in the form [scout_name: current_behavior]
16
processes = defaultdict() # Associative array in the form [scout_name: process_handler]
17
18
#
19
# ROS functions
20
#
21
def AddToSimulator(name, x, y, theta):
22 6e8c4ad2 Alex
    if name in scouts:
23
        return False
24
    scouts[name] = ""
25
    print "Calling ROS function to create scout "+name
26
    try:
27
        rospy.wait_for_service('/spawn');
28
        service = rospy.ServiceProxy('/spawn', Spawn);
29
        resp = service(x, y, theta, name)
30
        return True
31
    except rospy.ServiceException, e:
32
        return False
33 5755691e Matt Bryant
34
def GetBehaviors():
35 6e8c4ad2 Alex
    return ["CW Circle", "CCW Circle", "Odometry", "Navigation Map",
36
        "Scheduler", "Warehouse", "Line Follow", "WL Test"]
37
38
def SetBehavior(name, behaviorLabel, behaviorButton=False, behavior="Pause",
39
        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 + " "
46
                + str(GetBehaviors().index(selectedBehavior) + 1) + "",
47
                shell=True)
48
            print "Scout " + name + " was resumed."
49
            behaviorButton.SetLabel("Pause")
50
            behaviorLabel.SetLabel("  |  Current Behavior: " + scouts[name])
51
        else:
52
            processes[name].kill()
53
            processes[name].wait()
54
            subprocess.Popen("rosrun libscout libscout " + name + " 0",
55
                shell=True) 
56
            print "Scout "+name+" was paused."
57
            behaviorButton.SetLabel("Resume")
58
            behaviorLabel.SetLabel("  |  Current Behavior: Paused")
59
        return True
60
    if name in processes:
61
        processes[name].kill()
62
        processes[name].wait()
63
        # Running a new behavior while paused does not do anything and
64
        # Gives lots of behaviors.
65
    processes[name] = subprocess.Popen("rosrun libscout libscout " + name + " "
66
        + str(GetBehaviors().index(behavior) + 1) + "",
67
        shell=True) 
68
    behaviorButton.SetValue(False)
69
    behaviorButton.SetLabel("Pause")
70
    print "Scout " + name + " was set to behavior: " + behavior
71
    behaviorLabel.SetLabel("  |  Current Behavior: " + behavior)
72
    scouts[name] = behavior
73 5755691e Matt Bryant
74
def KillScout(name):
75 6e8c4ad2 Alex
    try:
76
        rospy.wait_for_service('/kill');
77
        service = rospy.ServiceProxy('/kill', Kill);
78
        if name in processes:
79
            # TODO BUG: Pausing a scout and then killing it crashes the program.
80
            processes[name].kill() # This is perhaps not working
81
        resp = service(name)
82
        del scouts[name]
83
        del processes[name]
84
        return True
85
    except rospy.ServiceException, e:
86
        return False
87 5755691e Matt Bryant
88 16d4e150 Priya
def PauseScout(name, behaviorLabel, behaviorButton, selectedBehavior):
89 6e8c4ad2 Alex
    return SetBehavior(name, behaviorLabel, behaviorButton,
90
        "Pause", selectedBehavior)
91 5755691e Matt Bryant
#
92
# End ROS functions
93
#
94
95
class GUI(wx.Frame):
96 6e8c4ad2 Alex
    def __init__(self, parent, title):
97
        super(GUI, self).__init__(parent, title=title,
98
            style=wx.DEFAULT_FRAME_STYLE ^ wx.RESIZE_BORDER, size=(600, 600))
99
        self.InitUI()
100
        self.Show()     
101
        self.AddScoutBox("scout1")     
102
103
    def InitUI(self):
104
        self.window = wx.ScrolledWindow(self, style=wx.VSCROLL)
105
        self.mainArea = wx.GridSizer(cols=1)
106
        sizer = wx.FlexGridSizer(rows=4, cols=8, hgap=5, vgap=5)
107
108
        # Labels
109
        blankText = wx.StaticText(self.window, label="")
110
        newScout = wx.StaticText(self.window, label="New Scout")
111
        newScoutName = wx.StaticText(self.window, label="Name:")
112
        startXTitle = wx.StaticText(self.window, label="X:")
113
        startYTitle = wx.StaticText(self.window, label="Y:")
114
        startThetaTitle = wx.StaticText(self.window, label="Rad:")
115
116
        # Inputs
117
        newScoutInput = wx.TextCtrl(self.window)
118
        startX = wx.lib.intctrl.IntCtrl(self.window, min=0)
119
        startY = wx.lib.intctrl.IntCtrl(self.window, min=0)
120
        startTheta = wx.lib.intctrl.IntCtrl(self.window, min=0, max=100)
121
        addButton = wx.Button(self.window, id=wx.ID_ADD)
122
123
        # Pretty Stuff
124
        hLine = wx.StaticLine(self.window, size=(600, 5))
125
        bottomHLine = wx.StaticLine(self.window, size=(600, 5))
126
127
        # Row 0
128
        sizer.Add(newScout)
129
        for i in range(7):
130
            sizer.AddStretchSpacer(1)
131
        # Row 1
132
        sizer.AddMany([newScoutName, (newScoutInput, 0, wx.EXPAND), startXTitle,
133
            startX, startYTitle, startY, startThetaTitle, startTheta])
134
        # Row 2
135
        for i in range(7):
136
            sizer.AddStretchSpacer(1)
137
        sizer.Add(addButton)
138
        # Row 3
139
140
        # Events
141
        addButton.Bind(wx.EVT_BUTTON, lambda event: self.NewScout(event,
142
            newScoutInput, startX, startY, startTheta))
143
144
        sizer.AddGrowableCol(idx=1)
145
        self.mainArea.Add(sizer, proportion=1, flag=wx.ALL|wx.EXPAND, border=10)
146
        self.window.SetSizer(self.mainArea)
147
        self.sizer = defaultdict()
148
149
    def NewScout(self, e, name, x, y, theta):
150
        if (name):
151
            scoutCreated = self.AddScout(name.GetValue(), x.GetValue(),
152
                y.GetValue(), theta.GetValue())
153
            if (scoutCreated):
154
                wx.MessageBox("Scout '" + name.GetValue()
155
                    + "' created successfully at (" + str(x.GetValue()) + ", "
156
                    + str(y.GetValue()) + ") facing " + str(theta.GetValue())
157
                    + u"\u00B0.", 'Scout Created', wx.OK | wx.ICON_INFORMATION)
158
                name.SetValue("")
159
                x.SetValue(0)
160
                y.SetValue(0)
161
                theta.SetValue(0)
162
                return True
163
        wx.MessageBox('Error creating scout.  Please check that all information'
164
            'was entered properly and there are no duplicate scout names.',
165
            'Info', wx.OK | wx.ICON_ERROR)
166
    
167
    def AddScoutBox(self, name):
168
        self.sizer[name] = wx.FlexGridSizer(rows=2, cols=4, hgap=5, vgap=5)
169
        # Labels
170
        scoutName = wx.StaticText(self.window, label="Scout: " + name)
171
        behaviorLabel = wx.StaticText(self.window, label="Behavior: ")
172
        currBehaviorLabel = wx.StaticText(self.window,
173
            label="  |  Current Behavior: Slacking off")
174
175
        # Inputs
176
        scoutChoices = wx.Choice(self.window, choices=GetBehaviors())
177
        pauseButton = wx.ToggleButton(self.window, label="Pause")
178
        runButton = wx.Button(self.window, label="Run")
179
        killButton = wx.Button(self.window, label="Kill")
180
181
        self.sizer[name].Add(scoutName)
182
        self.sizer[name].Add(currBehaviorLabel, wx.EXPAND | wx.ALIGN_RIGHT)
183
        self.sizer[name].AddStretchSpacer(1)
184
        self.sizer[name].Add(killButton, wx.ALIGN_RIGHT)
185
186
        self.sizer[name].Add(behaviorLabel)
187
        self.sizer[name].Add(scoutChoices, wx.EXPAND)
188
        self.sizer[name].Add(runButton)
189
        self.sizer[name].Add(pauseButton, wx.ALIGN_RIGHT)
190
        
191
        # Events
192
        pauseButton.Bind(wx.EVT_TOGGLEBUTTON,
193
            lambda event: PauseScout(name, currBehaviorLabel, pauseButton,
194
                scoutChoices.GetStringSelection()))
195
        runButton.Bind(wx.EVT_BUTTON,
196
            lambda event: SetBehavior(name, currBehaviorLabel, pauseButton,
197
                scoutChoices.GetStringSelection()))
198
        killButton.Bind(wx.EVT_BUTTON, lambda event: self.RemoveScout(name))
199
200
        self.mainArea.Add(self.sizer[name], proportion=1,
201
            flag=wx.ALL | wx.EXPAND, border=10)
202
        self.window.Layout()
203
        return True
204
205
    def AddScout(self, name, x, y, theta):
206
        if AddToSimulator(name, x, y, theta):
207
            return self.AddScoutBox(name)
208
        wx.MessageBox("ROS call failed!", 'ROS Error', wx.OK | wx.ICON_ERROR)
209
        return False
210
211
    def RemoveScout(self, name):
212
        # I don't trust this bit. Remove() should work by itself, 
213
        # but it doesn't want to. Hide()ing before Remove()ing does work 
214
        # for some reason though. It makes me think  the sizer is still around
215
        # somewhere, sneakily leaking memory. Alas, such is life.
216
        print("Hidden scoutLayout for " + name + ": "
217
            + str(self.mainArea.Hide(self.sizer[name], True)))
218
        print("Removed scoutLayout for " + name + ": "
219
            + str(self.mainArea.Remove(self.sizer[name])) + " (Maybe...)")
220
        del self.sizer[name]
221
        if KillScout(name):
222
            self.window.Layout()
223
            self.window.Refresh()
224
            self.window.Update()
225
        return
226 5755691e Matt Bryant
227
if __name__ == '__main__':
228 6e8c4ad2 Alex
    subprocess.Popen("rosrun scoutsim scoutsim_node race", shell=True)
229
    rospy.wait_for_service('/spawn')
230
    subprocess.Popen("rosservice call /spawn 1.4 0.275 0 scout1", shell=True)
231
    app = wx.App()
232
    GUI(None, title='Colony Scout Manager')
233
    app.MainLoop()