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