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 sys
00044 import rospy
00045 import actionlib
00046 import time
00047 import threading
00048 from cStringIO import StringIO
00049 from library import *
00050 from dynamic_action import DynamicAction
00051 
00052 SEND_FEEDBACK = 0
00053 SUCCEED = 1
00054 ABORT = 2
00055 PREEMPT = 3
00056 
00057 class AXServerApp(wx.App):
00058     def __init__(self, action_type, action_name):
00059         self.action_type = action_type
00060         wx.App.__init__(self)
00061 
00062         self.server = actionlib.SimpleActionServer(action_name, self.action_type.action, self.execute)
00063         self.condition = threading.Condition()
00064         self.feedback_msg = None
00065         self.result_msg = None
00066         self.execute_type = None
00067 
00068 
00069     def set_goal(self, goal):
00070         if goal is None:
00071             self.status_bg.SetBackgroundColour(wx.Colour(200,0,0))
00072             self.status.SetLabel("Waiting For Goal...")
00073             self.send_feedback.Disable()
00074             self.succeed.Disable()
00075             self.abort.Disable()            
00076             self.preempt.Disable()            
00077 
00078             self.goal.SetValue("")
00079 
00080         else:
00081             self.status_bg.SetBackgroundColour(wx.Colour(0,200,0))
00082             self.status.SetLabel("Received Goal.  Send feedback, succeed, or abort.")
00083             self.send_feedback.Enable()
00084             self.succeed.Enable()
00085             self.abort.Enable()
00086             self.preempt.Enable()            
00087 
00088             try:
00089                 self.goal.SetValue(to_yaml(goal))
00090             except UnicodeDecodeError:
00091                 self.goal.SetValue("Cannot display goal due to unprintable characters")
00092 
00093     def execute(self, goal):
00094 
00095         wx.CallAfter(self.set_goal, goal)
00096 
00097         self.condition.acquire()
00098 
00099         self.result_msg = None
00100         self.feedback_msg = None
00101         self.execute_type = None
00102         
00103         while self.execute_type is None or self.execute_type == SEND_FEEDBACK:
00104 
00105             self.result_msg = None
00106             self.feedback_msg = None
00107             self.execute_type = None
00108             
00109             while self.execute_type is None:
00110                 self.condition.wait()
00111 
00112             if self.execute_type == SEND_FEEDBACK:
00113                 if self.feedback_msg is not None:
00114                     self.server.publish_feedback(self.feedback_msg)
00115 
00116 
00117         if self.execute_type == SUCCEED:
00118             self.server.set_succeeded(self.result_msg)
00119 
00120         if self.execute_type == ABORT:
00121             self.server.set_aborted()
00122 
00123         if self.execute_type == PREEMPT:
00124             self.server.set_preempted()
00125 
00126         wx.CallAfter(self.set_goal, None)
00127 
00128         self.condition.release()
00129 
00130     def on_feedback(self, event):
00131         self.condition.acquire()
00132         
00133         try:
00134             self.feedback_msg = yaml_msg_str(self.action_type.feedback,
00135                                              self.feedback.GetValue())
00136             buff = StringIO()
00137             self.feedback_msg.serialize(buff)
00138             
00139             self.execute_type = SEND_FEEDBACK
00140             self.condition.notify()
00141         except roslib.message.SerializationError, e:
00142             self.feedback_msg = None
00143             wx.MessageBox(str(e), "Error serializing feedback", wx.OK)
00144 
00145         self.condition.release()
00146 
00147 
00148 
00149     def on_succeed(self, event):
00150         self.condition.acquire()
00151         
00152         try:
00153             self.result_msg = yaml_msg_str(self.action_type.result, self.result.GetValue())
00154             buff = StringIO()
00155             self.result_msg.serialize(buff)
00156             
00157             self.execute_type = SUCCEED
00158             self.condition.notify()
00159         except roslib.message.SerializationError, e:
00160             self.result_msg = None
00161             wx.MessageBox(str(e), "Error serializing result", wx.OK)
00162 
00163         self.condition.release()
00164 
00165     def on_abort(self, event):
00166         self.condition.acquire()
00167 
00168         self.execute_type = ABORT
00169         self.condition.notify()
00170 
00171         self.condition.release()
00172 
00173     def on_preempt(self, event):
00174         self.condition.acquire()
00175 
00176         self.execute_type = PREEMPT
00177         self.condition.notify()
00178 
00179         self.condition.release()
00180 
00181     def OnInit(self):
00182 
00183         self.frame = wx.Frame(None, -1, self.action_type.name + ' Standin')
00184 
00185         self.sz = wx.BoxSizer(wx.VERTICAL)
00186 
00187         tmp_feedback = self.action_type.feedback()
00188         tmp_result = self.action_type.result()
00189 
00190         self.goal = wx.TextCtrl(self.frame, -1, style=(wx.TE_MULTILINE | wx.TE_READONLY))
00191         self.goal_st_bx = wx.StaticBox(self.frame, -1, "Goal")
00192         self.goal_st = wx.StaticBoxSizer(self.goal_st_bx, wx.VERTICAL)
00193         self.goal_st.Add(self.goal, 1, wx.EXPAND)
00194         
00195         self.feedback = wx.TextCtrl(self.frame, -1, style=wx.TE_MULTILINE)
00196         self.feedback.SetValue(to_yaml(tmp_feedback))
00197         self.feedback_st_bx = wx.StaticBox(self.frame, -1, "Feedback")
00198         self.feedback_st = wx.StaticBoxSizer(self.feedback_st_bx, wx.VERTICAL)
00199         self.feedback_st.Add(self.feedback, 1, wx.EXPAND)
00200 
00201         self.result = wx.TextCtrl(self.frame, -1, style=wx.TE_MULTILINE)
00202         self.result.SetValue(to_yaml(tmp_result))
00203         self.result_st_bx = wx.StaticBox(self.frame, -1, "Result")
00204         self.result_st = wx.StaticBoxSizer(self.result_st_bx, wx.VERTICAL)
00205         self.result_st.Add(self.result, 1, wx.EXPAND)
00206 
00207         self.send_feedback = wx.Button(self.frame, -1, label="SEND FEEDBACK")
00208         self.send_feedback.Bind(wx.EVT_BUTTON, self.on_feedback)
00209 
00210         self.succeed = wx.Button(self.frame, -1, label="SUCCEED")
00211         self.succeed.Bind(wx.EVT_BUTTON, self.on_succeed)
00212 
00213         self.abort    = wx.Button(self.frame, -1, label="ABORT")
00214         self.abort.Bind(wx.EVT_BUTTON, self.on_abort)
00215 
00216         self.preempt    = wx.Button(self.frame, -1, label="PREEMPT")
00217         self.preempt.Bind(wx.EVT_BUTTON, self.on_preempt)
00218 
00219         self.status_bg = wx.Panel(self.frame, -1)
00220         self.status_bg.SetBackgroundColour(wx.Colour(200,0,0))
00221         self.status = wx.StaticText(self.status_bg, -1, label="Waiting For Goal...")
00222 
00223         self.sz.Add(self.goal_st, 1, wx.EXPAND)
00224         self.sz.Add(self.feedback_st, 1, wx.EXPAND)
00225         self.sz.Add(self.result_st, 1, wx.EXPAND)
00226         self.sz.Add(self.send_feedback, 0, wx.EXPAND)
00227         self.sz.Add(self.succeed, 0, wx.EXPAND)
00228         self.sz.Add(self.abort, 0, wx.EXPAND)
00229         self.sz.Add(self.preempt, 0, wx.EXPAND)
00230         self.sz.Add(self.status_bg, 0, wx.EXPAND)
00231 
00232         self.frame.SetSizer(self.sz)
00233 
00234         self.set_goal(None)
00235 
00236         self.sz.Layout()
00237         self.frame.Show()
00238 
00239         return True
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 Fri Aug 28 2015 10:04:41