Revision 6e8c4ad2 scout/scoutsim/BehaviorGUI.py

View differences:

scout/scoutsim/BehaviorGUI.py
19 19
# ROS functions
20 20
#
21 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
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 33

  
34 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", selectedBehavior="Pause"):
38
	if behavior == "Pause":
39
		if name not in processes:
40
			return False
41
		if not behaviorButton.GetValue():
42
			processes[name] = subprocess.Popen("rosrun libscout libscout "+name+" "+str(GetBehaviors().index(selectedBehavior)+1)+"", shell=True)
43
			print "Scout "+name+" was resumed."
44
			behaviorButton.SetLabel("Pause")
45
			behaviorLabel.SetLabel("  |  Current Behavior: "+scouts[name])
46
		else:
47
			processes[name].kill()
48
			processes[name].wait()
49
			subprocess.Popen("rosrun libscout libscout "+name+" 0", shell=True) 
50
			print "Scout "+name+" was paused."
51
			behaviorButton.SetLabel("Resume")
52
			behaviorLabel.SetLabel("  |  Current Behavior: Paused")
53
		return True
54
	if name in processes:
55
		processes[name].kill()
56
		processes[name].wait()
57
		# Running a new behavior while paused does not do anything and
58
		# Gives lots of behaviors.
59
	processes[name] = subprocess.Popen("rosrun libscout libscout "+name+" "+str(GetBehaviors().index(behavior)+1)+"", shell=True) 
60
	behaviorButton.SetValue(False)
61
	behaviorButton.SetLabel("Pause")
62
	print "Scout "+name+" was set to behavior: "+behavior
63
	behaviorLabel.SetLabel("  |  Current Behavior: "+behavior)
64
	scouts[name] = behavior
35
    return ["CW Circle", "CCW Circle", "Odometry", "Navigation Map",
36
        "Scheduler", "Warehouse", "Line Follow", "WL Test"]
37

  
38
def SetBehavior(name, behaviorLabel, behaviorButton=False, behavior="Pause",
39
        selectedBehavior="Pause"):
40
    if behavior == "Pause":
41
        if name not in processes:
42
            return False
43
        if not behaviorButton.GetValue():
44
            processes[name] = subprocess.Popen("rosrun libscout libscout "
45
                + name + " "
46
                + 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
        else:
52
            processes[name].kill()
53
            processes[name].wait()
54
            subprocess.Popen("rosrun libscout libscout " + name + " 0",
55
                shell=True) 
56
            print "Scout "+name+" was paused."
57
            behaviorButton.SetLabel("Resume")
58
            behaviorLabel.SetLabel("  |  Current Behavior: Paused")
59
        return True
60
    if name in processes:
61
        processes[name].kill()
62
        processes[name].wait()
63
        # Running a new behavior while paused does not do anything and
64
        # Gives lots of behaviors.
65
    processes[name] = subprocess.Popen("rosrun libscout libscout " + name + " "
66
        + str(GetBehaviors().index(behavior) + 1) + "",
67
        shell=True) 
68
    behaviorButton.SetValue(False)
69
    behaviorButton.SetLabel("Pause")
70
    print "Scout " + name + " was set to behavior: " + behavior
71
    behaviorLabel.SetLabel("  |  Current Behavior: " + behavior)
72
    scouts[name] = behavior
65 73

  
66 74
def KillScout(name):
67
	try:
68
		rospy.wait_for_service('/kill');
69
		service = rospy.ServiceProxy('/kill', Kill);
70
		if name in processes:
71
			# BUG: Pause-ing a scout and then killing it crashes the program.
72
			processes[name].kill() # This is perhaps not working
73
		resp = service(name)
74
		del scouts[name]
75
		del processes[name]
76
		return True
77
	except rospy.ServiceException, e:
78
		return False
75
    try:
76
        rospy.wait_for_service('/kill');
77
        service = rospy.ServiceProxy('/kill', Kill);
78
        if name in processes:
79
            # TODO BUG: Pausing a scout and then killing it crashes the program.
80
            processes[name].kill() # This is perhaps not working
81
        resp = service(name)
82
        del scouts[name]
83
        del processes[name]
84
        return True
85
    except rospy.ServiceException, e:
86
        return False
79 87

  
80 88
def PauseScout(name, behaviorLabel, behaviorButton, selectedBehavior):
81
	return SetBehavior(name, behaviorLabel, behaviorButton, "Pause", selectedBehavior)
89
    return SetBehavior(name, behaviorLabel, behaviorButton,
90
        "Pause", selectedBehavior)
82 91
#
83 92
# End ROS functions
84 93
#
85 94

  
86 95
class GUI(wx.Frame):
87
	def __init__(self, parent, title):
88
		super(GUI, self).__init__(parent, title=title, style=wx.DEFAULT_FRAME_STYLE ^ wx.RESIZE_BORDER, size=(600, 600))
89
		self.InitUI()
90
		self.Show()     
91
		self.AddScoutBox("scout1")     
92

  
93
	def InitUI(self):
94
		self.window = wx.ScrolledWindow(self, style=wx.VSCROLL)
95
		self.mainArea = wx.GridSizer(cols=1)
96
		sizer = wx.FlexGridSizer(rows=4, cols=8, hgap=5, vgap=5)
97

  
98
		# Labels
99
		blankText = wx.StaticText(self.window, label="")
100
		newScout = wx.StaticText(self.window, label="New Scout")
101
		newScoutName = wx.StaticText(self.window, label="Name:")
102
		startXTitle = wx.StaticText(self.window, label="X:")
103
		startYTitle = wx.StaticText(self.window, label="Y:")
104
		startThetaTitle = wx.StaticText(self.window, label="Rad:")
105

  
106
		# Inputs
107
		newScoutInput = wx.TextCtrl(self.window)
108
		startX = wx.lib.intctrl.IntCtrl(self.window, min=0)
109
		startY = wx.lib.intctrl.IntCtrl(self.window, min=0)
110
		startTheta = wx.lib.intctrl.IntCtrl(self.window, min=0, max=100)
111
		addButton = wx.Button(self.window, id=wx.ID_ADD)
112

  
113
		# Pretty Stuff
114
		hLine = wx.StaticLine(self.window, size=(600, 5))
115
		bottomHLine = wx.StaticLine(self.window, size=(600, 5))
116

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

  
129
		# Events
130
		addButton.Bind(wx.EVT_BUTTON, lambda event: self.NewScout(event, newScoutInput, startX, startY, startTheta))
131

  
132
		sizer.AddGrowableCol(idx=1)
133
		self.mainArea.Add(sizer, proportion=1, flag=wx.ALL|wx.EXPAND, border=10)
134
		self.window.SetSizer(self.mainArea)
135
		self.sizer = defaultdict()
136

  
137
	def NewScout(self, e, name, x, y, theta):
138
		if (name):
139
			scoutCreated = self.AddScout(name.GetValue(), x.GetValue(), y.GetValue(), theta.GetValue())
140
			if (scoutCreated):
141
				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)
142
				name.SetValue("")
143
				x.SetValue(0)
144
				y.SetValue(0)
145
				theta.SetValue(0)
146
				return True
147
		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)
148
	
149
	def AddScoutBox(self, name):
150
		self.sizer[name] = wx.FlexGridSizer(rows=2, cols=4, hgap=5, vgap=5)
151
		# Labels
152
		scoutName = wx.StaticText(self.window, label="Scout: "+name)
153
		behaviorLabel = wx.StaticText(self.window, label="Behavior: ")
154
		currBehaviorLabel = wx.StaticText(self.window, label="  |  Current Behavior: Slacking off")
155

  
156
		# Inputs
157
		scoutChoices = wx.Choice(self.window, choices=GetBehaviors())
158
		pauseButton = wx.ToggleButton(self.window, label="Pause")
159
		runButton = wx.Button(self.window, label="Run")
160
		killButton = wx.Button(self.window, label="Kill")
161

  
162
		self.sizer[name].Add(scoutName)
163
		self.sizer[name].Add(currBehaviorLabel, wx.EXPAND|wx.ALIGN_RIGHT)
164
		self.sizer[name].AddStretchSpacer(1)
165
		self.sizer[name].Add(killButton, wx.ALIGN_RIGHT)
166

  
167
		self.sizer[name].Add(behaviorLabel)
168
		self.sizer[name].Add(scoutChoices, wx.EXPAND)
169
		self.sizer[name].Add(runButton)
170
		self.sizer[name].Add(pauseButton, wx.ALIGN_RIGHT)
171
		
172
		# Events
173
		pauseButton.Bind(wx.EVT_TOGGLEBUTTON, lambda event: PauseScout(name, currBehaviorLabel, pauseButton, scoutChoices.GetStringSelection()))
174
		runButton.Bind(wx.EVT_BUTTON, lambda event: SetBehavior(name, currBehaviorLabel, pauseButton, scoutChoices.GetStringSelection()))
175
		killButton.Bind(wx.EVT_BUTTON, lambda event: self.RemoveScout(name))
176

  
177
		self.mainArea.Add(self.sizer[name], proportion=1, flag=wx.ALL|wx.EXPAND, border=10)
178
		self.window.Layout()
179
		return True
180

  
181
	def AddScout(self, name, x, y, theta):
182
		if AddToSimulator(name, x, y, theta):
183
			return self.AddScoutBox(name)
184
		wx.MessageBox("ROS call failed!", 'ROS Error', wx.OK | wx.ICON_ERROR)
185
		return False
186

  
187
	def RemoveScout(self, name):
188
		# I don't trust this bit. Remove() should work by itself, but it doesn't want to.
189
		# Hide()ing before Remove()ing does work for some reason though. It makes me think 
190
		# the sizer is still around somewhere, sneakily leaking memory.
191
		# Alas, such is life.
192
		print "Hidden scoutLayout for "+name+": "+str(self.mainArea.Hide(self.sizer[name], True))
193
		print "Removed scoutLayout for "+name+": "+str(self.mainArea.Remove(self.sizer[name]))+" (Maybe...)"
194
		del self.sizer[name]
195
		if KillScout(name):
196
			self.window.Layout()
197
			self.window.Refresh()
198
			self.window.Update()
199
		return
96
    def __init__(self, parent, title):
97
        super(GUI, self).__init__(parent, title=title,
98
            style=wx.DEFAULT_FRAME_STYLE ^ wx.RESIZE_BORDER, size=(600, 600))
99
        self.InitUI()
100
        self.Show()     
101
        self.AddScoutBox("scout1")     
102

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

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

  
116
        # Inputs
117
        newScoutInput = wx.TextCtrl(self.window)
118
        startX = wx.lib.intctrl.IntCtrl(self.window, min=0)
119
        startY = wx.lib.intctrl.IntCtrl(self.window, min=0)
120
        startTheta = wx.lib.intctrl.IntCtrl(self.window, min=0, max=100)
121
        addButton = wx.Button(self.window, id=wx.ID_ADD)
122

  
123
        # Pretty Stuff
124
        hLine = wx.StaticLine(self.window, size=(600, 5))
125
        bottomHLine = wx.StaticLine(self.window, size=(600, 5))
126

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

  
140
        # Events
141
        addButton.Bind(wx.EVT_BUTTON, lambda event: self.NewScout(event,
142
            newScoutInput, startX, startY, startTheta))
143

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

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

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

  
181
        self.sizer[name].Add(scoutName)
182
        self.sizer[name].Add(currBehaviorLabel, wx.EXPAND | wx.ALIGN_RIGHT)
183
        self.sizer[name].AddStretchSpacer(1)
184
        self.sizer[name].Add(killButton, wx.ALIGN_RIGHT)
185

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

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

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

  
211
    def RemoveScout(self, name):
212
        # I don't trust this bit. Remove() should work by itself, 
213
        # but it doesn't want to. Hide()ing before Remove()ing does work 
214
        # for some reason though. It makes me think  the sizer is still around
215
        # somewhere, sneakily leaking memory. Alas, such is life.
216
        print("Hidden scoutLayout for " + name + ": "
217
            + str(self.mainArea.Hide(self.sizer[name], True)))
218
        print("Removed scoutLayout for " + name + ": "
219
            + str(self.mainArea.Remove(self.sizer[name])) + " (Maybe...)")
220
        del self.sizer[name]
221
        if KillScout(name):
222
            self.window.Layout()
223
            self.window.Refresh()
224
            self.window.Update()
225
        return
200 226

  
201 227
if __name__ == '__main__':
202
	subprocess.Popen("rosrun scoutsim scoutsim_node race", shell=True)
203
	app = wx.App()
204
	GUI(None, title='Colony Scout Manager')
205
	app.MainLoop()
228
    subprocess.Popen("rosrun scoutsim scoutsim_node race", shell=True)
229
    rospy.wait_for_service('/spawn')
230
    subprocess.Popen("rosservice call /spawn 1.4 0.275 0 scout1", shell=True)
231
    app = wx.App()
232
    GUI(None, title='Colony Scout Manager')
233
    app.MainLoop()

Also available in: Unified diff