axserver.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # Software License Agreement (BSD License)
00003 #
00004 # Copyright (c) 2009, Willow Garage, Inc.
00005 # All rights reserved.
00006 #
00007 # Redistribution and use in source and binary forms, with or without
00008 # modification, are permitted provided that the following conditions
00009 # are met:
00010 #
00011 #  * Redistributions of source code must retain the above copyright
00012 #    notice, this list of conditions and the following disclaimer.
00013 #  * Redistributions in binary form must reproduce the above
00014 #    copyright notice, this list of conditions and the following
00015 #    disclaimer in the documentation and/or other materials provided
00016 #    with the distribution.
00017 #  * Neither the name of Willow Garage, Inc. nor the names of its
00018 #    contributors may be used to endorse or promote products derived
00019 #    from this software without specific prior written permission.
00020 #
00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
00033 
00034 """
00035 usage: %prog /action_name action_type
00036 """
00037 
00038 PKG = 'actionlib'
00039 
00040 from optparse import OptionParser
00041 import roslib.message
00042 import wx
00043 import rospy
00044 import actionlib
00045 import threading
00046 from cStringIO import StringIO
00047 from library import to_yaml, yaml_msg_str
00048 from dynamic_action import DynamicAction
00049 
00050 SEND_FEEDBACK = 0
00051 SUCCEED = 1
00052 ABORT = 2
00053 PREEMPT = 3
00054 
00055 
00056 class AXServerApp(wx.App):
00057     def __init__(self, action_type, action_name):
00058         self.action_type = action_type
00059         wx.App.__init__(self)
00060 
00061         self.server = actionlib.SimpleActionServer(action_name, self.action_type.action, self.execute)
00062         self.condition = threading.Condition()
00063         self.feedback_msg = None
00064         self.result_msg = None
00065         self.execute_type = None
00066 
00067     def set_goal(self, goal):
00068         if goal is None:
00069             self.status_bg.SetBackgroundColour(wx.Colour(200, 0, 0))
00070             self.status.SetLabel("Waiting For Goal...")
00071             self.send_feedback.Disable()
00072             self.succeed.Disable()
00073             self.abort.Disable()
00074             self.preempt.Disable()
00075 
00076             self.goal.SetValue("")
00077 
00078         else:
00079             self.status_bg.SetBackgroundColour(wx.Colour(0, 200, 0))
00080             self.status.SetLabel("Received Goal.  Send feedback, succeed, or abort.")
00081             self.send_feedback.Enable()
00082             self.succeed.Enable()
00083             self.abort.Enable()
00084             self.preempt.Enable()
00085 
00086             try:
00087                 self.goal.SetValue(to_yaml(goal))
00088             except UnicodeDecodeError:
00089                 self.goal.SetValue("Cannot display goal due to unprintable characters")
00090 
00091     def set_preempt_requested(self):
00092         self.status_bg.SetBackgroundColour(wx.Colour(0, 200, 200))
00093         self.status.SetLabel("Preempt requested...")
00094 
00095     def execute(self, goal):
00096 
00097         wx.CallAfter(self.set_goal, goal)
00098         self.condition.acquire()
00099 
00100         self.result_msg = None
00101         self.feedback_msg = None
00102         self.execute_type = None
00103 
00104         while self.execute_type is None or self.execute_type == SEND_FEEDBACK:
00105             self.result_msg = None
00106             self.feedback_msg = None
00107             self.execute_type = None
00108 
00109             while self.execute_type is None:
00110                 if self.server.is_preempt_requested():
00111                     wx.CallAfter(self.set_preempt_requested)
00112                 self.condition.wait(1.0)
00113 
00114             if self.execute_type == SEND_FEEDBACK:
00115                 if self.feedback_msg is not None:
00116                     self.server.publish_feedback(self.feedback_msg)
00117 
00118         if self.execute_type == SUCCEED:
00119             self.server.set_succeeded(self.result_msg)
00120 
00121         if self.execute_type == ABORT:
00122             self.server.set_aborted()
00123 
00124         if self.execute_type == PREEMPT:
00125             self.server.set_preempted()
00126 
00127         wx.CallAfter(self.set_goal, None)
00128 
00129         self.condition.release()
00130 
00131     def on_feedback(self, event):
00132         self.condition.acquire()
00133 
00134         try:
00135             self.feedback_msg = yaml_msg_str(self.action_type.feedback,
00136                                              self.feedback.GetValue())
00137             buff = StringIO()
00138             self.feedback_msg.serialize(buff)
00139 
00140             self.execute_type = SEND_FEEDBACK
00141             self.condition.notify()
00142         except roslib.message.SerializationError as e:
00143             self.feedback_msg = None
00144             wx.MessageBox(str(e), "Error serializing feedback", wx.OK)
00145 
00146         self.condition.release()
00147 
00148     def on_succeed(self, event):
00149         self.condition.acquire()
00150 
00151         try:
00152             self.result_msg = yaml_msg_str(self.action_type.result, self.result.GetValue())
00153             buff = StringIO()
00154             self.result_msg.serialize(buff)
00155 
00156             self.execute_type = SUCCEED
00157             self.condition.notify()
00158         except roslib.message.SerializationError as e:
00159             self.result_msg = None
00160             wx.MessageBox(str(e), "Error serializing result", wx.OK)
00161 
00162         self.condition.release()
00163 
00164     def on_abort(self, event):
00165         self.condition.acquire()
00166 
00167         self.execute_type = ABORT
00168         self.condition.notify()
00169 
00170         self.condition.release()
00171 
00172     def on_preempt(self, event):
00173         self.condition.acquire()
00174 
00175         self.execute_type = PREEMPT
00176         self.condition.notify()
00177 
00178         self.condition.release()
00179 
00180     def OnInit(self):
00181 
00182         self.frame = wx.Frame(None, -1, self.action_type.name + ' Standin')
00183 
00184         self.sz = wx.BoxSizer(wx.VERTICAL)
00185 
00186         tmp_feedback = self.action_type.feedback()
00187         tmp_result = self.action_type.result()
00188 
00189         self.goal = wx.TextCtrl(self.frame, -1, style=(wx.TE_MULTILINE | wx.TE_READONLY))
00190         self.goal_st_bx = wx.StaticBox(self.frame, -1, "Goal")
00191         self.goal_st = wx.StaticBoxSizer(self.goal_st_bx, wx.VERTICAL)
00192         self.goal_st.Add(self.goal, 1, wx.EXPAND)
00193 
00194         self.feedback = wx.TextCtrl(self.frame, -1, style=wx.TE_MULTILINE)
00195         self.feedback.SetValue(to_yaml(tmp_feedback))
00196         self.feedback_st_bx = wx.StaticBox(self.frame, -1, "Feedback")
00197         self.feedback_st = wx.StaticBoxSizer(self.feedback_st_bx, wx.VERTICAL)
00198         self.feedback_st.Add(self.feedback, 1, wx.EXPAND)
00199 
00200         self.result = wx.TextCtrl(self.frame, -1, style=wx.TE_MULTILINE)
00201         self.result.SetValue(to_yaml(tmp_result))
00202         self.result_st_bx = wx.StaticBox(self.frame, -1, "Result")
00203         self.result_st = wx.StaticBoxSizer(self.result_st_bx, wx.VERTICAL)
00204         self.result_st.Add(self.result, 1, wx.EXPAND)
00205 
00206         self.send_feedback = wx.Button(self.frame, -1, label="SEND FEEDBACK")
00207         self.send_feedback.Bind(wx.EVT_BUTTON, self.on_feedback)
00208 
00209         self.succeed = wx.Button(self.frame, -1, label="SUCCEED")
00210         self.succeed.Bind(wx.EVT_BUTTON, self.on_succeed)
00211 
00212         self.abort = wx.Button(self.frame, -1, label="ABORT")
00213         self.abort.Bind(wx.EVT_BUTTON, self.on_abort)
00214 
00215         self.preempt = wx.Button(self.frame, -1, label="PREEMPT")
00216         self.preempt.Bind(wx.EVT_BUTTON, self.on_preempt)
00217 
00218         self.status_bg = wx.Panel(self.frame, -1)
00219         self.status_bg.SetBackgroundColour(wx.Colour(200, 0, 0))
00220         self.status = wx.StaticText(self.status_bg, -1, label="Waiting For Goal...")
00221 
00222         self.sz.Add(self.goal_st, 1, wx.EXPAND)
00223         self.sz.Add(self.feedback_st, 1, wx.EXPAND)
00224         self.sz.Add(self.result_st, 1, wx.EXPAND)
00225         self.sz.Add(self.send_feedback, 0, wx.EXPAND)
00226         self.sz.Add(self.succeed, 0, wx.EXPAND)
00227         self.sz.Add(self.abort, 0, wx.EXPAND)
00228         self.sz.Add(self.preempt, 0, wx.EXPAND)
00229         self.sz.Add(self.status_bg, 0, wx.EXPAND)
00230 
00231         self.frame.SetSizer(self.sz)
00232 
00233         self.set_goal(None)
00234 
00235         self.sz.Layout()
00236         self.frame.Show()
00237 
00238         return True
00239 
00240 
00241 if __name__ == '__main__':
00242     rospy.init_node('axserver', anonymous=True)
00243 
00244     parser = OptionParser(__doc__.strip())
00245 #    parser.add_option("-t","--test",action="store_true", dest="test",default=False,
00246 #                      help="A testing flag")
00247 #  parser.add_option("-v","--var",action="store",type="string", dest="var",default="blah")
00248 
00249     (options, args) = parser.parse_args(rospy.myargv())
00250 
00251     if (len(args) != 3):
00252         parser.error("You must specify the action name and type. Eg: ./axserver.py my_action actionlib/Test")
00253 
00254     action = DynamicAction(args[2])
00255 
00256     app = AXServerApp(action, args[1])
00257     app.MainLoop()
00258     rospy.signal_shutdown('GUI shutdown')


actionlib
Author(s): Eitan Marder-Eppstein, Vijay Pradeep
autogenerated on Thu Apr 19 2018 02:52:35