Statistics
| Branch: | Revision:

root / scout / scoutsim / BehaviorGUI.py @ fa5ca6e3

History | View | Annotate | Download (8.98 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

    
11
# ROS imports
12
import roslib; roslib.load_manifest("scoutsim")
13
import rospy
14
from scoutsim.srv import *
15

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

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

    
38
def GetBehaviors():
39
    return ["CW Circle", "CCW Circle", "Odometry", "Navigation Map",
40
        "Scheduler", "Warehouse", "Line Follow", "WL Test"]
41

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

    
79
def KillScout(name):
80
    try:
81
        rospy.wait_for_service('/kill')
82
        service = rospy.ServiceProxy('/kill', Kill)
83
        if name in processes:
84
            subprocess.Popen("rosnode kill /" + name + "_behavior", shell=True)
85
            del processes[name]
86
        resp = service(name)
87
        if name in scouts:
88
            del scouts[name]
89
        return True
90
    except rospy.ServiceException, e:
91
        return False
92

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

    
228
if __name__ == '__main__':
229
    ScoutSimGui = subprocess.Popen("rosrun scoutsim scoutsim_node race", shell=True)
230
    rospy.wait_for_service('/spawn')
231
    app = wx.App()
232
    GUI(None, title='Colony Scout Manager')
233
    app.MainLoop()
234
    #Figure out how to terminate this for reals.
235
    for process in processes:
236
        process.terminate()
237
    subprocess.Popen("rosnode kill /scoutsim", shell=True)