action_server.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: Alexander Sorokin.
# Based on C++ action_server.h by Eitan Marder-Eppstein
import rospy
import threading

from actionlib_msgs.msg import GoalID, GoalStatus, GoalStatusArray

from actionlib.goal_id_generator import GoalIDGenerator
from actionlib.status_tracker import StatusTracker

from actionlib.handle_tracker_deleter import HandleTrackerDeleter
from actionlib.server_goal_handle import ServerGoalHandle

from actionlib.exceptions import ActionException


def nop_cb(goal_handle):
    pass


def ros_timer(callable, frequency):
    rate = rospy.Rate(frequency)

    rospy.logdebug("Starting timer")
    while not rospy.is_shutdown():
        try:
            rate.sleep()
            callable()
        except rospy.exceptions.ROSInterruptException:
            rospy.logdebug("Sleep interrupted")


## @class ActionServer
## @brief The ActionServer is a helpful tool for managing goal requests to a
## node. It allows the user to specify callbacks that are invoked when goal
## or cancel requests come over the wire, and passes back GoalHandles that
## can be used to track the state of a given goal request. The ActionServer
## makes no assumptions about the policy used to service these goals, and
## sends status for each goal over the wire until the last GoalHandle
## associated with a goal request is destroyed.
class ActionServer:
    ## @brief  Constructor for an ActionServer
    ## @param  ns/name A namespace for the action server
    ## @param  actionspec An explicit specification of the action
    ## @param  goal_cb A goal callback to be called when the ActionServer receives a new goal over the wire
    ## @param  cancel_cb A cancel callback to be called when the ActionServer receives a new cancel request over the wire
    ## @param  auto_start A boolean value that tells the ActionServer wheteher or not to start publishing as soon as it comes up. THIS SHOULD ALWAYS BE SET TO FALSE TO AVOID RACE CONDITIONS and start() should be called after construction of the server.
    def __init__(self, ns,  ActionSpec, goal_cb, cancel_cb=nop_cb, auto_start=True):
        self.ns = ns

        try:
            a = ActionSpec()

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

        self.goal_sub = None
        self.cancel_sub = None
        self.status_pub = None
        self.result_pub = None
        self.feedback_pub = None

        self.lock = threading.RLock()

        self.status_timer = None

        self.status_list = []

        self.last_cancel = rospy.Time()
        self.status_list_timeout = rospy.Duration()

        self.id_generator = GoalIDGenerator()

        self.goal_callback = goal_cb
        assert(self.goal_callback)

        self.cancel_callback = cancel_cb
        self.auto_start = auto_start

        self.started = False

        if self.auto_start:
            rospy.logwarn("You've passed in true for auto_start to the python action server, you should always pass "
                          "in false to avoid race conditions.")
            self.start()

    ## @brief  Register a callback to be invoked when a new goal is received, this will replace any  previously registered callback
    ## @param  cb The callback to invoke
    def register_goal_callback(self, cb):
        self.goal_callback = cb

    ## @brief  Register a callback to be invoked when a new cancel is received, this will replace any  previously registered callback
    ## @param  cb The callback to invoke
    def register_cancel_callback(self, cancel_cb):
        self.cancel_callback = cancel_cb

    ## @brief  Start the action server
    def start(self):
        with self.lock:
            self.initialize()
            self.started = True
            self.publish_status()

    ## @brief  Initialize all ROS connections and setup timers
    def initialize(self):
        self.pub_queue_size = rospy.get_param('actionlib_server_pub_queue_size', 50)
        if self.pub_queue_size < 0:
            self.pub_queue_size = 50
        self.status_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/status", GoalStatusArray, latch=True, queue_size=self.pub_queue_size)
        self.result_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/result", self.ActionResult, queue_size=self.pub_queue_size)
        self.feedback_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/feedback", self.ActionFeedback, queue_size=self.pub_queue_size)

        self.sub_queue_size = rospy.get_param('actionlib_server_sub_queue_size', -1)
        if self.sub_queue_size < 0:
            self.sub_queue_size = None
        self.goal_sub = rospy.Subscriber(rospy.remap_name(self.ns)+"/goal", self.ActionGoal, callback=self.internal_goal_callback, queue_size=self.sub_queue_size)

        self.cancel_sub = rospy.Subscriber(rospy.remap_name(self.ns)+"/cancel", GoalID, callback=self.internal_cancel_callback, queue_size=self.sub_queue_size)

        # read the frequency with which to publish status from the parameter server
        # if not specified locally explicitly, use search param to find actionlib_status_frequency
        resolved_status_frequency_name = rospy.remap_name(self.ns)+"/status_frequency"
        if rospy.has_param(resolved_status_frequency_name):
            self.status_frequency = rospy.get_param(resolved_status_frequency_name, 5.0)
            rospy.logwarn("You're using the deprecated status_frequency parameter, please switch to actionlib_status_frequency.")
        else:
            search_status_frequency_name = rospy.search_param("actionlib_status_frequency")
            if search_status_frequency_name is None:
                self.status_frequency = 5.0
            else:
                self.status_frequency = rospy.get_param(search_status_frequency_name, 5.0)

        status_list_timeout = rospy.get_param(rospy.remap_name(self.ns)+"/status_list_timeout", 5.0)
        self.status_list_timeout = rospy.Duration(status_list_timeout)

        self.status_timer = threading.Thread(None, ros_timer, None, (self.publish_status_async, self.status_frequency))
        self.status_timer.start()

    ## @brief  Publishes a result for a given goal
    ## @param status The status of the goal with which the result is associated
    ## @param result The result to publish
    def publish_result(self, status, result):
        with self.lock:
            ar = self.ActionResult()
            ar.header.stamp = rospy.Time.now()
            ar.status = status
            ar.result = result
            if not rospy.is_shutdown():
                self.result_pub.publish(ar)
            self.publish_status()

    ## @brief  Publishes feedback for a given goal
    ## @param status The status of the goal with which the feedback is associated
    ## @param feedback The feedback to publish
    def publish_feedback(self, status, feedback):
        with self.lock:
            af = self.ActionFeedback()
            af.header.stamp = rospy.Time.now()
            af.status = status
            af.feedback = feedback
            if not rospy.is_shutdown():
                self.feedback_pub.publish(af)

    ## @brief  The ROS callback for cancel requests coming into the ActionServer
    def internal_cancel_callback(self, goal_id):
        with self.lock:

            # if we're not started... then we're not actually going to do anything
            if not self.started:
                return

            # we need to handle a cancel for the user
            rospy.logdebug("The action server has received a new cancel request")

            goal_id_found = False
            for st in self.status_list[:]:
                # check if the goal id is zero or if it is equal to the goal id of
                # the iterator or if the time of the iterator warrants a cancel

                cancel_everything = (goal_id.id == "" and goal_id.stamp == rospy.Time())   # rospy::Time()) #id and stamp 0 --> cancel everything
                cancel_this_one = (goal_id.id == st.status.goal_id.id)   # ids match... cancel that goal
                cancel_before_stamp = (goal_id.stamp != rospy.Time() and st.status.goal_id.stamp <= goal_id.stamp)  # //stamp != 0 --> cancel everything before stamp

                if cancel_everything or cancel_this_one or cancel_before_stamp:
                    # we need to check if we need to store this cancel request for later
                    if goal_id.id == st.status.goal_id.id:
                        goal_id_found = True

                    # attempt to get the handle_tracker for the list item if it exists
                    handle_tracker = st.handle_tracker

                    if handle_tracker is None:
                        # if the handle tracker is expired, then we need to create a new one
                        handle_tracker = HandleTrackerDeleter(self, st)
                        st.handle_tracker = handle_tracker

                        # we also need to reset the time that the status is supposed to be removed from the list
                        st.handle_destruction_time = rospy.Time.now()

                    # set the status of the goal to PREEMPTING or RECALLING as approriate
                    # and check if the request should be passed on to the user
                    gh = ServerGoalHandle(st, self, handle_tracker)
                    if gh.set_cancel_requested():
                        # call the user's cancel callback on the relevant goal
                        self.cancel_callback(gh)

            # if the requested goal_id was not found, and it is non-zero, then we need to store the cancel request
            if goal_id.id != "" and not goal_id_found:
                tracker = StatusTracker(goal_id, GoalStatus.RECALLING)
                self.status_list.append(tracker)
                # start the timer for how long the status will live in the list without a goal handle to it
                tracker.handle_destruction_time = rospy.Time.now()

            # make sure to set last_cancel_ based on the stamp associated with this cancel request
            if goal_id.stamp > self.last_cancel:
                self.last_cancel = goal_id.stamp

    ## @brief  The ROS callback for goals coming into the ActionServer
    def internal_goal_callback(self, goal):
        with self.lock:
            # if we're not started... then we're not actually going to do anything
            if not self.started:
                return

            rospy.logdebug("The action server has received a new goal request")

            # we need to check if this goal already lives in the status list
            for st in self.status_list[:]:
                if goal.goal_id.id == st.status.goal_id.id:
                    rospy.logdebug("Goal %s was already in the status list with status %i" % (goal.goal_id.id, st.status.status))
                    # Goal could already be in recalling state if a cancel came in before the goal
                    if st.status.status == GoalStatus.RECALLING:
                        st.status.status = GoalStatus.RECALLED
                        self.publish_result(st.status, self.ActionResultType())

                    # if this is a request for a goal that has no active handles left,
                    # we'll bump how long it stays in the list
                    if st.handle_tracker is None:
                        st.handle_destruction_time = rospy.Time.now()

                    # make sure not to call any user callbacks or add duplicate status onto the list
                    return

            # if the goal is not in our list, we need to create a StatusTracker associated with this goal and push it on
            st = StatusTracker(None, None, goal)
            self.status_list.append(st)

            # we need to create a handle tracker for the incoming goal and update the StatusTracker
            handle_tracker = HandleTrackerDeleter(self, st)

            st.handle_tracker = handle_tracker

            # check if this goal has already been canceled based on its timestamp
            gh = ServerGoalHandle(st, self, handle_tracker)
            if goal.goal_id.stamp != rospy.Time() and goal.goal_id.stamp <= self.last_cancel:
                # if it has... just create a GoalHandle for it and setCanceled
                gh.set_canceled(None, "This goal handle was canceled by the action server because its timestamp is before the timestamp of the last cancel request")
            else:
                # now, we need to create a goal handle and call the user's callback
                self.goal_callback(gh)

    ## @brief  Publish status for all goals on a timer event
    def publish_status_async(self):
        with self.lock:
            # we won't publish status unless we've been started
            if not self.started:
                return
            self.publish_status()

    ## @brief  Explicitly publish status
    def publish_status(self):
        with self.lock:
            # build a status array
            status_array = GoalStatusArray()

            # status_array.set_status_list_size(len(self.status_list));

            i = 0
            while i < len(self.status_list):
                st = self.status_list[i]
                # check if the item is due for deletion from the status list
                if st.handle_destruction_time != rospy.Time() and st.handle_destruction_time + self.status_list_timeout < rospy.Time.now():
                    rospy.logdebug("Item %s with destruction time of %.3f being removed from list.  Now = %.3f" %
                                   (st.status.goal_id, st.handle_destruction_time.to_sec(), rospy.Time.now().to_sec()))
                    del self.status_list[i]
                else:
                    status_array.status_list.append(st.status)
                    i += 1

            status_array.header.stamp = rospy.Time.now()
            if not rospy.is_shutdown():
                self.status_pub.publish(status_array)