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 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
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')