actionlib: axclient.py Source 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; roslib.load_manifest(PKG)
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 from cStringIO import StringIO
00053 from library import *
00054 from dynamic_action import DynamicAction
00055 from actionlib_msgs.msg import GoalStatus
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 
00078     def set_server_status(self, label, color, enabled):
00079         self.server_status_bg.SetBackgroundColour(color)
00080         self.server_status.SetLabel(label)
00081         if enabled:
00082             self.send_goal.Enable()
00083         else:
00084             self.send_goal.Disable()
00085 
00086     def server_check(self, event):
00087         TIMEOUT = 0.01
00088         if self.client.wait_for_server(rospy.Duration.from_sec(TIMEOUT)):
00089             wx.CallAfter(self.set_server_status, "Connected to server",
00090                          wx.Colour(192, 252, 253), True)
00091         else:
00092             wx.CallAfter(self.set_server_status, "Disconnected from server",
00093                          wx.Colour(200, 0, 0), False)
00094 
00095     def on_cancel(self, event):
00096         #we'll cancel the current goal
00097         self.client.cancel_goal()
00098         self.set_status("Canceling goal", wx.Colour(211, 34, 243))
00099 
00100     def on_goal(self, event):
00101         try:
00102             self.goal_msg = yaml_msg_str(self.action_type.goal,
00103                                          self.goal.GetValue())
00104             buff = StringIO()
00105             self.goal_msg.serialize(buff)
00106 
00107             #send the goal to the action server and register the relevant
00108             #callbacks
00109             self.client.send_goal(self.goal_msg, self.done_cb, self.active_cb,
00110                                   self.feedback_cb)
00111             self.set_status("Goal is pending", wx.Colour(255, 174, 59))
00112             self.set_cancel_button(True)
00113 
00114         except roslib.message.SerializationError, e:
00115             self.goal_msg = None
00116             wx.MessageBox(str(e), "Error serializing goal", wx.OK)
00117 
00118     def set_result(self, result):
00119         try:
00120             self.result.SetValue(to_yaml(result))
00121         except UnicodeDecodeError:
00122             self.result.SetValue("Cannot display result due to unprintable characters")
00123 
00124     def status_gui(self, status):
00125         return {GoalStatus.PENDING: ['PENDING', wx.Colour(255, 174, 59)],
00126                 GoalStatus.ACTIVE: ['ACTIVE', wx.Colour(0, 255, 0)],
00127                 GoalStatus.PREEMPTED: ['PREEMPTED', wx.Colour(255,252,16)],
00128                 GoalStatus.SUCCEEDED: ['SUCCEEDED',wx.Colour(38,250,253)],
00129                 GoalStatus.ABORTED: ['ABORTED',wx.Colour(200,0,0)],
00130                 GoalStatus.REJECTED: ['REJECTED',wx.Colour(253,38,159)],
00131                 GoalStatus.PREEMPTING: ['PREEMPTING',wx.Colour(253,38,159)],
00132                 GoalStatus.RECALLING: ['RECALLING',wx.Colour(230,38,253)],
00133                 GoalStatus.RECALLED: ['RECALLED',wx.Colour(230,38,253)],
00134                 GoalStatus.LOST: ['LOST',wx.Colour(255,0,0)]}[status]
00135 
00136     def done_cb(self, state, result):
00137         status_string, status_color = self.status_gui(state)
00138         wx.CallAfter(self.set_status, ''.join(["Goal finished with status: ",
00139                                                status_string]), status_color)
00140         wx.CallAfter(self.set_result, result)
00141         wx.CallAfter(self.set_cancel_button, False)
00142 
00143     def active_cb(self):
00144         wx.CallAfter(self.set_status, "Goal is active", wx.Colour(0,200,0))
00145 
00146     def set_feedback(self, feedback):
00147         try:
00148             self.feedback.SetValue(to_yaml(feedback))
00149         except UnicodeDecodeError:
00150             self.feedback.SetValue("Cannot display feedback due to unprintable characters")
00151 
00152     def feedback_cb(self, feedback):
00153         wx.CallAfter(self.set_feedback, feedback)
00154 
00155     def OnQuit(self):
00156         self.server_check_timer.Stop()
00157 
00158     def OnInit(self):
00159 
00160         self.frame = wx.Frame(None, -1, self.action_type.name + ' GUI Client')
00161 
00162         self.sz = wx.BoxSizer(wx.VERTICAL)
00163 
00164         tmp_goal = self.action_type.goal()
00165 
00166         self.goal = wx.TextCtrl(self.frame, -1, style=wx.TE_MULTILINE)
00167         self.goal.SetValue(to_yaml(tmp_goal))
00168         self.goal_st_bx = wx.StaticBox(self.frame, -1, "Goal")
00169         self.goal_st = wx.StaticBoxSizer(self.goal_st_bx, wx.VERTICAL)
00170         self.goal_st.Add(self.goal, 1, wx.EXPAND)
00171         
00172         self.feedback = wx.TextCtrl(self.frame, -1, style=(wx.TE_MULTILINE |
00173                                                            wx.TE_READONLY))
00174         self.feedback_st_bx = wx.StaticBox(self.frame, -1, "Feedback")
00175         self.feedback_st = wx.StaticBoxSizer(self.feedback_st_bx, wx.VERTICAL)
00176         self.feedback_st.Add(self.feedback, 1, wx.EXPAND)
00177 
00178         self.result = wx.TextCtrl(self.frame, -1, style=(wx.TE_MULTILINE |
00179                                                          wx.TE_READONLY))
00180         self.result_st_bx = wx.StaticBox(self.frame, -1, "Result")
00181         self.result_st = wx.StaticBoxSizer(self.result_st_bx, wx.VERTICAL)
00182         self.result_st.Add(self.result, 1, wx.EXPAND)
00183 
00184         self.send_goal = wx.Button(self.frame, -1, label="SEND GOAL")
00185         self.send_goal.Bind(wx.EVT_BUTTON, self.on_goal)
00186         self.send_goal.Disable()
00187 
00188         self.cancel_goal = wx.Button(self.frame, -1, label="CANCEL GOAL")
00189         self.cancel_goal.Bind(wx.EVT_BUTTON, self.on_cancel)
00190         self.cancel_goal.Disable()
00191 
00192         self.status_bg = wx.Panel(self.frame, -1)
00193         self.status_bg.SetBackgroundColour(wx.Colour(200,0,0))
00194         self.status = wx.StaticText(self.status_bg, -1, label="No Goal")
00195 
00196         self.server_status_bg = wx.Panel(self.frame, -1)
00197         self.server_status_bg.SetBackgroundColour(wx.Colour(200,0,0))
00198         self.server_status = wx.StaticText(self.server_status_bg, -1, label="Disconnected from server.")
00199 
00200         self.sz.Add(self.goal_st, 1, wx.EXPAND)
00201         self.sz.Add(self.feedback_st, 1, wx.EXPAND)
00202         self.sz.Add(self.result_st, 1, wx.EXPAND)
00203         self.sz.Add(self.send_goal, 0, wx.EXPAND)
00204         self.sz.Add(self.cancel_goal, 0, wx.EXPAND)
00205         self.sz.Add(self.status_bg, 0, wx.EXPAND)
00206         self.sz.Add(self.server_status_bg, 0, wx.EXPAND)
00207 
00208         self.frame.SetSizer(self.sz)
00209 
00210         self.server_check_timer = wx.Timer(self.frame)
00211         self.frame.Bind(wx.EVT_TIMER, self.server_check,
00212                         self.server_check_timer)
00213         self.server_check_timer.Start(1000)
00214 
00215         self.sz.Layout()
00216         self.frame.Show()
00217 
00218         return True
00219 
00220 if __name__ == '__main__':
00221     rospy.init_node('axclient', anonymous=True)
00222 
00223     parser = OptionParser(__doc__.strip())
00224 #    parser.add_option("-t","--test",action="store_true", dest="test",default=False,
00225 #                      help="A testing flag")
00226 #  parser.add_option("-v","--var",action="store",type="string", dest="var",default="blah")
00227     
00228     (options, args) = parser.parse_args(rospy.myargv())
00229 
00230     if (len(args) != 3):
00231         parser.error("You must specify the action name and type. Eg: ./axclient.py my_action actionlib/Test")
00232 
00233     action = DynamicAction(args[2])
00234 
00235     app = AXClientApp(action, args[1])
00236     app.MainLoop()
00237     app.OnQuit()
00238     rospy.signal_shutdown('GUI shutdown')