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
00036
00037 """
00038 usage: %prog /action_name action_type
00039 """
00040
00041 PKG = 'actionlib'
00042
00043 import roslib.message
00044
00045 from optparse import OptionParser
00046 import wx
00047 import rospy
00048 import actionlib
00049 import threading
00050 import rostopic
00051 from cStringIO import StringIO
00052 from library import to_yaml, yaml_msg_str
00053 from dynamic_action import DynamicAction
00054 from actionlib_msgs.msg import GoalStatus
00055
00056
00057 class AXClientApp(wx.App):
00058 def __init__(self, action_type, action_name):
00059 self.action_type = action_type
00060 wx.App.__init__(self)
00061
00062 self.client = actionlib.SimpleActionClient(action_name, self.action_type.action)
00063 self.condition = threading.Condition()
00064 self.goal_msg = None
00065 self.execute_type = None
00066
00067 def set_status(self, label, color):
00068 self.status_bg.SetBackgroundColour(color)
00069 self.status.SetLabel(label)
00070
00071 def set_cancel_button(self, enabled):
00072 if enabled:
00073 self.cancel_goal.Enable()
00074 else:
00075 self.cancel_goal.Disable()
00076
00077 def set_server_status(self, label, color, enabled):
00078 self.server_status_bg.SetBackgroundColour(color)
00079 self.server_status.SetLabel(label)
00080 if enabled:
00081 self.send_goal.Enable()
00082 else:
00083 self.send_goal.Disable()
00084
00085 def server_check(self, event):
00086 TIMEOUT = 0.01
00087 if self.client.wait_for_server(rospy.Duration.from_sec(TIMEOUT)):
00088 wx.CallAfter(self.set_server_status, "Connected to server",
00089 wx.Colour(192, 252, 253), True)
00090 else:
00091 wx.CallAfter(self.set_server_status, "Disconnected from server",
00092 wx.Colour(200, 0, 0), False)
00093
00094 def on_cancel(self, event):
00095
00096 self.client.cancel_goal()
00097 self.set_status("Canceling goal", wx.Colour(211, 34, 243))
00098
00099 def on_goal(self, event):
00100 try:
00101 self.goal_msg = yaml_msg_str(self.action_type.goal,
00102 self.goal.GetValue())
00103 buff = StringIO()
00104 self.goal_msg.serialize(buff)
00105
00106
00107
00108 self.client.send_goal(self.goal_msg, self.done_cb, self.active_cb,
00109 self.feedback_cb)
00110 self.set_status("Goal is pending", wx.Colour(255, 174, 59))
00111 self.set_cancel_button(True)
00112
00113 except roslib.message.SerializationError as e:
00114 self.goal_msg = None
00115 wx.MessageBox(str(e), "Error serializing goal", wx.OK)
00116
00117 def set_result(self, result):
00118 try:
00119 self.result.SetValue(to_yaml(result))
00120 except UnicodeDecodeError:
00121 self.result.SetValue("Cannot display result due to unprintable characters")
00122
00123 def status_gui(self, status):
00124 return {GoalStatus.PENDING: ['PENDING', wx.Colour(255, 174, 59)],
00125 GoalStatus.ACTIVE: ['ACTIVE', wx.Colour(0, 255, 0)],
00126 GoalStatus.PREEMPTED: ['PREEMPTED', wx.Colour(255, 252, 16)],
00127 GoalStatus.SUCCEEDED: ['SUCCEEDED', wx.Colour(38, 250, 253)],
00128 GoalStatus.ABORTED: ['ABORTED', wx.Colour(200, 0, 0)],
00129 GoalStatus.REJECTED: ['REJECTED', wx.Colour(253, 38, 159)],
00130 GoalStatus.PREEMPTING: ['PREEMPTING', wx.Colour(253, 38, 159)],
00131 GoalStatus.RECALLING: ['RECALLING', wx.Colour(230, 38, 253)],
00132 GoalStatus.RECALLED: ['RECALLED', wx.Colour(230, 38, 253)],
00133 GoalStatus.LOST: ['LOST', wx.Colour(255, 0, 0)]}[status]
00134
00135 def done_cb(self, state, result):
00136 status_string, status_color = self.status_gui(state)
00137 wx.CallAfter(self.set_status, ''.join(["Goal finished with status: ",
00138 status_string]), status_color)
00139 wx.CallAfter(self.set_result, result)
00140 wx.CallAfter(self.set_cancel_button, False)
00141
00142 def active_cb(self):
00143 wx.CallAfter(self.set_status, "Goal is active", wx.Colour(0, 200, 0))
00144
00145 def set_feedback(self, feedback):
00146 try:
00147 self.feedback.SetValue(to_yaml(feedback))
00148 except UnicodeDecodeError:
00149 self.feedback.SetValue("Cannot display feedback due to unprintable characters")
00150
00151 def feedback_cb(self, feedback):
00152 wx.CallAfter(self.set_feedback, feedback)
00153
00154 def OnQuit(self):
00155 self.server_check_timer.Stop()
00156
00157 def OnInit(self):
00158
00159 self.frame = wx.Frame(None, -1, self.action_type.name + ' GUI Client')
00160
00161 self.sz = wx.BoxSizer(wx.VERTICAL)
00162
00163 tmp_goal = self.action_type.goal()
00164
00165 self.goal = wx.TextCtrl(self.frame, -1, style=wx.TE_MULTILINE)
00166 self.goal.SetValue(to_yaml(tmp_goal))
00167 self.goal_st_bx = wx.StaticBox(self.frame, -1, "Goal")
00168 self.goal_st = wx.StaticBoxSizer(self.goal_st_bx, wx.VERTICAL)
00169 self.goal_st.Add(self.goal, 1, wx.EXPAND)
00170
00171 self.feedback = wx.TextCtrl(self.frame, -1, style=(wx.TE_MULTILINE |
00172 wx.TE_READONLY))
00173 self.feedback_st_bx = wx.StaticBox(self.frame, -1, "Feedback")
00174 self.feedback_st = wx.StaticBoxSizer(self.feedback_st_bx, wx.VERTICAL)
00175 self.feedback_st.Add(self.feedback, 1, wx.EXPAND)
00176
00177 self.result = wx.TextCtrl(self.frame, -1, style=(wx.TE_MULTILINE |
00178 wx.TE_READONLY))
00179 self.result_st_bx = wx.StaticBox(self.frame, -1, "Result")
00180 self.result_st = wx.StaticBoxSizer(self.result_st_bx, wx.VERTICAL)
00181 self.result_st.Add(self.result, 1, wx.EXPAND)
00182
00183 self.send_goal = wx.Button(self.frame, -1, label="SEND GOAL")
00184 self.send_goal.Bind(wx.EVT_BUTTON, self.on_goal)
00185 self.send_goal.Disable()
00186
00187 self.cancel_goal = wx.Button(self.frame, -1, label="CANCEL GOAL")
00188 self.cancel_goal.Bind(wx.EVT_BUTTON, self.on_cancel)
00189 self.cancel_goal.Disable()
00190
00191 self.status_bg = wx.Panel(self.frame, -1)
00192 self.status_bg.SetBackgroundColour(wx.Colour(200, 0, 0))
00193 self.status = wx.StaticText(self.status_bg, -1, label="No Goal")
00194
00195 self.server_status_bg = wx.Panel(self.frame, -1)
00196 self.server_status_bg.SetBackgroundColour(wx.Colour(200, 0, 0))
00197 self.server_status = wx.StaticText(self.server_status_bg, -1, label="Disconnected from server.")
00198
00199 self.sz.Add(self.goal_st, 1, wx.EXPAND)
00200 self.sz.Add(self.feedback_st, 1, wx.EXPAND)
00201 self.sz.Add(self.result_st, 1, wx.EXPAND)
00202 self.sz.Add(self.send_goal, 0, wx.EXPAND)
00203 self.sz.Add(self.cancel_goal, 0, wx.EXPAND)
00204 self.sz.Add(self.status_bg, 0, wx.EXPAND)
00205 self.sz.Add(self.server_status_bg, 0, wx.EXPAND)
00206
00207 self.frame.SetSizer(self.sz)
00208
00209 self.server_check_timer = wx.Timer(self.frame)
00210 self.frame.Bind(wx.EVT_TIMER, self.server_check,
00211 self.server_check_timer)
00212 self.server_check_timer.Start(1000)
00213
00214 self.sz.Layout()
00215 self.frame.Show()
00216
00217 return True
00218
00219
00220 def main():
00221 rospy.init_node('axclient', anonymous=True)
00222
00223 parser = OptionParser(__doc__.strip())
00224
00225
00226
00227
00228 (options, args) = parser.parse_args(rospy.myargv())
00229
00230 if (len(args) == 2):
00231
00232 topic_type = rostopic._get_topic_type("%s/goal" % args[1])[0]
00233
00234 assert("Goal" in topic_type)
00235 topic_type = topic_type[0:len(topic_type)-4]
00236 elif (len(args) == 3):
00237 topic_type = args[2]
00238 print(topic_type)
00239 assert("Action" in topic_type)
00240 else:
00241 parser.error("You must specify the action topic name (and optionally type) Eg: ./axclient.py action_topic actionlib/TwoIntsAction ")
00242
00243 action = DynamicAction(topic_type)
00244 app = AXClientApp(action, args[1])
00245 app.MainLoop()
00246 app.OnQuit()
00247 rospy.signal_shutdown('GUI shutdown')
00248
00249
00250 if __name__ == '__main__':
00251 main()