Statistics
| Branch: | Revision:

root / scout / scoutsim / BehaviorGUI.py @ d2dedb17

History | View | Annotate | Download (8.87 KB)

1 5755691e Matt Bryant
#!/usr/bin/python
2
3
import wx
4
import wx.lib.intctrl 
5 8fa2bd2e Matt Bryant
import wx.lib.agw.floatspin
6 5755691e Matt Bryant
import string
7
import subprocess
8
import signal
9
from collections import defaultdict
10 d2dedb17 Alex
11 5755691e Matt Bryant
# ROS imports
12
import roslib; roslib.load_manifest("scoutsim")
13
import rospy
14
from scoutsim.srv import *
15
16 27f73b95 Matt Bryant
# Everyone global variables (PLEASE REFACTOR ME)
17 8fa2bd2e Matt Bryant
scouts = defaultdict(str) # Dict in the form [scout_name: current_behavior]
18
processes = defaultdict() # Dict in the form [scout_name: process_handler]
19 5755691e Matt Bryant
20
#
21
# ROS functions
22
#
23
def AddToSimulator(name, x, y, theta):
24 6e8c4ad2 Alex
    if name in scouts:
25
        return False
26
    scouts[name] = ""
27
    print "Calling ROS function to create scout "+name
28
    try:
29
        rospy.wait_for_service('/spawn');
30
        service = rospy.ServiceProxy('/spawn', Spawn);
31
        resp = service(x, y, theta, name)
32
        return True
33
    except rospy.ServiceException, e:
34
        return False
35 5755691e Matt Bryant
36
def GetBehaviors():
37 6e8c4ad2 Alex
    return ["CW Circle", "CCW Circle", "Odometry", "Navigation Map",
38
        "Scheduler", "Warehouse", "Line Follow", "WL Test"]
39
40 da8ace01 Matt Bryant
def SetBehavior(name, behaviorLabel, behaviorButton=False, behavior="Pause", selectedBehavior="Pause"):
41 d2dedb17 Alex
    if behavior == "Pause":
42
        if name not in processes:
43
            return False
44
        if not behaviorButton.GetValue():
45
            processes[name] = subprocess.Popen("rosrun libscout libscout " 
46
                    + name + " " + 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
            return True
52 6e8c4ad2 Alex
        else:
53 d2dedb17 Alex
            if name in processes:
54
                processes[name].terminate()
55
                del processes[name]
56
            subprocess.Popen("rosrun libscout libscout " + name + " 0", shell=True) 
57
            print "Scout "+name+" was paused."
58
            behaviorButton.SetLabel("Resume")
59
            behaviorLabel.SetLabel("  |  Current Behavior: Paused")
60
            return True
61
    if name in processes:
62
        processes[name].terminate()
63 73adca60 Alex
        # TODO Running a new behavior while paused does not do anything and
64
        # Gives lots of errors.
65 d2dedb17 Alex
    #
66
    # -- Matt 11/30 --
67
    # Might be fixed, can't run it to tell.
68
    processes[name] = subprocess.Popen("rosrun libscout libscout " + name + " "
69
            + str(GetBehaviors().index(behavior) + 1) + "",
70
        shell=True) 
71
    behaviorButton.SetValue(False)
72
    behaviorButton.SetLabel("Pause")
73
    print "Scout " + name + " was set to behavior: " + behavior
74
    behaviorLabel.SetLabel("  |  Current Behavior: " + behavior)
75
    scouts[name] = behavior
76 5755691e Matt Bryant
77
def KillScout(name):
78 d2dedb17 Alex
    try:
79
        rospy.wait_for_service('/kill')
80
        service = rospy.ServiceProxy('/kill', Kill)
81
        if name in processes:
82
            processes[name].kill()
83
            del processes[name]
84
        resp = service(name)
85
        if name in scouts:
86
            del scouts[name]
87
        return True
88
    except rospy.ServiceException, e:
89
        return False
90 5755691e Matt Bryant
91 16d4e150 Priya
def PauseScout(name, behaviorLabel, behaviorButton, selectedBehavior):
92 6e8c4ad2 Alex
    return SetBehavior(name, behaviorLabel, behaviorButton,
93
        "Pause", selectedBehavior)
94 5755691e Matt Bryant
#
95
# End ROS functions
96
#
97
98
class GUI(wx.Frame):
99 6e8c4ad2 Alex
    def __init__(self, parent, title):
100
        super(GUI, self).__init__(parent, title=title,
101
            style=wx.DEFAULT_FRAME_STYLE ^ wx.RESIZE_BORDER, size=(600, 600))
102
        self.InitUI()
103
        self.Show()     
104
        self.AddScoutBox("scout1")     
105
106
    def InitUI(self):
107
        self.window = wx.ScrolledWindow(self, style=wx.VSCROLL)
108
        self.mainArea = wx.GridSizer(cols=1)
109
        sizer = wx.FlexGridSizer(rows=4, cols=8, hgap=5, vgap=5)
110
111
        # Labels
112
        blankText = wx.StaticText(self.window, label="")
113
        newScout = wx.StaticText(self.window, label="New Scout")
114
        newScoutName = wx.StaticText(self.window, label="Name:")
115
        startXTitle = wx.StaticText(self.window, label="X:")
116
        startYTitle = wx.StaticText(self.window, label="Y:")
117
        startThetaTitle = wx.StaticText(self.window, label="Rad:")
118
119
        # Inputs
120
        newScoutInput = wx.TextCtrl(self.window)
121 8fa2bd2e Matt Bryant
        startX = wx.lib.agw.floatspin.FloatSpin(self.window, min_val=0, digits=5)
122
        startY = wx.lib.agw.floatspin.FloatSpin(self.window, min_val=0, digits=5)
123
        startTheta = wx.lib.agw.floatspin.FloatSpin(self.window, min_val=0, digits=5)
124 6e8c4ad2 Alex
        addButton = wx.Button(self.window, id=wx.ID_ADD)
125
126
        # Pretty Stuff
127
        hLine = wx.StaticLine(self.window, size=(600, 5))
128
        bottomHLine = wx.StaticLine(self.window, size=(600, 5))
129
130
        # Row 0
131
        sizer.Add(newScout)
132
        for i in range(7):
133
            sizer.AddStretchSpacer(1)
134
        # Row 1
135
        sizer.AddMany([newScoutName, (newScoutInput, 0, wx.EXPAND), startXTitle,
136
            startX, startYTitle, startY, startThetaTitle, startTheta])
137
        # Row 2
138
        for i in range(7):
139
            sizer.AddStretchSpacer(1)
140
        sizer.Add(addButton)
141
        # Row 3
142
143
        # Events
144
        addButton.Bind(wx.EVT_BUTTON, lambda event: self.NewScout(event,
145
            newScoutInput, startX, startY, startTheta))
146
147
        sizer.AddGrowableCol(idx=1)
148
        self.mainArea.Add(sizer, proportion=1, flag=wx.ALL|wx.EXPAND, border=10)
149
        self.window.SetSizer(self.mainArea)
150
        self.sizer = defaultdict()
151
152
    def NewScout(self, e, name, x, y, theta):
153
        if (name):
154
            scoutCreated = self.AddScout(name.GetValue(), x.GetValue(),
155
                y.GetValue(), theta.GetValue())
156
            if (scoutCreated):
157
                wx.MessageBox("Scout '" + name.GetValue()
158
                    + "' created successfully at (" + str(x.GetValue()) + ", "
159
                    + str(y.GetValue()) + ") facing " + str(theta.GetValue())
160
                    + u"\u00B0.", 'Scout Created', wx.OK | wx.ICON_INFORMATION)
161
                name.SetValue("")
162
                x.SetValue(0)
163
                y.SetValue(0)
164
                theta.SetValue(0)
165
                return True
166
        wx.MessageBox('Error creating scout.  Please check that all information'
167
            'was entered properly and there are no duplicate scout names.',
168
            'Info', wx.OK | wx.ICON_ERROR)
169
    
170
    def AddScoutBox(self, name):
171
        self.sizer[name] = wx.FlexGridSizer(rows=2, cols=4, hgap=5, vgap=5)
172
        # Labels
173
        scoutName = wx.StaticText(self.window, label="Scout: " + name)
174
        behaviorLabel = wx.StaticText(self.window, label="Behavior: ")
175
        currBehaviorLabel = wx.StaticText(self.window,
176
            label="  |  Current Behavior: Slacking off")
177
178
        # Inputs
179
        scoutChoices = wx.Choice(self.window, choices=GetBehaviors())
180
        pauseButton = wx.ToggleButton(self.window, label="Pause")
181
        runButton = wx.Button(self.window, label="Run")
182
        killButton = wx.Button(self.window, label="Kill")
183
184
        self.sizer[name].Add(scoutName)
185
        self.sizer[name].Add(currBehaviorLabel, wx.EXPAND | wx.ALIGN_RIGHT)
186
        self.sizer[name].AddStretchSpacer(1)
187
        self.sizer[name].Add(killButton, wx.ALIGN_RIGHT)
188
189
        self.sizer[name].Add(behaviorLabel)
190
        self.sizer[name].Add(scoutChoices, wx.EXPAND)
191
        self.sizer[name].Add(runButton)
192
        self.sizer[name].Add(pauseButton, wx.ALIGN_RIGHT)
193
        
194
        # Events
195
        pauseButton.Bind(wx.EVT_TOGGLEBUTTON,
196
            lambda event: PauseScout(name, currBehaviorLabel, pauseButton,
197
                scoutChoices.GetStringSelection()))
198
        runButton.Bind(wx.EVT_BUTTON,
199
            lambda event: SetBehavior(name, currBehaviorLabel, pauseButton,
200
                scoutChoices.GetStringSelection()))
201
        killButton.Bind(wx.EVT_BUTTON, lambda event: self.RemoveScout(name))
202
203
        self.mainArea.Add(self.sizer[name], proportion=1,
204
            flag=wx.ALL | wx.EXPAND, border=10)
205
        self.window.Layout()
206
        return True
207
208
    def AddScout(self, name, x, y, theta):
209
        if AddToSimulator(name, x, y, theta):
210
            return self.AddScoutBox(name)
211
        wx.MessageBox("ROS call failed!", 'ROS Error', wx.OK | wx.ICON_ERROR)
212
        return False
213
214
    def RemoveScout(self, name):
215
        print("Hidden scoutLayout for " + name + ": "
216
            + str(self.mainArea.Hide(self.sizer[name], True)))
217
        print("Removed scoutLayout for " + name + ": "
218
            + str(self.mainArea.Remove(self.sizer[name])) + " (Maybe...)")
219
        del self.sizer[name]
220
        if KillScout(name):
221
            self.window.Layout()
222
            self.window.Refresh()
223
            self.window.Update()
224
        return
225 5755691e Matt Bryant
226
if __name__ == '__main__':
227 d2dedb17 Alex
    ScoutSimGui = subprocess.Popen("rosrun scoutsim scoutsim_node race", shell=True)
228
    rospy.wait_for_service('/spawn')
229
    #subprocess.Popen("rosservice call /spawn 1.4 0.275 0 scout1", shell=True)  # why was this here?
230
    app = wx.App()
231
    GUI(None, title='Colony Scout Manager')
232
    app.MainLoop()
233
    # This doesn't run.  I'm sure there is a reason why.
234
    for process in processes:
235
        process.kill()
236
    ScoutSimGui.kill()