Statistics
| Branch: | Revision:

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

History | View | Annotate | Download (7.08 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
        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 9a88eb2e Priya
                        subprocess.Popen("rosrun libscout libscout "+name+" 0", shell=True) 
49 5755691e Matt Bryant
                        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 9a88eb2e Priya
        processes[name] = subprocess.Popen("rosrun libscout libscout "+name+" "+str(GetBehaviors().index(behavior)+1)+"", shell=True) 
56 27f73b95 Matt Bryant
        behaviorButton.SetValue(False)
57
        behaviorButton.SetLabel("Pause")
58 5755691e Matt Bryant
        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 9a88eb2e Priya
                self.AddScoutBox("scout1")     
87 5755691e Matt Bryant
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 27f73b95 Matt Bryant
                startThetaTitle = wx.StaticText(self.window, label="Rad:")
100 5755691e Matt Bryant
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 27f73b95 Matt Bryant
                startTheta = wx.lib.intctrl.IntCtrl(self.window, min=0, max=100)
106 5755691e Matt Bryant
                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 27f73b95 Matt Bryant
                runButton.Bind(wx.EVT_BUTTON, lambda event: SetBehavior(name, currBehaviorLabel, pauseButton, scoutChoices.GetStringSelection()))
170 5755691e Matt Bryant
                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()