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