00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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
00246
00247
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')