action_client.py

Go to the documentation of this file.

#! /usr/bin/env python
# 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 the 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: Stuart Glaser
'''
Example:

from move_base.msg import *
rospy.init_node('foo')


from move_base.msg import *
from geometry_msgs.msg import *
g1 = MoveBaseGoal(PoseStamped(Header(frame_id = 'base_link'),
                              Pose(Point(2, 0, 0),
                                   Quaternion(0, 0, 0, 1))))
g2 = MoveBaseGoal(PoseStamped(Header(frame_id = 'base_link'),
                              Pose(Point(5, 0, 0),
                                   Quaternion(0, 0, 0, 1))))

client = ActionClient('move_base', MoveBaseAction)

h1 = client.send_goal(g1)
h2 = client.send_goal(g2)
client.cancel_all_goals()
'''

import threading
import weakref
import time
import rospy
from rospy import Header
from actionlib_msgs.msg import GoalID, GoalStatus, GoalStatusArray
from actionlib.exceptions import ActionException

g_goal_id = 1


def get_name_of_constant(C, n):
    for k, v in C.__dict__.items():
        if isinstance(v, int) and v == n:
            return k
    return "NO_SUCH_STATE_%d" % n


class CommState(object):
    WAITING_FOR_GOAL_ACK = 0
    PENDING = 1
    ACTIVE = 2
    WAITING_FOR_RESULT = 3
    WAITING_FOR_CANCEL_ACK = 4
    RECALLING = 5
    PREEMPTING = 6
    DONE = 7
    LOST = 8


class TerminalState(object):
    RECALLED = GoalStatus.RECALLED
    REJECTED = GoalStatus.REJECTED
    PREEMPTED = GoalStatus.PREEMPTED
    ABORTED = GoalStatus.ABORTED
    SUCCEEDED = GoalStatus.SUCCEEDED
    LOST = GoalStatus.LOST


GoalStatus.to_string = classmethod(get_name_of_constant)
CommState.to_string = classmethod(get_name_of_constant)
TerminalState.to_string = classmethod(get_name_of_constant)


def _find_status_by_goal_id(status_array, id):
    for s in status_array.status_list:
        if s.goal_id.id == id:
            return s
    return None



class ClientGoalHandle:
    
    def __init__(self, comm_state_machine):
        self.comm_state_machine = comm_state_machine

        # print "GH created.  id = %.3f" % self.comm_state_machine.action_goal.goal_id.stamp.to_sec()

    
    def __eq__(self, o):
        if not o:
            return False
        return self.comm_state_machine == o.comm_state_machine

    
    def __ne__(self, o):
        if not o:
            return True
        return not (self.comm_state_machine == o.comm_state_machine)

    
    def __hash__(self):
        return hash(self.comm_state_machine)

    
    def cancel(self):
        with self.comm_state_machine.mutex:
            cancel_msg = GoalID(stamp=rospy.Time(0),
                                id=self.comm_state_machine.action_goal.goal_id.id)
            self.comm_state_machine.send_cancel_fn(cancel_msg)
            self.comm_state_machine.transition_to(CommState.WAITING_FOR_CANCEL_ACK)

    
    def get_comm_state(self):
        if not self.comm_state_machine:
            rospy.logerr("Trying to get_comm_state on an inactive ClientGoalHandle.")
            return CommState.LOST
        return self.comm_state_machine.state

    
    def get_goal_status(self):
        if not self.comm_state_machine:
            rospy.logerr("Trying to get_goal_status on an inactive ClientGoalHandle.")
            return GoalStatus.PENDING
        return self.comm_state_machine.latest_goal_status.status

    
    def get_goal_status_text(self):
        if not self.comm_state_machine:
            rospy.logerr("Trying to get_goal_status_text on an inactive ClientGoalHandle.")
            return "ERROR: Trying to get_goal_status_text on an inactive ClientGoalHandle."
        return self.comm_state_machine.latest_goal_status.text

    
    def get_result(self):
        if not self.comm_state_machine:
            rospy.logerr("Trying to get_result on an inactive ClientGoalHandle.")
            return None
        if not self.comm_state_machine.latest_result:
            # rospy.logerr("Trying to get_result on a ClientGoalHandle when no result has been received.")
            return None
        return self.comm_state_machine.latest_result.result

    
    def get_terminal_state(self):
        if not self.comm_state_machine:
            rospy.logerr("Trying to get_terminal_state on an inactive ClientGoalHandle.")
            return GoalStatus.LOST

        with self.comm_state_machine.mutex:
            if self.comm_state_machine.state != CommState.DONE:
                rospy.logwarn("Asking for the terminal state when we're in [%s]",
                              CommState.to_string(self.comm_state_machine.state))

            goal_status = self.comm_state_machine.latest_goal_status.status
            if goal_status in [GoalStatus.PREEMPTED, GoalStatus.SUCCEEDED,
                               GoalStatus.ABORTED, GoalStatus.REJECTED,
                               GoalStatus.RECALLED, GoalStatus.LOST]:
                return goal_status

            rospy.logerr("Asking for a terminal state, but the goal status is %d", goal_status)
            return GoalStatus.LOST


NO_TRANSITION = -1
INVALID_TRANSITION = -2
_transitions = {
    CommState.WAITING_FOR_GOAL_ACK: {
        GoalStatus.PENDING:    CommState.PENDING,
        GoalStatus.ACTIVE:     CommState.ACTIVE,
        GoalStatus.REJECTED:   (CommState.PENDING, CommState.WAITING_FOR_RESULT),
        GoalStatus.RECALLING:  (CommState.PENDING, CommState.RECALLING),
        GoalStatus.RECALLED:   (CommState.PENDING, CommState.WAITING_FOR_RESULT),
        GoalStatus.PREEMPTED:  (CommState.ACTIVE, CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
        GoalStatus.SUCCEEDED:  (CommState.ACTIVE, CommState.WAITING_FOR_RESULT),
        GoalStatus.ABORTED:    (CommState.ACTIVE, CommState.WAITING_FOR_RESULT),
        GoalStatus.PREEMPTING: (CommState.ACTIVE, CommState.PREEMPTING)},
    CommState.PENDING: {
        GoalStatus.PENDING:    NO_TRANSITION,
        GoalStatus.ACTIVE:     CommState.ACTIVE,
        GoalStatus.REJECTED:   CommState.WAITING_FOR_RESULT,
        GoalStatus.RECALLING:  CommState.RECALLING,
        GoalStatus.RECALLED:   (CommState.RECALLING, CommState.WAITING_FOR_RESULT),
        GoalStatus.PREEMPTED:  (CommState.ACTIVE, CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
        GoalStatus.SUCCEEDED:  (CommState.ACTIVE, CommState.WAITING_FOR_RESULT),
        GoalStatus.ABORTED:    (CommState.ACTIVE, CommState.WAITING_FOR_RESULT),
        GoalStatus.PREEMPTING: (CommState.ACTIVE, CommState.PREEMPTING)},
    CommState.ACTIVE: {
        GoalStatus.PENDING:    INVALID_TRANSITION,
        GoalStatus.ACTIVE:     NO_TRANSITION,
        GoalStatus.REJECTED:   INVALID_TRANSITION,
        GoalStatus.RECALLING:  INVALID_TRANSITION,
        GoalStatus.RECALLED:   INVALID_TRANSITION,
        GoalStatus.PREEMPTED:  (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
        GoalStatus.SUCCEEDED:  CommState.WAITING_FOR_RESULT,
        GoalStatus.ABORTED:    CommState.WAITING_FOR_RESULT,
        GoalStatus.PREEMPTING: CommState.PREEMPTING},
    CommState.WAITING_FOR_RESULT: {
        GoalStatus.PENDING:    INVALID_TRANSITION,
        GoalStatus.ACTIVE:     NO_TRANSITION,
        GoalStatus.REJECTED:   NO_TRANSITION,
        GoalStatus.RECALLING:  INVALID_TRANSITION,
        GoalStatus.RECALLED:   NO_TRANSITION,
        GoalStatus.PREEMPTED:  NO_TRANSITION,
        GoalStatus.SUCCEEDED:  NO_TRANSITION,
        GoalStatus.ABORTED:    NO_TRANSITION,
        GoalStatus.PREEMPTING: INVALID_TRANSITION},
    CommState.WAITING_FOR_CANCEL_ACK: {
        GoalStatus.PENDING:    NO_TRANSITION,
        GoalStatus.ACTIVE:     NO_TRANSITION,
        GoalStatus.REJECTED:   CommState.WAITING_FOR_RESULT,
        GoalStatus.RECALLING:  CommState.RECALLING,
        GoalStatus.RECALLED:   (CommState.RECALLING, CommState.WAITING_FOR_RESULT),
        GoalStatus.PREEMPTED:  (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
        GoalStatus.SUCCEEDED:  (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
        GoalStatus.ABORTED:    (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
        GoalStatus.PREEMPTING: CommState.PREEMPTING},
    CommState.RECALLING: {
        GoalStatus.PENDING:    INVALID_TRANSITION,
        GoalStatus.ACTIVE:     INVALID_TRANSITION,
        GoalStatus.REJECTED:   CommState.WAITING_FOR_RESULT,
        GoalStatus.RECALLING:  NO_TRANSITION,
        GoalStatus.RECALLED:   CommState.WAITING_FOR_RESULT,
        GoalStatus.PREEMPTED:  (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
        GoalStatus.SUCCEEDED:  (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
        GoalStatus.ABORTED:    (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
        GoalStatus.PREEMPTING: CommState.PREEMPTING},
    CommState.PREEMPTING: {
        GoalStatus.PENDING:    INVALID_TRANSITION,
        GoalStatus.ACTIVE:     INVALID_TRANSITION,
        GoalStatus.REJECTED:   INVALID_TRANSITION,
        GoalStatus.RECALLING:  INVALID_TRANSITION,
        GoalStatus.RECALLED:   INVALID_TRANSITION,
        GoalStatus.PREEMPTED:  CommState.WAITING_FOR_RESULT,
        GoalStatus.SUCCEEDED:  CommState.WAITING_FOR_RESULT,
        GoalStatus.ABORTED:    CommState.WAITING_FOR_RESULT,
        GoalStatus.PREEMPTING: NO_TRANSITION},
    CommState.DONE: {
        GoalStatus.PENDING:    INVALID_TRANSITION,
        GoalStatus.ACTIVE:     INVALID_TRANSITION,
        GoalStatus.REJECTED:   NO_TRANSITION,
        GoalStatus.RECALLING:  INVALID_TRANSITION,
        GoalStatus.RECALLED:   NO_TRANSITION,
        GoalStatus.PREEMPTED:  NO_TRANSITION,
        GoalStatus.SUCCEEDED:  NO_TRANSITION,
        GoalStatus.ABORTED:    NO_TRANSITION,
        GoalStatus.PREEMPTING: INVALID_TRANSITION}}


class CommStateMachine:
    def __init__(self, action_goal, transition_cb, feedback_cb, send_goal_fn, send_cancel_fn):
        self.action_goal = action_goal
        self.transition_cb = transition_cb
        self.feedback_cb = feedback_cb
        self.send_goal_fn = send_goal_fn
        self.send_cancel_fn = send_cancel_fn

        self.state = CommState.WAITING_FOR_GOAL_ACK
        self.mutex = threading.RLock()
        self.latest_goal_status = GoalStatus(status=GoalStatus.PENDING)
        self.latest_result = None

    def __eq__(self, o):
        return self.action_goal.goal_id.id == o.action_goal.goal_id.id

    
    def __hash__(self):
        return hash(self.action_goal.goal_id.id)

    def set_state(self, state):
        rospy.logdebug("Transitioning CommState from %s to %s",
                       CommState.to_string(self.state), CommState.to_string(state))
        self.state = state

    
    def update_status(self, status_array):
        with self.mutex:
            if self.state == CommState.DONE:
                return

            status = _find_status_by_goal_id(status_array, self.action_goal.goal_id.id)

            # You mean you haven't heard of me?
            if not status:
                if self.state not in [CommState.WAITING_FOR_GOAL_ACK,
                                      CommState.WAITING_FOR_RESULT,
                                      CommState.DONE]:
                    self._mark_as_lost()
                return

            self.latest_goal_status = status

            # Determines the next state from the lookup table
            if self.state not in _transitions:
                rospy.logerr("CommStateMachine is in a funny state: %i" % self.state)
                return
            if status.status not in _transitions[self.state]:
                rospy.logerr("Got an unknown status from the ActionServer: %i" % status.status)
                return
            next_state = _transitions[self.state][status.status]

            # Knowing the next state, what should we do?
            if next_state == NO_TRANSITION:
                pass
            elif next_state == INVALID_TRANSITION:
                rospy.logerr("Invalid goal status transition from %s to %s" %
                             (CommState.to_string(self.state), GoalStatus.to_string(status.status)))
            else:
                if hasattr(next_state, '__getitem__'):
                    for s in next_state:
                        self.transition_to(s)
                else:
                    self.transition_to(next_state)

    def transition_to(self, state):
        rospy.logdebug("Transitioning to %s (from %s, goal: %s)",
                       CommState.to_string(state), CommState.to_string(self.state),
                       self.action_goal.goal_id.id)
        self.state = state
        if self.transition_cb:
            self.transition_cb(ClientGoalHandle(self))

    def _mark_as_lost(self):
        self.latest_goal_status.status = GoalStatus.LOST
        self.transition_to(CommState.DONE)

    def update_result(self, action_result):
        # Might not be for us
        if self.action_goal.goal_id.id != action_result.status.goal_id.id:
            return

        with self.mutex:
            self.latest_goal_status = action_result.status
            self.latest_result = action_result

            if self.state in [CommState.WAITING_FOR_GOAL_ACK,
                              CommState.WAITING_FOR_CANCEL_ACK,
                              CommState.PENDING,
                              CommState.ACTIVE,
                              CommState.WAITING_FOR_RESULT,
                              CommState.RECALLING,
                              CommState.PREEMPTING]:
                # Stuffs the goal status in the result into a GoalStatusArray
                status_array = GoalStatusArray()
                status_array.status_list.append(action_result.status)
                self.update_status(status_array)

                self.transition_to(CommState.DONE)
            elif self.state == CommState.DONE:
                rospy.logerr("Got a result when we were already in the DONE state")
            else:
                rospy.logerr("In a funny state: %i" % self.state)

    def update_feedback(self, action_feedback):
        # Might not be for us
        if self.action_goal.goal_id.id != action_feedback.status.goal_id.id:
            return

        # with self.mutex:
        if self.feedback_cb and self.state != CommState.DONE:
            self.feedback_cb(ClientGoalHandle(self), action_feedback.feedback)


class GoalManager:

    # statuses - a list of weak references to CommStateMachine objects

    def __init__(self, ActionSpec):
        self.list_mutex = threading.RLock()
        self.statuses = []
        self.send_goal_fn = None

        try:
            a = ActionSpec()

            self.ActionSpec = ActionSpec
            self.ActionGoal = type(a.action_goal)
            self.ActionResult = type(a.action_result)
            self.ActionFeedback = type(a.action_feedback)
        except AttributeError:
            raise ActionException("Type is not an action spec: %s" % str(ActionSpec))

    def _generate_id(self):
        global g_goal_id
        id, g_goal_id = g_goal_id, g_goal_id + 1
        now = rospy.Time.now()
        return GoalID(id="%s-%i-%.3f" % (rospy.get_caller_id(), id, now.to_sec()), stamp=now)

    def register_send_goal_fn(self, fn):
        self.send_goal_fn = fn

    def register_cancel_fn(self, fn):
        self.cancel_fn = fn

    
    def init_goal(self, goal, transition_cb=None, feedback_cb=None):
        action_goal = self.ActionGoal(header=Header(),
                                      goal_id=self._generate_id(),
                                      goal=goal)
        action_goal.header.stamp = rospy.get_rostime()

        csm = CommStateMachine(action_goal, transition_cb, feedback_cb,
                               self.send_goal_fn, self.cancel_fn)

        with self.list_mutex:
            self.statuses.append(weakref.ref(csm))

        self.send_goal_fn(action_goal)

        return ClientGoalHandle(csm)

    # Pulls out the statuses that are still live (creating strong
    # references to them)
    def _get_live_statuses(self):
        with self.list_mutex:
            live_statuses = [r() for r in self.statuses]
            live_statuses = filter(lambda x: x, live_statuses)
            return live_statuses

    
    def update_statuses(self, status_array):

        with self.list_mutex:
            # Garbage collects dead status objects
            self.statuses = [r for r in self.statuses if r()]

        for status in self._get_live_statuses():
            status.update_status(status_array)

    def update_results(self, action_result):
        for status in self._get_live_statuses():
            status.update_result(action_result)

    def update_feedbacks(self, action_feedback):
        for status in self._get_live_statuses():
            status.update_feedback(action_feedback)


class ActionClient:
    
    def __init__(self, ns, ActionSpec):
        self.ns = ns
        self.last_status_msg = None

        try:
            a = ActionSpec()

            self.ActionSpec = ActionSpec
            self.ActionGoal = type(a.action_goal)
            self.ActionResult = type(a.action_result)
            self.ActionFeedback = type(a.action_feedback)
        except AttributeError:
            raise ActionException("Type is not an action spec: %s" % str(ActionSpec))

        self.pub_queue_size = rospy.get_param('actionlib_client_pub_queue_size', 10)
        if self.pub_queue_size < 0:
            self.pub_queue_size = 10
        self.pub_goal = rospy.Publisher(rospy.remap_name(ns) + '/goal', self.ActionGoal, queue_size=self.pub_queue_size)
        self.pub_cancel = rospy.Publisher(rospy.remap_name(ns) + '/cancel', GoalID, queue_size=self.pub_queue_size)

        self.manager = GoalManager(ActionSpec)
        self.manager.register_send_goal_fn(self.pub_goal.publish)
        self.manager.register_cancel_fn(self.pub_cancel.publish)

        self.sub_queue_size = rospy.get_param('actionlib_client_sub_queue_size', -1)
        if self.sub_queue_size < 0:
            self.sub_queue_size = None
        self.status_sub = rospy.Subscriber(rospy.remap_name(ns) + '/status', GoalStatusArray, callback=self._status_cb, queue_size=self.sub_queue_size)
        self.result_sub = rospy.Subscriber(rospy.remap_name(ns) + '/result', self.ActionResult, callback=self._result_cb, queue_size=self.sub_queue_size)
        self.feedback_sub = rospy.Subscriber(rospy.remap_name(ns) + '/feedback', self.ActionFeedback, callback=self._feedback_cb, queue_size=self.sub_queue_size)

    
    def send_goal(self, goal, transition_cb=None, feedback_cb=None):
        return self.manager.init_goal(goal, transition_cb, feedback_cb)

    
    def cancel_all_goals(self):
        cancel_msg = GoalID(stamp=rospy.Time.from_sec(0.0), id="")
        self.pub_cancel.publish(cancel_msg)

    

    def cancel_goals_at_and_before_time(self, time):
        cancel_msg = GoalID(stamp=time, id="")
        self.pub_cancel.publish(cancel_msg)

    
    def wait_for_action_server_to_start(self, timeout=rospy.Duration(0.0)):
        return self.wait_for_server(timeout)

    
    def wait_for_server(self, timeout=rospy.Duration(0.0)):
        started = False
        timeout_time = rospy.get_rostime() + timeout
        while not rospy.is_shutdown():
            if self.last_status_msg:
                server_id = self.last_status_msg._connection_header['callerid']

                if self.pub_goal.impl.has_connection(server_id) and \
                        self.pub_cancel.impl.has_connection(server_id):
                    # We'll also check that all of the subscribers have at least
                    # one publisher, this isn't a perfect check, but without
                    # publisher callbacks... it'll have to do
                    status_num_pubs = 0
                    for stat in self.status_sub.impl.get_stats()[1]:
                        if stat[4]:
                            status_num_pubs += 1

                    result_num_pubs = 0
                    for stat in self.result_sub.impl.get_stats()[1]:
                        if stat[4]:
                            result_num_pubs += 1

                    feedback_num_pubs = 0
                    for stat in self.feedback_sub.impl.get_stats()[1]:
                        if stat[4]:
                            feedback_num_pubs += 1

                    if status_num_pubs > 0 and result_num_pubs > 0 and feedback_num_pubs > 0:
                        started = True
                        break

            if timeout != rospy.Duration(0.0) and rospy.get_rostime() >= timeout_time:
                break

            time.sleep(0.01)

        return started

    def _status_cb(self, msg):
        self.last_status_msg = msg
        self.manager.update_statuses(msg)

    def _result_cb(self, msg):
        self.manager.update_results(msg)

    def _feedback_cb(self, msg):
        self.manager.update_feedbacks(msg)