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