actionlib: axclient.py Source File

Go to the documentation of this file.
00001 #! /usr/bin/python
00002 #***********************************************************
00003 #* Software License Agreement (BSD License)
00004 #*
00005 #*  Copyright (c) 2009, Willow Garage, Inc.
00006 #*  All rights reserved.
00007 #*
00008 #*  Redistribution and use in source and binary forms, with or without
00009 #*  modification, are permitted provided that the following conditions
00010 #*  are met:
00011 #*
00012 #*   * Redistributions of source code must retain the above copyright
00013 #*     notice, this list of conditions and the following disclaimer.
00014 #*   * Redistributions in binary form must reproduce the above
00015 #*     copyright notice, this list of conditions and the following
00016 #*     disclaimer in the documentation and/or other materials provided
00017 #*     with the distribution.
00018 #*   * Neither the name of Willow Garage, Inc. nor the names of its
00019 #*     contributors may be used to endorse or promote products derived
00020 #*     from this software without specific prior written permission.
00021 #*
00022 #*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 #*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 #*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 #*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 #*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 #*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 #*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 #*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 #*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 #*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 #*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 #*  POSSIBILITY OF SUCH DAMAGE.
00034 #* 
00035 #* Author: Eitan Marder-Eppstein
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         #we'll cancel the current goal
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             #send the goal to the action server and register the relevant
00110             #callbacks
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 #    parser.add_option("-t","--test",action="store_true", dest="test",default=False,
00230 #                      help="A testing flag")
00231 #  parser.add_option("-v","--var",action="store",type="string", dest="var",default="blah")
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     # get action type from topic
00239     topic_type = rostopic._get_topic_type("%s/goal"%args[1])[0]
00240 
00241     #check if topic exists
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     # remove "Goal" string from action type
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()