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 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         # we'll cancel the current goal
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             # send the goal to the action server and register the relevant
00109             # callbacks
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 #    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         # get action type via rostopic
00237         topic_type = rostopic._get_topic_type("%s/goal" % args[1])[0]
00238         # remove "Goal" string from action type
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()