Statistics
| Branch: | Revision:

root / scout / scoutsim / BehaviorGUI.py @ fa5ca6e3

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