Revision 5755691e

View differences:

scout/libscout/src/behaviors/draw_ccw_circle.cpp
29 29
{
30 30
    while(ok())
31 31
    {
32
        motors->set_sides(80, 20, MOTOR_ABSOLUTE);
32
        motors->set_sides(20, 80, MOTOR_ABSOLUTE);
33 33

  
34 34
        spinOnce();
35 35
        loop_rate->sleep();
scout/libscout/src/behaviors/draw_cw_circle.cpp
29 29
{
30 30
    while(ok())
31 31
    {
32
        motors->set_sides(20, 80, MOTOR_ABSOLUTE);
32
        motors->set_sides(80, 20, MOTOR_ABSOLUTE);
33 33

  
34 34
        encoder_readings readings = encoders->query();
35 35

  
scout/scoutsim/BehaviorGUI.py
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 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()

Also available in: Unified diff