root / scout / scoutsim / GUI.py @ 01f09975
History | View | Annotate | Download (10.7 KB)
1 | 01f09975 | Yuyang Guo | #!/usr/bin/python
|
---|---|---|---|
2 | |||
3 | # Behavior GUI
|
||
4 | |||
5 | # shell subprocess libs
|
||
6 | import subprocess |
||
7 | import shlex #for converting string commands to tokens |
||
8 | |||
9 | # GUI libs
|
||
10 | import wx |
||
11 | import wx.lib.intctrl |
||
12 | import wx.lib.agw.floatspin |
||
13 | |||
14 | # not sure why these 2 are here
|
||
15 | import string |
||
16 | import signal |
||
17 | |||
18 | # ros imports for spawning and killing scout
|
||
19 | import rospy |
||
20 | import roslib |
||
21 | roslib.load_manifest("scoutsim")
|
||
22 | |||
23 | # scoutsim imports
|
||
24 | from scoutsim.srv import * |
||
25 | |||
26 | from collections import defaultdict |
||
27 | |||
28 | # each scout is represented by this class
|
||
29 | class Scout(object): |
||
30 | numOfScouts = 0 # class variable keeping track of scouts |
||
31 | |||
32 | def __init__(self, x=0, y=0, theta=0, name=u"", |
||
33 | Scouts=None, oninit = False): |
||
34 | print (len(name)) |
||
35 | Scout.numOfScouts += 1 # reverted if not successful |
||
36 | if len(name) == 0: |
||
37 | # automatically give name according to numOfScouts
|
||
38 | name = self.autoNameGen(Scouts)
|
||
39 | # set vars
|
||
40 | self.name = name
|
||
41 | self.behavior = "chilling" |
||
42 | self.process = None# used to keep track of all processes |
||
43 | # related to this scout
|
||
44 | self.valid = False |
||
45 | # these vars are right now just the original position
|
||
46 | # but when ODOMETRY is done, we could actually use them
|
||
47 | self.x, self.y = x, y |
||
48 | self.theta = theta
|
||
49 | |||
50 | # spawn the scout if it is not scout1 (automatically spawned)
|
||
51 | if (not oninit): |
||
52 | if (self.SpawnInSimulator()): |
||
53 | # spawned correctly
|
||
54 | self.valid = True |
||
55 | else:
|
||
56 | Scout.numOfScouts -= 1;
|
||
57 | |||
58 | |||
59 | def autoNameGen(self, Scouts): |
||
60 | i = 1
|
||
61 | while (True): |
||
62 | if not (("scout%d"%i) in Scouts): |
||
63 | return "scout%d"%i |
||
64 | i+=1
|
||
65 | |||
66 | |||
67 | # update the classvariable
|
||
68 | |||
69 | |||
70 | def SpawnInSimulator(self): |
||
71 | print "trying to spawn scout!!!" |
||
72 | try:
|
||
73 | # ros spawn routine
|
||
74 | rospy.wait_for_service('/spawn');
|
||
75 | service = rospy.ServiceProxy('/spawn', Spawn);
|
||
76 | response = service(self.x, self.y, self.theta, self.name) |
||
77 | return True |
||
78 | except rospy.ServiceException as Error: |
||
79 | print "things are bad!!!" |
||
80 | print Error
|
||
81 | return False |
||
82 | |||
83 | def changeBehavior(self, behavior): |
||
84 | self.behavior = behavior
|
||
85 | |||
86 | # WARNING: Check this but ROS kills the old behavior for us
|
||
87 | # kill the old behavior
|
||
88 | # if self.process != None:
|
||
89 | # self.process.terminate() #TODO: this kills the process
|
||
90 | # # but maybe not the rosnode
|
||
91 | # # change to "rosnode kill" instead
|
||
92 | # # make sure the old behavior is terminated before we run the new one
|
||
93 | # self.process.wait()
|
||
94 | |||
95 | # do rosprocess calls for new behavior
|
||
96 | roscommand = shlex.split("rosrun libscout libscout %s %s"%
|
||
97 | (self.name, self.behavior)) |
||
98 | subprocess.Popen(roscommand, shell=False)
|
||
99 | |||
100 | def killSelf(self): |
||
101 | # terminate its current behavior
|
||
102 | if self.process != None: |
||
103 | self.process.terminate() # TODO: same here, try "rosnode kill" |
||
104 | self.process.wait()
|
||
105 | # ros call to kill scout in simulator
|
||
106 | try:
|
||
107 | rospy.wait_for_service("/kill")
|
||
108 | service = rospy.ServiceProxy("/kill", Kill)
|
||
109 | response = service(self.name)
|
||
110 | Scout.numOfScouts -= 1
|
||
111 | self.valid = False |
||
112 | except rospy.ServiceException, e:
|
||
113 | print "warning: kill scout unsuccessful" # TODO: change to alert |
||
114 | self.valid = True |
||
115 | # remove from the class
|
||
116 | |||
117 | |||
118 | # a wrapper for wx Frame
|
||
119 | class Window(wx.App): |
||
120 | def __init__(self, title=""): |
||
121 | super(Window, self).__init__() |
||
122 | self.initUIFrame()
|
||
123 | |||
124 | def initUIFrame(self): |
||
125 | self.GUIFrame = GUI(None, "hallo!") |
||
126 | |||
127 | |||
128 | # actual GUI frame
|
||
129 | class GUI(wx.Frame): |
||
130 | def __init__(self, parent, title): |
||
131 | super(GUI, self).__init__(parent, title=title, |
||
132 | style=wx.DEFAULT_FRAME_STYLE ^ wx.RESIZE_BORDER, size=(600, 600)) |
||
133 | |||
134 | # open up scoutsim race
|
||
135 | command = shlex.split("rosrun scoutsim scoutsim_node race")
|
||
136 | self.simWindowProcess = subprocess.Popen(command, shell=False) |
||
137 | #rospy.wait_for_service('/spawn') #don't know why this is here
|
||
138 | |||
139 | # register call back for on close cleanup
|
||
140 | self.Bind(wx.EVT_CLOSE, self.onClose) |
||
141 | |||
142 | |||
143 | self.initData()
|
||
144 | self.initUI()
|
||
145 | self.Show(True) |
||
146 | |||
147 | # do clean up after user close the window
|
||
148 | def onClose(self, event): |
||
149 | print "Cleaning up all Processes" |
||
150 | |||
151 | # kill all the behavior processes
|
||
152 | for scout in self.scouts: |
||
153 | process = self.scouts[scout].process
|
||
154 | if (process != None): |
||
155 | process.terminate() |
||
156 | |||
157 | # kill the scout sim window
|
||
158 | self.simWindowProcess.terminate()
|
||
159 | print "done" |
||
160 | self.Destroy()
|
||
161 | |||
162 | def initData(self): |
||
163 | self.scouts = {}
|
||
164 | self.scouts["scout1"] = Scout(x=0, y=0, theta=0, name="scout1", |
||
165 | Scouts = self.scouts, oninit=True) |
||
166 | #TODO: these arguments are not right...fix them!!!
|
||
167 | |||
168 | # addButton callback
|
||
169 | def addScout(self, x_wx, y_wx, theta_wx, name_wx): |
||
170 | # x, y, theta, name are given as wx Inputs
|
||
171 | x = x_wx.GetValue() |
||
172 | y = y_wx.GetValue() |
||
173 | theta = theta_wx.GetValue() |
||
174 | name = name_wx.GetValue() |
||
175 | print "in GUI trying to add scout!" |
||
176 | newSc = Scout(x, y, theta, name, self.scouts)
|
||
177 | if (newSc.valid):
|
||
178 | # successful
|
||
179 | self.scouts[newSc.name] = newSc
|
||
180 | self.addScoutBox(newSc.name)
|
||
181 | # alert user spawn successful
|
||
182 | wx.MessageBox(u"Scout '%s' created successfully at (%d,%d,%d\u00B0)"
|
||
183 | %(newSc.name, newSc.x, newSc.y, newSc.theta), |
||
184 | "Scout Created", wx.OK | wx.ICON_INFORMATION)
|
||
185 | |||
186 | else:
|
||
187 | #failed to create scout
|
||
188 | wx.MessageBox("Scout Creation failed :( Check for duplicate name",
|
||
189 | "Add Scout failed", wx.OK | wx.ICON_ERROR)
|
||
190 | |||
191 | def removeScout(self, name): |
||
192 | sct = self.scouts[name]
|
||
193 | print "removing scout %s"%name |
||
194 | sct.killSelf() |
||
195 | if (sct.valid == False): |
||
196 | # successful, remove from the dictionary
|
||
197 | del self.scouts[name] |
||
198 | # refresh the display
|
||
199 | print self.mainArea.Hide(self.sizer[name]) |
||
200 | print self.mainArea.Remove(self.sizer[name]) |
||
201 | del self.sizer[name] |
||
202 | self.window.Layout()
|
||
203 | self.window.Refresh()
|
||
204 | self.window.Update()
|
||
205 | else:
|
||
206 | raise Exception |
||
207 | |||
208 | ############################ UI stuff here ###########################
|
||
209 | |||
210 | def initUI(self): |
||
211 | self.initAddScoutArea()
|
||
212 | |||
213 | # the labels and input boxes for adding a scout through GUI
|
||
214 | def initAddScoutArea(self): |
||
215 | # all the layout stuff copied over: using grid layout
|
||
216 | # button callbacks are changed
|
||
217 | self.totalCols = 8 |
||
218 | self.window = wx.ScrolledWindow(self, style=wx.VSCROLL) |
||
219 | self.mainArea = wx.GridSizer(cols=1) |
||
220 | sizer = wx.FlexGridSizer(rows=4, cols=self.totalCols, hgap=5, vgap=5) |
||
221 | |||
222 | # Labels
|
||
223 | blankText = wx.StaticText(self.window, label="") |
||
224 | newScout = wx.StaticText(self.window, label="New Scout") |
||
225 | newScoutName = wx.StaticText(self.window, label="Name:") |
||
226 | startXTitle = wx.StaticText(self.window, label="X:") |
||
227 | startYTitle = wx.StaticText(self.window, label="Y:") |
||
228 | startThetaTitle = wx.StaticText(self.window, label="Rad:") |
||
229 | |||
230 | # Inputs
|
||
231 | newScoutInput = wx.TextCtrl(self.window)
|
||
232 | startX = wx.lib.agw.floatspin.FloatSpin(self.window, min_val=0, digits=5) |
||
233 | startY = wx.lib.agw.floatspin.FloatSpin(self.window, min_val=0, digits=5) |
||
234 | startTheta = wx.lib.agw.floatspin.FloatSpin(self.window, min_val=0, digits=5) |
||
235 | addButton = wx.Button(self.window, id=wx.ID_ADD)
|
||
236 | |||
237 | # Pretty Stuff
|
||
238 | hLine = wx.StaticLine(self.window, size=(600, 5)) |
||
239 | bottomHLine = wx.StaticLine(self.window, size=(600, 5)) |
||
240 | |||
241 | # Row 0: just the label add scout
|
||
242 | sizer.Add(newScout) |
||
243 | for i in range(7): |
||
244 | sizer.AddStretchSpacer(1)
|
||
245 | # Row 1: input(name), x coord, y coord, rad,
|
||
246 | sizer.AddMany([newScoutName, (newScoutInput, 0, wx.EXPAND), startXTitle,
|
||
247 | startX, startYTitle, startY, startThetaTitle, startTheta]) |
||
248 | # Row 2: just the add button to the right
|
||
249 | for i in range(7): |
||
250 | sizer.AddStretchSpacer(1)
|
||
251 | sizer.Add(addButton) |
||
252 | # Row 3
|
||
253 | |||
254 | # Events
|
||
255 | addButton.Bind(wx.EVT_BUTTON, lambda event: self.addScout( |
||
256 | startX, startY, startTheta, newScoutInput)) |
||
257 | |||
258 | sizer.AddGrowableCol(idx=1)
|
||
259 | self.mainArea.Add(sizer, proportion=1, flag=wx.ALL|wx.EXPAND, border=10) |
||
260 | self.window.SetSizer(self.mainArea) |
||
261 | self.sizer = defaultdict()
|
||
262 | self.window.Layout()
|
||
263 | |||
264 | # make the scout1's controller
|
||
265 | self.addScoutBox("scout1"); |
||
266 | |||
267 | # copied over from old GUI
|
||
268 | def addScoutBox(self, name): |
||
269 | self.sizer[name] = wx.FlexGridSizer(rows=2, cols=5, hgap=5, vgap=5) |
||
270 | # Labels
|
||
271 | scoutName = wx.StaticText(self.window, label="Scout: " + name) |
||
272 | behaviorLabel = wx.StaticText(self.window, label="Behavior: ") |
||
273 | currBehaviorLabel = wx.StaticText(self.window,
|
||
274 | label=" | Current Behavior: Slacking off")
|
||
275 | |||
276 | # Inputs
|
||
277 | # drop down menue
|
||
278 | scoutChoices = wx.Choice(self.window) #choices=GetBehaviors()) |
||
279 | # buttons
|
||
280 | pauseButton = wx.ToggleButton(self.window, label="Pause") |
||
281 | runButton = wx.Button(self.window, label="Run") |
||
282 | killButton = wx.Button(self.window, label="Kill") |
||
283 | teleopButton = wx.ToggleButton(self.window, label="Teleop") |
||
284 | |||
285 | # row 0
|
||
286 | self.sizer[name].Add(scoutName)
|
||
287 | self.sizer[name].Add(currBehaviorLabel, wx.EXPAND | wx.ALIGN_RIGHT)
|
||
288 | self.sizer[name].AddStretchSpacer(1) |
||
289 | self.sizer[name].AddStretchSpacer(1) |
||
290 | self.sizer[name].Add(killButton, wx.ALIGN_RIGHT)
|
||
291 | |||
292 | # row 1
|
||
293 | self.sizer[name].Add(behaviorLabel)
|
||
294 | self.sizer[name].Add(scoutChoices, wx.EXPAND)
|
||
295 | self.sizer[name].Add(runButton)
|
||
296 | self.sizer[name].Add(pauseButton, wx.ALIGN_RIGHT)
|
||
297 | self.sizer[name].Add(teleopButton)
|
||
298 | # Events
|
||
299 | killButton.Bind(wx.EVT_BUTTON, lambda event: self.removeScout(name)) |
||
300 | |||
301 | self.mainArea.Add(self.sizer[name], proportion=1, |
||
302 | flag=wx.ALL | wx.EXPAND, border=10)
|
||
303 | self.window.Layout()
|
||
304 | return True |
||
305 | |||
306 | |||
307 | |||
308 | if __name__ == '__main__': |
||
309 | # open up GUI
|
||
310 | window = Window(title="Colony Scout Manager")
|
||
311 | window.MainLoop() |