Statistics
| Branch: | Revision:

root / scout / scoutsim / BehaviorGUI.py @ d2dedb17

History | View | Annotate | Download (8.87 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 in scouts:
25
        return False
26
    scouts[name] = ""
27
    print "Calling ROS function to create scout "+name
28
    try:
29
        rospy.wait_for_service('/spawn');
30
        service = rospy.ServiceProxy('/spawn', Spawn);
31
        resp = service(x, y, theta, name)
32
        return True
33
    except rospy.ServiceException, e:
34
        return False
35

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

    
40
def SetBehavior(name, behaviorLabel, behaviorButton=False, behavior="Pause", 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 + " " + 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
            return True
52
        else:
53
            if name in processes:
54
                processes[name].terminate()
55
                del processes[name]
56
            subprocess.Popen("rosrun libscout libscout " + name + " 0", shell=True) 
57
            print "Scout "+name+" was paused."
58
            behaviorButton.SetLabel("Resume")
59
            behaviorLabel.SetLabel("  |  Current Behavior: Paused")
60
            return True
61
    if name in processes:
62
        processes[name].terminate()
63
        # TODO Running a new behavior while paused does not do anything and
64
        # Gives lots of errors.
65
    #
66
    # -- Matt 11/30 --
67
    # Might be fixed, can't run it to tell.
68
    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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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