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