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         self.action_name = action_name
00060         wx.App.__init__(self)
00061 
00062         self.server = actionlib.SimpleActionServer(
00063             self.action_name,
00064             self.action_type.action,
00065             self.execute
00066         )
00067         self.condition = threading.Condition()
00068         self.feedback_msg = None
00069         self.result_msg = None
00070         self.execute_type = None
00071 
00072     def set_goal(self, goal):
00073         if goal is None:
00074             self.status_bg.SetBackgroundColour(wx.Colour(200, 0, 0))
00075             self.status.SetLabel("Waiting For Goal...")
00076             self.send_feedback.Disable()
00077             self.succeed.Disable()
00078             self.abort.Disable()
00079             self.preempt.Disable()
00080 
00081             self.goal.SetValue("")
00082 
00083         else:
00084             self.status_bg.SetBackgroundColour(wx.Colour(0, 200, 0))
00085             self.status.SetLabel("Received Goal.  Send feedback, succeed, or abort.")
00086             self.send_feedback.Enable()
00087             self.succeed.Enable()
00088             self.abort.Enable()
00089             self.preempt.Enable()
00090 
00091             try:
00092                 self.goal.SetValue(to_yaml(goal))
00093             except UnicodeDecodeError:
00094                 self.goal.SetValue("Cannot display goal due to unprintable characters")
00095 
00096     def set_preempt_requested(self):
00097         self.status_bg.SetBackgroundColour(wx.Colour(0, 200, 200))
00098         self.status.SetLabel("Preempt requested...")
00099 
00100     def execute(self, goal):
00101 
00102         wx.CallAfter(self.set_goal, goal)
00103         self.condition.acquire()
00104 
00105         self.result_msg = None
00106         self.feedback_msg = None
00107         self.execute_type = None
00108 
00109         while self.execute_type is None or self.execute_type == SEND_FEEDBACK:
00110             self.result_msg = None
00111             self.feedback_msg = None
00112             self.execute_type = None
00113 
00114             while self.execute_type is None:
00115                 if self.server.is_preempt_requested():
00116                     wx.CallAfter(self.set_preempt_requested)
00117                 self.condition.wait(1.0)
00118 
00119             if self.execute_type == SEND_FEEDBACK:
00120                 if self.feedback_msg is not None:
00121                     self.server.publish_feedback(self.feedback_msg)
00122 
00123         if self.execute_type == SUCCEED:
00124             self.server.set_succeeded(self.result_msg)
00125 
00126         if self.execute_type == ABORT:
00127             self.server.set_aborted()
00128 
00129         if self.execute_type == PREEMPT:
00130             self.server.set_preempted()
00131 
00132         wx.CallAfter(self.set_goal, None)
00133 
00134         self.condition.release()
00135 
00136     def on_feedback(self, event):
00137         self.condition.acquire()
00138 
00139         try:
00140             self.feedback_msg = yaml_msg_str(self.action_type.feedback,
00141                                              self.feedback.GetValue())
00142             buff = StringIO()
00143             self.feedback_msg.serialize(buff)
00144 
00145             self.execute_type = SEND_FEEDBACK
00146             self.condition.notify()
00147         except roslib.message.SerializationError as e:
00148             self.feedback_msg = None
00149             wx.MessageBox(str(e), "Error serializing feedback", wx.OK)
00150 
00151         self.condition.release()
00152 
00153     def on_succeed(self, event):
00154         self.condition.acquire()
00155 
00156         try:
00157             self.result_msg = yaml_msg_str(self.action_type.result, self.result.GetValue())
00158             buff = StringIO()
00159             self.result_msg.serialize(buff)
00160 
00161             self.execute_type = SUCCEED
00162             self.condition.notify()
00163         except roslib.message.SerializationError as e:
00164             self.result_msg = None
00165             wx.MessageBox(str(e), "Error serializing result", wx.OK)
00166 
00167         self.condition.release()
00168 
00169     def on_abort(self, event):
00170         self.condition.acquire()
00171 
00172         self.execute_type = ABORT
00173         self.condition.notify()
00174 
00175         self.condition.release()
00176 
00177     def on_preempt(self, event):
00178         self.condition.acquire()
00179 
00180         self.execute_type = PREEMPT
00181         self.condition.notify()
00182 
00183         self.condition.release()
00184 
00185     def OnInit(self):
00186 
00187         self.frame = wx.Frame(
00188             None, -1,
00189             self.action_name + ' - ' + self.action_type.name + ' - Standin'
00190         )
00191 
00192         self.sz = wx.BoxSizer(wx.VERTICAL)
00193 
00194         tmp_feedback = self.action_type.feedback()
00195         tmp_result = self.action_type.result()
00196 
00197         self.goal = wx.TextCtrl(self.frame, -1, style=(wx.TE_MULTILINE | wx.TE_READONLY))
00198         self.goal_st_bx = wx.StaticBox(self.frame, -1, "Goal")
00199         self.goal_st = wx.StaticBoxSizer(self.goal_st_bx, wx.VERTICAL)
00200         self.goal_st.Add(self.goal, 1, wx.EXPAND)
00201 
00202         self.feedback = wx.TextCtrl(self.frame, -1, style=wx.TE_MULTILINE)
00203         self.feedback.SetValue(to_yaml(tmp_feedback))
00204         self.feedback_st_bx = wx.StaticBox(self.frame, -1, "Feedback")
00205         self.feedback_st = wx.StaticBoxSizer(self.feedback_st_bx, wx.VERTICAL)
00206         self.feedback_st.Add(self.feedback, 1, wx.EXPAND)
00207 
00208         self.result = wx.TextCtrl(self.frame, -1, style=wx.TE_MULTILINE)
00209         self.result.SetValue(to_yaml(tmp_result))
00210         self.result_st_bx = wx.StaticBox(self.frame, -1, "Result")
00211         self.result_st = wx.StaticBoxSizer(self.result_st_bx, wx.VERTICAL)
00212         self.result_st.Add(self.result, 1, wx.EXPAND)
00213 
00214         self.send_feedback = wx.Button(self.frame, -1, label="SEND FEEDBACK")
00215         self.send_feedback.Bind(wx.EVT_BUTTON, self.on_feedback)
00216 
00217         self.succeed = wx.Button(self.frame, -1, label="SUCCEED")
00218         self.succeed.Bind(wx.EVT_BUTTON, self.on_succeed)
00219 
00220         self.abort = wx.Button(self.frame, -1, label="ABORT")
00221         self.abort.Bind(wx.EVT_BUTTON, self.on_abort)
00222 
00223         self.preempt = wx.Button(self.frame, -1, label="PREEMPT")
00224         self.preempt.Bind(wx.EVT_BUTTON, self.on_preempt)
00225 
00226         self.status_bg = wx.Panel(self.frame, -1)
00227         self.status_bg.SetBackgroundColour(wx.Colour(200, 0, 0))
00228         self.status = wx.StaticText(self.status_bg, -1, label="Waiting For Goal...")
00229 
00230         self.sz.Add(self.goal_st, 1, wx.EXPAND)
00231         self.sz.Add(self.feedback_st, 1, wx.EXPAND)
00232         self.sz.Add(self.result_st, 1, wx.EXPAND)
00233         self.sz.Add(self.send_feedback, 0, wx.EXPAND)
00234         self.sz.Add(self.succeed, 0, wx.EXPAND)
00235         self.sz.Add(self.abort, 0, wx.EXPAND)
00236         self.sz.Add(self.preempt, 0, wx.EXPAND)
00237         self.sz.Add(self.status_bg, 0, wx.EXPAND)
00238 
00239         self.frame.SetSizer(self.sz)
00240 
00241         self.set_goal(None)
00242 
00243         self.sz.Layout()
00244         self.frame.Show()
00245 
00246         return True
00247 
00248 
00249 if __name__ == '__main__':
00250     rospy.init_node('axserver', anonymous=True)
00251 
00252     parser = OptionParser(__doc__.strip())
00253 #    parser.add_option("-t","--test",action="store_true", dest="test",default=False,
00254 #                      help="A testing flag")
00255 #  parser.add_option("-v","--var",action="store",type="string", dest="var",default="blah")
00256 
00257     (options, args) = parser.parse_args(rospy.myargv())
00258 
00259     if (len(args) != 3):
00260         parser.error("You must specify the action name and type. Eg: ./axserver.py my_action actionlib/Test")
00261 
00262     action = DynamicAction(args[2])
00263 
00264     app = AXServerApp(action, args[1])
00265     app.MainLoop()
00266     rospy.signal_shutdown('GUI shutdown')


actionlib
Author(s): Eitan Marder-Eppstein, Vijay Pradeep
autogenerated on Wed Aug 22 2018 02:38:23