axclient.py

Go to the documentation of this file.

#! /usr/bin/python
# **********************************************************
#  Software License Agreement (BSD License)
#
#   Copyright (c) 2009, Willow Garage, Inc.
#   All rights reserved.
#
#   Redistribution and use in source and binary forms, with or without
#   modification, are permitted provided that the following conditions
#   are met:
#
#    * Redistributions of source code must retain the above copyright
#      notice, this list of conditions and the following disclaimer.
#    * Redistributions in binary form must reproduce the above
#      copyright notice, this list of conditions and the following
#      disclaimer in the documentation and/or other materials provided
#      with the distribution.
#    * Neither the name of Willow Garage, Inc. nor the names of its
#      contributors may be used to endorse or promote products derived
#      from this software without specific prior written permission.
#
#   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
#   "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
#   LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
#   FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
#   COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
#   INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
#   BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
#   LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
#   CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
#   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
#   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
#   POSSIBILITY OF SUCH DAMAGE.
#
#  Author: Eitan Marder-Eppstein
# **********************************************************
"""
usage: %prog /action_name action_type
"""

PKG = 'actionlib'

import roslib.message

from optparse import OptionParser
import wx
import rospy
import actionlib
import threading
import rostopic
from cStringIO import StringIO
from library import to_yaml, yaml_msg_str
from dynamic_action import DynamicAction
from actionlib_msgs.msg import GoalStatus


class AXClientApp(wx.App):
    def __init__(self, action_type, action_name):
        self.action_type = action_type
        self.action_name = action_name
        wx.App.__init__(self)

        self.client = actionlib.SimpleActionClient(
            self.action_name, self.action_type.action)
        self.condition = threading.Condition()
        self.goal_msg = None
        self.execute_type = None

    def set_status(self, label, color):
        self.status_bg.SetBackgroundColour(color)
        self.status.SetLabel(label)

    def set_cancel_button(self, enabled):
        if enabled:
            self.cancel_goal.Enable()
        else:
            self.cancel_goal.Disable()

    def set_server_status(self, label, color, enabled):
        self.server_status_bg.SetBackgroundColour(color)
        self.server_status.SetLabel(label)
        if enabled:
            self.send_goal.Enable()
        else:
            self.send_goal.Disable()

    def server_check(self, event):
        TIMEOUT = 0.01
        if self.client.wait_for_server(rospy.Duration.from_sec(TIMEOUT)):
            wx.CallAfter(self.set_server_status, "Connected to server",
                         wx.Colour(192, 252, 253), True)
        else:
            wx.CallAfter(self.set_server_status, "Disconnected from server",
                         wx.Colour(200, 0, 0), False)

    def on_cancel(self, event):
        # we'll cancel the current goal
        self.client.cancel_goal()
        self.set_status("Canceling goal", wx.Colour(211, 34, 243))

    def on_goal(self, event):
        try:
            self.goal_msg = yaml_msg_str(self.action_type.goal,
                                         self.goal.GetValue())
            buff = StringIO()
            self.goal_msg.serialize(buff)

            # send the goal to the action server and register the relevant
            # callbacks
            self.client.send_goal(self.goal_msg, self.done_cb, self.active_cb,
                                  self.feedback_cb)
            self.set_status("Goal is pending", wx.Colour(255, 174, 59))
            self.set_cancel_button(True)

        except roslib.message.SerializationError as e:
            self.goal_msg = None
            wx.MessageBox(str(e), "Error serializing goal", wx.OK)

    def set_result(self, result):
        try:
            self.result.SetValue(to_yaml(result))
        except UnicodeDecodeError:
            self.result.SetValue("Cannot display result due to unprintable characters")

    def status_gui(self, status):
        return {GoalStatus.PENDING: ['PENDING', wx.Colour(255, 174, 59)],
                GoalStatus.ACTIVE: ['ACTIVE', wx.Colour(0, 255, 0)],
                GoalStatus.PREEMPTED: ['PREEMPTED', wx.Colour(255, 252, 16)],
                GoalStatus.SUCCEEDED: ['SUCCEEDED', wx.Colour(38, 250, 253)],
                GoalStatus.ABORTED: ['ABORTED', wx.Colour(200, 0, 0)],
                GoalStatus.REJECTED: ['REJECTED', wx.Colour(253, 38, 159)],
                GoalStatus.PREEMPTING: ['PREEMPTING', wx.Colour(253, 38, 159)],
                GoalStatus.RECALLING: ['RECALLING', wx.Colour(230, 38, 253)],
                GoalStatus.RECALLED: ['RECALLED', wx.Colour(230, 38, 253)],
                GoalStatus.LOST: ['LOST', wx.Colour(255, 0, 0)]}[status]

    def done_cb(self, state, result):
        status_string, status_color = self.status_gui(state)
        wx.CallAfter(self.set_status, ''.join(["Goal finished with status: ",
                                               status_string]), status_color)
        wx.CallAfter(self.set_result, result)
        wx.CallAfter(self.set_cancel_button, False)

    def active_cb(self):
        wx.CallAfter(self.set_status, "Goal is active", wx.Colour(0, 200, 0))

    def set_feedback(self, feedback):
        try:
            self.feedback.SetValue(to_yaml(feedback))
        except UnicodeDecodeError:
            self.feedback.SetValue("Cannot display feedback due to unprintable characters")

    def feedback_cb(self, feedback):
        wx.CallAfter(self.set_feedback, feedback)

    def OnQuit(self):
        self.server_check_timer.Stop()

    def OnInit(self):

        self.frame = wx.Frame(
            None, -1,
            self.action_name + ' - ' + self.action_type.name + ' - GUI Client'
        )

        self.sz = wx.BoxSizer(wx.VERTICAL)

        tmp_goal = self.action_type.goal()

        self.goal = wx.TextCtrl(self.frame, -1, style=wx.TE_MULTILINE)
        self.goal.SetValue(to_yaml(tmp_goal))
        self.goal_st_bx = wx.StaticBox(self.frame, -1, "Goal")
        self.goal_st = wx.StaticBoxSizer(self.goal_st_bx, wx.VERTICAL)
        self.goal_st.Add(self.goal, 1, wx.EXPAND)

        self.feedback = wx.TextCtrl(self.frame, -1, style=(wx.TE_MULTILINE |
                                                           wx.TE_READONLY))
        self.feedback_st_bx = wx.StaticBox(self.frame, -1, "Feedback")
        self.feedback_st = wx.StaticBoxSizer(self.feedback_st_bx, wx.VERTICAL)
        self.feedback_st.Add(self.feedback, 1, wx.EXPAND)

        self.result = wx.TextCtrl(self.frame, -1, style=(wx.TE_MULTILINE |
                                                         wx.TE_READONLY))
        self.result_st_bx = wx.StaticBox(self.frame, -1, "Result")
        self.result_st = wx.StaticBoxSizer(self.result_st_bx, wx.VERTICAL)
        self.result_st.Add(self.result, 1, wx.EXPAND)

        self.send_goal = wx.Button(self.frame, -1, label="SEND GOAL")
        self.send_goal.Bind(wx.EVT_BUTTON, self.on_goal)
        self.send_goal.Disable()

        self.cancel_goal = wx.Button(self.frame, -1, label="CANCEL GOAL")
        self.cancel_goal.Bind(wx.EVT_BUTTON, self.on_cancel)
        self.cancel_goal.Disable()

        self.status_bg = wx.Panel(self.frame, -1)
        self.status_bg.SetBackgroundColour(wx.Colour(200, 0, 0))
        self.status = wx.StaticText(self.status_bg, -1, label="No Goal")

        self.server_status_bg = wx.Panel(self.frame, -1)
        self.server_status_bg.SetBackgroundColour(wx.Colour(200, 0, 0))
        self.server_status = wx.StaticText(self.server_status_bg, -1, label="Disconnected from server.")

        self.sz.Add(self.goal_st, 1, wx.EXPAND)
        self.sz.Add(self.feedback_st, 1, wx.EXPAND)
        self.sz.Add(self.result_st, 1, wx.EXPAND)
        self.sz.Add(self.send_goal, 0, wx.EXPAND)
        self.sz.Add(self.cancel_goal, 0, wx.EXPAND)
        self.sz.Add(self.status_bg, 0, wx.EXPAND)
        self.sz.Add(self.server_status_bg, 0, wx.EXPAND)

        self.frame.SetSizer(self.sz)

        self.server_check_timer = wx.Timer(self.frame)
        self.frame.Bind(wx.EVT_TIMER, self.server_check,
                        self.server_check_timer)
        self.server_check_timer.Start(1000)

        self.sz.Layout()
        self.frame.Show()

        return True


def main():
    rospy.init_node('axclient', anonymous=True)

    parser = OptionParser(__doc__.strip())
#    parser.add_option("-t","--test",action="store_true", dest="test",default=False,
#                      help="A testing flag")
#  parser.add_option("-v","--var",action="store",type="string", dest="var",default="blah")

    (options, args) = parser.parse_args(rospy.myargv())

    if (len(args) == 2):
        # get action type via rostopic
        topic_type = rostopic._get_topic_type("%s/goal" % args[1])[0]
        # remove "Goal" string from action type
        assert("Goal" in topic_type)
        topic_type = topic_type[0:len(topic_type)-4]
    elif (len(args) == 3):
        topic_type = args[2]
        print(topic_type)
        assert("Action" in topic_type)
    else:
        parser.error("You must specify the action topic name (and optionally type) Eg: ./axclient.py action_topic actionlib/TwoIntsAction ")

    action = DynamicAction(topic_type)
    app = AXClientApp(action, args[1])
    app.MainLoop()
    app.OnQuit()
    rospy.signal_shutdown('GUI shutdown')


if __name__ == '__main__':
    main()