axserver.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # Software License Agreement (BSD License)
3 #
4 # Copyright (c) 2009, Willow Garage, Inc.
5 # All rights reserved.
6 #
7 # Redistribution and use in source and binary forms, with or without
8 # modification, are permitted provided that the following conditions
9 # are met:
10 #
11 # * Redistributions of source code must retain the above copyright
12 # notice, this list of conditions and the following disclaimer.
13 # * Redistributions in binary form must reproduce the above
14 # copyright notice, this list of conditions and the following
15 # disclaimer in the documentation and/or other materials provided
16 # with the distribution.
17 # * Neither the name of Willow Garage, Inc. nor the names of its
18 # contributors may be used to endorse or promote products derived
19 # from this software without specific prior written permission.
20 #
21 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 # POSSIBILITY OF SUCH DAMAGE.
33 
34 """
35 usage: %prog /action_name action_type
36 """
37 
38 PKG = 'actionlib'
39 
40 from optparse import OptionParser
41 import roslib.message
42 import wx
43 import rospy
44 import actionlib
45 import threading
46 from cStringIO import StringIO
47 from library import to_yaml, yaml_msg_str
48 from dynamic_action import DynamicAction
49 
50 SEND_FEEDBACK = 0
51 SUCCEED = 1
52 ABORT = 2
53 PREEMPT = 3
54 
55 
56 class AXServerApp(wx.App):
57  def __init__(self, action_type, action_name):
58  self.action_type = action_type
59  wx.App.__init__(self)
60 
61  self.server = actionlib.SimpleActionServer(action_name, self.action_type.action, self.execute)
62  self.condition = threading.Condition()
63  self.feedback_msg = None
64  self.result_msg = None
65  self.execute_type = None
66 
67  def set_goal(self, goal):
68  if goal is None:
69  self.status_bg.SetBackgroundColour(wx.Colour(200, 0, 0))
70  self.status.SetLabel("Waiting For Goal...")
71  self.send_feedback.Disable()
72  self.succeed.Disable()
73  self.abort.Disable()
74  self.preempt.Disable()
75 
76  self.goal.SetValue("")
77 
78  else:
79  self.status_bg.SetBackgroundColour(wx.Colour(0, 200, 0))
80  self.status.SetLabel("Received Goal. Send feedback, succeed, or abort.")
81  self.send_feedback.Enable()
82  self.succeed.Enable()
83  self.abort.Enable()
84  self.preempt.Enable()
85 
86  try:
87  self.goal.SetValue(to_yaml(goal))
88  except UnicodeDecodeError:
89  self.goal.SetValue("Cannot display goal due to unprintable characters")
90 
92  self.status_bg.SetBackgroundColour(wx.Colour(0, 200, 200))
93  self.status.SetLabel("Preempt requested...")
94 
95  def execute(self, goal):
96 
97  wx.CallAfter(self.set_goal, goal)
98  self.condition.acquire()
99 
100  self.result_msg = None
101  self.feedback_msg = None
102  self.execute_type = None
103 
104  while self.execute_type is None or self.execute_type == SEND_FEEDBACK:
105  self.result_msg = None
106  self.feedback_msg = None
107  self.execute_type = None
108 
109  while self.execute_type is None:
110  if self.server.is_preempt_requested():
111  wx.CallAfter(self.set_preempt_requested)
112  self.condition.wait(1.0)
113 
114  if self.execute_type == SEND_FEEDBACK:
115  if self.feedback_msg is not None:
116  self.server.publish_feedback(self.feedback_msg)
117 
118  if self.execute_type == SUCCEED:
119  self.server.set_succeeded(self.result_msg)
120 
121  if self.execute_type == ABORT:
122  self.server.set_aborted()
123 
124  if self.execute_type == PREEMPT:
125  self.server.set_preempted()
126 
127  wx.CallAfter(self.set_goal, None)
128 
129  self.condition.release()
130 
131  def on_feedback(self, event):
132  self.condition.acquire()
133 
134  try:
135  self.feedback_msg = yaml_msg_str(self.action_type.feedback,
136  self.feedback.GetValue())
137  buff = StringIO()
138  self.feedback_msg.serialize(buff)
139 
140  self.execute_type = SEND_FEEDBACK
141  self.condition.notify()
142  except roslib.message.SerializationError as e:
143  self.feedback_msg = None
144  wx.MessageBox(str(e), "Error serializing feedback", wx.OK)
145 
146  self.condition.release()
147 
148  def on_succeed(self, event):
149  self.condition.acquire()
150 
151  try:
152  self.result_msg = yaml_msg_str(self.action_type.result, self.result.GetValue())
153  buff = StringIO()
154  self.result_msg.serialize(buff)
155 
156  self.execute_type = SUCCEED
157  self.condition.notify()
158  except roslib.message.SerializationError as e:
159  self.result_msg = None
160  wx.MessageBox(str(e), "Error serializing result", wx.OK)
161 
162  self.condition.release()
163 
164  def on_abort(self, event):
165  self.condition.acquire()
166 
167  self.execute_type = ABORT
168  self.condition.notify()
169 
170  self.condition.release()
171 
172  def on_preempt(self, event):
173  self.condition.acquire()
174 
175  self.execute_type = PREEMPT
176  self.condition.notify()
177 
178  self.condition.release()
179 
180  def OnInit(self):
181 
182  self.frame = wx.Frame(None, -1, self.action_type.name + ' Standin')
183 
184  self.sz = wx.BoxSizer(wx.VERTICAL)
185 
186  tmp_feedback = self.action_type.feedback()
187  tmp_result = self.action_type.result()
188 
189  self.goal = wx.TextCtrl(self.frame, -1, style=(wx.TE_MULTILINE | wx.TE_READONLY))
190  self.goal_st_bx = wx.StaticBox(self.frame, -1, "Goal")
191  self.goal_st = wx.StaticBoxSizer(self.goal_st_bx, wx.VERTICAL)
192  self.goal_st.Add(self.goal, 1, wx.EXPAND)
193 
194  self.feedback = wx.TextCtrl(self.frame, -1, style=wx.TE_MULTILINE)
195  self.feedback.SetValue(to_yaml(tmp_feedback))
196  self.feedback_st_bx = wx.StaticBox(self.frame, -1, "Feedback")
197  self.feedback_st = wx.StaticBoxSizer(self.feedback_st_bx, wx.VERTICAL)
198  self.feedback_st.Add(self.feedback, 1, wx.EXPAND)
199 
200  self.result = wx.TextCtrl(self.frame, -1, style=wx.TE_MULTILINE)
201  self.result.SetValue(to_yaml(tmp_result))
202  self.result_st_bx = wx.StaticBox(self.frame, -1, "Result")
203  self.result_st = wx.StaticBoxSizer(self.result_st_bx, wx.VERTICAL)
204  self.result_st.Add(self.result, 1, wx.EXPAND)
205 
206  self.send_feedback = wx.Button(self.frame, -1, label="SEND FEEDBACK")
207  self.send_feedback.Bind(wx.EVT_BUTTON, self.on_feedback)
208 
209  self.succeed = wx.Button(self.frame, -1, label="SUCCEED")
210  self.succeed.Bind(wx.EVT_BUTTON, self.on_succeed)
211 
212  self.abort = wx.Button(self.frame, -1, label="ABORT")
213  self.abort.Bind(wx.EVT_BUTTON, self.on_abort)
214 
215  self.preempt = wx.Button(self.frame, -1, label="PREEMPT")
216  self.preempt.Bind(wx.EVT_BUTTON, self.on_preempt)
217 
218  self.status_bg = wx.Panel(self.frame, -1)
219  self.status_bg.SetBackgroundColour(wx.Colour(200, 0, 0))
220  self.status = wx.StaticText(self.status_bg, -1, label="Waiting For Goal...")
221 
222  self.sz.Add(self.goal_st, 1, wx.EXPAND)
223  self.sz.Add(self.feedback_st, 1, wx.EXPAND)
224  self.sz.Add(self.result_st, 1, wx.EXPAND)
225  self.sz.Add(self.send_feedback, 0, wx.EXPAND)
226  self.sz.Add(self.succeed, 0, wx.EXPAND)
227  self.sz.Add(self.abort, 0, wx.EXPAND)
228  self.sz.Add(self.preempt, 0, wx.EXPAND)
229  self.sz.Add(self.status_bg, 0, wx.EXPAND)
230 
231  self.frame.SetSizer(self.sz)
232 
233  self.set_goal(None)
234 
235  self.sz.Layout()
236  self.frame.Show()
237 
238  return True
239 
240 
241 if __name__ == '__main__':
242  rospy.init_node('axserver', anonymous=True)
243 
244  parser = OptionParser(__doc__.strip())
245 # parser.add_option("-t","--test",action="store_true", dest="test",default=False,
246 # help="A testing flag")
247 # parser.add_option("-v","--var",action="store",type="string", dest="var",default="blah")
248 
249  (options, args) = parser.parse_args(rospy.myargv())
250 
251  if (len(args) != 3):
252  parser.error("You must specify the action name and type. Eg: ./axserver.py my_action actionlib/Test")
253 
254  action = DynamicAction(args[2])
255 
256  app = AXServerApp(action, args[1])
257  app.MainLoop()
258  rospy.signal_shutdown('GUI shutdown')
def on_preempt(self, event)
Definition: axserver.py:172
def __init__(self, action_type, action_name)
Definition: axserver.py:57
def on_succeed(self, event)
Definition: axserver.py:148
def execute(self, goal)
Definition: axserver.py:95
def on_feedback(self, event)
Definition: axserver.py:131
def on_abort(self, event)
Definition: axserver.py:164
def set_preempt_requested(self)
Definition: axserver.py:91
def OnInit(self)
Definition: axserver.py:180
def set_goal(self, goal)
Definition: axserver.py:67
def yaml_msg_str(type_, yaml_str, filename=None)
Definition: library.py:85
def to_yaml(obj)
Definition: library.py:77


actionlib
Author(s): Eitan Marder-Eppstein, Vijay Pradeep
autogenerated on Thu May 24 2018 02:39:31