Statistics
| Branch: | Revision:

root / scout / scoutsim / BehaviorGUI.py @ 5755691e

History | View | Annotate | Download (6.89 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
# Everyone likes some nice globals
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
                        print "Scout "+name+" was paused."
49
                        behaviorButton.SetLabel("Resume")
50
                        behaviorLabel.SetLabel("  |  Current Behavior: Paused")
51
                return True
52
        if name in processes:
53
                processes[name].kill()
54
        processes[name] = subprocess.Popen("rosrun libscout libscout "+name+" "+str(GetBehaviors().index(behavior))+"", shell=True) 
55
        print "Scout "+name+" was set to behavior: "+behavior
56
        behaviorLabel.SetLabel("  |  Current Behavior: "+behavior)
57
        scouts[name] = behavior
58
59
def KillScout(name):
60
        try:
61
                rospy.wait_for_service('/kill');
62
                service = rospy.ServiceProxy('/kill', Kill);
63
                if name in processes:
64
                        processes[name].kill() # This is perhaps not working
65
                resp = service(name)
66
                del scouts[name]
67
                del processes[name]
68
                return True
69
        except rospy.ServiceException, e:
70
                return False
71
72
def PauseScout(name, behaviorLabel, behaviorButton):
73
        return SetBehavior(name, behaviorLabel, behaviorButton)
74
#
75
# End ROS functions
76
#
77
78
79
class GUI(wx.Frame):
80
        def __init__(self, parent, title):
81
                super(GUI, self).__init__(parent, title=title, style=wx.DEFAULT_FRAME_STYLE ^ wx.RESIZE_BORDER, size=(600, 600))
82
                self.InitUI()
83
                self.Show()     
84
85
        def InitUI(self):
86
                self.window = wx.ScrolledWindow(self, style=wx.VSCROLL)
87
                self.mainArea = wx.GridSizer(cols=1)
88
                sizer = wx.FlexGridSizer(rows=4, cols=8, hgap=5, vgap=5)
89
90
                # Labels
91
                blankText = wx.StaticText(self.window, label="")
92
                newScout = wx.StaticText(self.window, label="New Scout")
93
                newScoutName = wx.StaticText(self.window, label="Name:")
94
                startXTitle = wx.StaticText(self.window, label="X:")
95
                startYTitle = wx.StaticText(self.window, label="Y:")
96
                startThetaTitle = wx.StaticText(self.window, label=u'\u0398')
97
98
                # Inputs
99
                newScoutInput = wx.TextCtrl(self.window)
100
                startX = wx.lib.intctrl.IntCtrl(self.window, min=0)
101
                startY = wx.lib.intctrl.IntCtrl(self.window, min=0)
102
                startTheta = wx.lib.intctrl.IntCtrl(self.window, min=0, max=360)
103
                addButton = wx.Button(self.window, id=wx.ID_ADD)
104
105
                # Pretty Stuff
106
                hLine = wx.StaticLine(self.window, size=(600, 5))
107
                bottomHLine = wx.StaticLine(self.window, size=(600, 5))
108
109
                # Row 0
110
                sizer.Add(newScout)
111
                for i in range(7):
112
                        sizer.AddStretchSpacer(1)
113
                # Row 1
114
                sizer.AddMany([newScoutName, (newScoutInput, 0, wx.EXPAND), startXTitle, startX, startYTitle, startY, startThetaTitle, startTheta])
115
                # Row 2
116
                for i in range(7):
117
                        sizer.AddStretchSpacer(1)
118
                sizer.Add(addButton)
119
                # Row 3
120
121
                # Events
122
                addButton.Bind(wx.EVT_BUTTON, lambda event: self.NewScout(event, newScoutInput, startX, startY, startTheta))
123
124
                sizer.AddGrowableCol(idx=1)
125
                self.mainArea.Add(sizer, proportion=1, flag=wx.ALL|wx.EXPAND, border=10)
126
                self.window.SetSizer(self.mainArea)
127
                self.sizer = defaultdict()
128
129
        def NewScout(self, e, name, x, y, theta):
130
                if (name):
131
                        scoutCreated = self.AddScout(name.GetValue(), x.GetValue(), y.GetValue(), theta.GetValue())
132
                        if (scoutCreated):
133
                                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)
134
                                name.SetValue("")
135
                                x.SetValue(0)
136
                                y.SetValue(0)
137
                                theta.SetValue(0)
138
                                return True
139
                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)
140
        
141
        def AddScoutBox(self, name):
142
                self.sizer[name] = wx.FlexGridSizer(rows=2, cols=4, hgap=5, vgap=5)
143
                # Labels
144
                scoutName = wx.StaticText(self.window, label="Scout: "+name)
145
                behaviorLabel = wx.StaticText(self.window, label="Behavior: ")
146
                currBehaviorLabel = wx.StaticText(self.window, label="  |  Current Behavior: Slacking off")
147
148
                # Inputs
149
                scoutChoices = wx.Choice(self.window, choices=GetBehaviors())
150
                pauseButton = wx.ToggleButton(self.window, label="Pause")
151
                runButton = wx.Button(self.window, label="Run")
152
                killButton = wx.Button(self.window, label="Kill")
153
154
                self.sizer[name].Add(scoutName)
155
                self.sizer[name].Add(currBehaviorLabel, wx.EXPAND|wx.ALIGN_RIGHT)
156
                self.sizer[name].AddStretchSpacer(1)
157
                self.sizer[name].Add(killButton, wx.ALIGN_RIGHT)
158
159
                self.sizer[name].Add(behaviorLabel)
160
                self.sizer[name].Add(scoutChoices, wx.EXPAND)
161
                self.sizer[name].Add(runButton)
162
                self.sizer[name].Add(pauseButton, wx.ALIGN_RIGHT)
163
                
164
                # Events
165
                pauseButton.Bind(wx.EVT_TOGGLEBUTTON, lambda event: PauseScout(name, currBehaviorLabel, pauseButton))
166
                runButton.Bind(wx.EVT_BUTTON, lambda event: SetBehavior(name, scoutChoices.GetStringSelection(), currBehaviorLabel))
167
                killButton.Bind(wx.EVT_BUTTON, lambda event: self.RemoveScout(name))
168
169
                self.mainArea.Add(self.sizer[name], proportion=1, flag=wx.ALL|wx.EXPAND, border=10)
170
                self.window.Layout()
171
                return True
172
173
        def AddScout(self, name, x, y, theta):
174
                if AddToSimulator(name, x, y, theta):
175
                        return self.AddScoutBox(name)
176
                wx.MessageBox("ROS call failed!", 'ROS Error', wx.OK | wx.ICON_ERROR)
177
                return False
178
179
        def RemoveScout(self, name):
180
                # I don't trust this bit. Remove() should work by itself, but it doesn't want to.
181
                # Hide()ing before Remove()ing does work for some reason though. It makes me think 
182
                # the sizer is still around somewhere, sneakily leaking memory.
183
                # Alas, such is life.
184
                print "Hidden scoutLayout for "+name+": "+str(self.mainArea.Hide(self.sizer[name], True))
185
                print "Removed scoutLayout for "+name+": "+str(self.mainArea.Remove(self.sizer[name]))+" (Maybe...)"
186
                del self.sizer[name]
187
                if KillScout(name):
188
                        self.window.Layout()
189
                        self.window.Refresh()
190
                        self.window.Update()
191
                return
192
193
if __name__ == '__main__':
194
        subprocess.Popen("rosrun scoutsim scoutsim_node race", shell=True)
195
        app = wx.App()
196
        GUI(None, title='Colony Scout Manager')
197
        app.MainLoop()