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