Statistics
| Branch: | Revision:

scoutos / scout / scoutsim / BehaviorGUI.py @ 9a88eb2e

History | View | Annotate | Download (7.08 KB)

1
#!/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
# Everyone global variables (PLEASE REFACTOR ME)
15
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
        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

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

    
37
def SetBehavior(name, behaviorLabel, behaviorButton=False, behavior="Pause"):
38
        if behavior == "Pause":
39
                if name not in processes:
40
                        return False
41
                if not behaviorButton.GetValue():
42
                        processes[name].send_signal(signal.SIGCONT)
43
                        print "Scout "+name+" was resumed."
44
                        behaviorButton.SetLabel("Pause")
45
                        behaviorLabel.SetLabel("  |  Current Behavior: "+scouts[name])
46
                else:
47
                        processes[name].send_signal(signal.SIGSTOP)
48
                        subprocess.Popen("rosrun libscout libscout "+name+" 0", shell=True) 
49
                        print "Scout "+name+" was paused."
50
                        behaviorButton.SetLabel("Resume")
51
                        behaviorLabel.SetLabel("  |  Current Behavior: Paused")
52
                return True
53
        if name in processes:
54
                processes[name].kill()
55
        processes[name] = subprocess.Popen("rosrun libscout libscout "+name+" "+str(GetBehaviors().index(behavior)+1)+"", shell=True) 
56
        behaviorButton.SetValue(False)
57
        behaviorButton.SetLabel("Pause")
58
        print "Scout "+name+" was set to behavior: "+behavior
59
        behaviorLabel.SetLabel("  |  Current Behavior: "+behavior)
60
        scouts[name] = behavior
61

    
62
def KillScout(name):
63
        try:
64
                rospy.wait_for_service('/kill');
65
                service = rospy.ServiceProxy('/kill', Kill);
66
                if name in processes:
67
                        processes[name].kill() # This is perhaps not working
68
                resp = service(name)
69
                del scouts[name]
70
                del processes[name]
71
                return True
72
        except rospy.ServiceException, e:
73
                return False
74

    
75
def PauseScout(name, behaviorLabel, behaviorButton):
76
        return SetBehavior(name, behaviorLabel, behaviorButton)
77
#
78
# End ROS functions
79
#
80

    
81
class GUI(wx.Frame):
82
        def __init__(self, parent, title):
83
                super(GUI, self).__init__(parent, title=title, style=wx.DEFAULT_FRAME_STYLE ^ wx.RESIZE_BORDER, size=(600, 600))
84
                self.InitUI()
85
                self.Show()     
86
                self.AddScoutBox("scout1")     
87

    
88
        def InitUI(self):
89
                self.window = wx.ScrolledWindow(self, style=wx.VSCROLL)
90
                self.mainArea = wx.GridSizer(cols=1)
91
                sizer = wx.FlexGridSizer(rows=4, cols=8, hgap=5, vgap=5)
92

    
93
                # Labels
94
                blankText = wx.StaticText(self.window, label="")
95
                newScout = wx.StaticText(self.window, label="New Scout")
96
                newScoutName = wx.StaticText(self.window, label="Name:")
97
                startXTitle = wx.StaticText(self.window, label="X:")
98
                startYTitle = wx.StaticText(self.window, label="Y:")
99
                startThetaTitle = wx.StaticText(self.window, label="Rad:")
100

    
101
                # Inputs
102
                newScoutInput = wx.TextCtrl(self.window)
103
                startX = wx.lib.intctrl.IntCtrl(self.window, min=0)
104
                startY = wx.lib.intctrl.IntCtrl(self.window, min=0)
105
                startTheta = wx.lib.intctrl.IntCtrl(self.window, min=0, max=100)
106
                addButton = wx.Button(self.window, id=wx.ID_ADD)
107

    
108
                # Pretty Stuff
109
                hLine = wx.StaticLine(self.window, size=(600, 5))
110
                bottomHLine = wx.StaticLine(self.window, size=(600, 5))
111

    
112
                # Row 0
113
                sizer.Add(newScout)
114
                for i in range(7):
115
                        sizer.AddStretchSpacer(1)
116
                # Row 1
117
                sizer.AddMany([newScoutName, (newScoutInput, 0, wx.EXPAND), startXTitle, startX, startYTitle, startY, startThetaTitle, startTheta])
118
                # Row 2
119
                for i in range(7):
120
                        sizer.AddStretchSpacer(1)
121
                sizer.Add(addButton)
122
                # Row 3
123

    
124
                # Events
125
                addButton.Bind(wx.EVT_BUTTON, lambda event: self.NewScout(event, newScoutInput, startX, startY, startTheta))
126

    
127
                sizer.AddGrowableCol(idx=1)
128
                self.mainArea.Add(sizer, proportion=1, flag=wx.ALL|wx.EXPAND, border=10)
129
                self.window.SetSizer(self.mainArea)
130
                self.sizer = defaultdict()
131

    
132
        def NewScout(self, e, name, x, y, theta):
133
                if (name):
134
                        scoutCreated = self.AddScout(name.GetValue(), x.GetValue(), y.GetValue(), theta.GetValue())
135
                        if (scoutCreated):
136
                                wx.MessageBox("Scout '"+name.GetValue()+"' created successfully at ("+str(x.GetValue())+", "+str(y.GetValue())+") facing "+str(theta.GetValue())+u"\u00B0.", 'Scout Created', wx.OK | wx.ICON_INFORMATION)
137
                                name.SetValue("")
138
                                x.SetValue(0)
139
                                y.SetValue(0)
140
                                theta.SetValue(0)
141
                                return True
142
                wx.MessageBox('Error creating scout.  Please check that all information was entered properly and there are no duplicate scout names.', 'Info', wx.OK | wx.ICON_ERROR)
143
        
144
        def AddScoutBox(self, name):
145
                self.sizer[name] = wx.FlexGridSizer(rows=2, cols=4, hgap=5, vgap=5)
146
                # Labels
147
                scoutName = wx.StaticText(self.window, label="Scout: "+name)
148
                behaviorLabel = wx.StaticText(self.window, label="Behavior: ")
149
                currBehaviorLabel = wx.StaticText(self.window, label="  |  Current Behavior: Slacking off")
150

    
151
                # Inputs
152
                scoutChoices = wx.Choice(self.window, choices=GetBehaviors())
153
                pauseButton = wx.ToggleButton(self.window, label="Pause")
154
                runButton = wx.Button(self.window, label="Run")
155
                killButton = wx.Button(self.window, label="Kill")
156

    
157
                self.sizer[name].Add(scoutName)
158
                self.sizer[name].Add(currBehaviorLabel, wx.EXPAND|wx.ALIGN_RIGHT)
159
                self.sizer[name].AddStretchSpacer(1)
160
                self.sizer[name].Add(killButton, wx.ALIGN_RIGHT)
161

    
162
                self.sizer[name].Add(behaviorLabel)
163
                self.sizer[name].Add(scoutChoices, wx.EXPAND)
164
                self.sizer[name].Add(runButton)
165
                self.sizer[name].Add(pauseButton, wx.ALIGN_RIGHT)
166
                
167
                # Events
168
                pauseButton.Bind(wx.EVT_TOGGLEBUTTON, lambda event: PauseScout(name, currBehaviorLabel, pauseButton))
169
                runButton.Bind(wx.EVT_BUTTON, lambda event: SetBehavior(name, currBehaviorLabel, pauseButton, scoutChoices.GetStringSelection()))
170
                killButton.Bind(wx.EVT_BUTTON, lambda event: self.RemoveScout(name))
171

    
172
                self.mainArea.Add(self.sizer[name], proportion=1, flag=wx.ALL|wx.EXPAND, border=10)
173
                self.window.Layout()
174
                return True
175

    
176
        def AddScout(self, name, x, y, theta):
177
                if AddToSimulator(name, x, y, theta):
178
                        return self.AddScoutBox(name)
179
                wx.MessageBox("ROS call failed!", 'ROS Error', wx.OK | wx.ICON_ERROR)
180
                return False
181

    
182
        def RemoveScout(self, name):
183
                # I don't trust this bit. Remove() should work by itself, but it doesn't want to.
184
                # Hide()ing before Remove()ing does work for some reason though. It makes me think 
185
                # the sizer is still around somewhere, sneakily leaking memory.
186
                # Alas, such is life.
187
                print "Hidden scoutLayout for "+name+": "+str(self.mainArea.Hide(self.sizer[name], True))
188
                print "Removed scoutLayout for "+name+": "+str(self.mainArea.Remove(self.sizer[name]))+" (Maybe...)"
189
                del self.sizer[name]
190
                if KillScout(name):
191
                        self.window.Layout()
192
                        self.window.Refresh()
193
                        self.window.Update()
194
                return
195

    
196
if __name__ == '__main__':
197
        subprocess.Popen("rosrun scoutsim scoutsim_node race", shell=True)
198
        app = wx.App()
199
        GUI(None, title='Colony Scout Manager')
200
        app.MainLoop()