action_server_base.h

Go to the documentation of this file.

/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2008, 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
*********************************************************************/
#ifndef ACTIONLIB__SERVER__ACTION_SERVER_BASE_H_
#define ACTIONLIB__SERVER__ACTION_SERVER_BASE_H_

#include <ros/ros.h>
#include <boost/thread.hpp>
#include <boost/shared_ptr.hpp>
#include <actionlib_msgs/GoalID.h>
#include <actionlib_msgs/GoalStatusArray.h>
#include <actionlib_msgs/GoalStatus.h>
#include <actionlib/enclosure_deleter.h>
#include <actionlib/goal_id_generator.h>
#include <actionlib/action_definition.h>
#include <actionlib/server/status_tracker.h>
#include <actionlib/server/handle_tracker_deleter.h>
#include <actionlib/server/server_goal_handle.h>
#include <actionlib/destruction_guard.h>

#include <list>

namespace actionlib
{
template<class ActionSpec>
class ActionServerBase
{
public:
  // for convenience when referring to ServerGoalHandles
  typedef ServerGoalHandle<ActionSpec> GoalHandle;

  // generates typedefs that we'll use to make our lives easier
  ACTION_DEFINITION(ActionSpec);

  ActionServerBase(
    boost::function<void(GoalHandle)> goal_cb,
    boost::function<void(GoalHandle)> cancel_cb,
    bool auto_start = false);


  virtual ~ActionServerBase();

  void registerGoalCallback(boost::function<void(GoalHandle)> cb);

  void registerCancelCallback(boost::function<void(GoalHandle)> cb);

  void start();


  void goalCallback(const boost::shared_ptr<const ActionGoal> & goal);

  void cancelCallback(const boost::shared_ptr<const actionlib_msgs::GoalID> & goal_id);

protected:
  // Allow access to protected fields for helper classes
  friend class ServerGoalHandle<ActionSpec>;
  friend class HandleTrackerDeleter<ActionSpec>;

  virtual void initialize() = 0;

  virtual void publishResult(const actionlib_msgs::GoalStatus & status, const Result & result) = 0;

  virtual void publishFeedback(const actionlib_msgs::GoalStatus & status,
    const Feedback & feedback) = 0;

  virtual void publishStatus() = 0;

  boost::recursive_mutex lock_;

  std::list<StatusTracker<ActionSpec> > status_list_;

  boost::function<void(GoalHandle)> goal_callback_;
  boost::function<void(GoalHandle)> cancel_callback_;

  ros::Time last_cancel_;
  ros::Duration status_list_timeout_;

  GoalIDGenerator id_generator_;
  bool started_;
  boost::shared_ptr<DestructionGuard> guard_;
};

template<class ActionSpec>
ActionServerBase<ActionSpec>::ActionServerBase(
  boost::function<void(GoalHandle)> goal_cb,
  boost::function<void(GoalHandle)> cancel_cb,
  bool auto_start)
: goal_callback_(goal_cb),
  cancel_callback_(cancel_cb),
  started_(auto_start),
  guard_(new DestructionGuard)
{
}

template<class ActionSpec>
ActionServerBase<ActionSpec>::~ActionServerBase()
{
  // Block until we can safely destruct
  guard_->destruct();
}

template<class ActionSpec>
void ActionServerBase<ActionSpec>::registerGoalCallback(boost::function<void(GoalHandle)> cb)
{
  goal_callback_ = cb;
}

template<class ActionSpec>
void ActionServerBase<ActionSpec>::registerCancelCallback(boost::function<void(GoalHandle)> cb)
{
  cancel_callback_ = cb;
}

template<class ActionSpec>
void ActionServerBase<ActionSpec>::start()
{
  initialize();
  started_ = true;
  publishStatus();
}


template<class ActionSpec>
void ActionServerBase<ActionSpec>::goalCallback(const boost::shared_ptr<const ActionGoal> & goal)
{
  boost::recursive_mutex::scoped_lock lock(lock_);

  // if we're not started... then we're not actually going to do anything
  if (!started_) {
    return;
  }

  ROS_DEBUG_NAMED("actionlib", "The action server has received a new goal request");

  // we need to check if this goal already lives in the status list
  for (typename std::list<StatusTracker<ActionSpec> >::iterator it = status_list_.begin();
    it != status_list_.end(); ++it)
  {
    if (goal->goal_id.id == (*it).status_.goal_id.id) {
      // The goal could already be in a recalling state if a cancel came in before the goal
      if ( (*it).status_.status == actionlib_msgs::GoalStatus::RECALLING) {
        (*it).status_.status = actionlib_msgs::GoalStatus::RECALLED;
        publishResult((*it).status_, Result());
      }

      // 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 ((*it).handle_tracker_.expired()) {
        (*it).handle_destruction_time_ = goal->goal_id.stamp;
      }

      // 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
  typename std::list<StatusTracker<ActionSpec> >::iterator it = status_list_.insert(
    status_list_.end(), StatusTracker<ActionSpec>(goal));

  // we need to create a handle tracker for the incoming goal and update the StatusTracker
  HandleTrackerDeleter<ActionSpec> d(this, it, guard_);
  boost::shared_ptr<void> handle_tracker((void *)NULL, d);
  (*it).handle_tracker_ = handle_tracker;

  // check if this goal has already been canceled based on its timestamp
  if (goal->goal_id.stamp != ros::Time() && goal->goal_id.stamp <= last_cancel_) {
    // if it has... just create a GoalHandle for it and setCanceled
    GoalHandle gh(it, this, handle_tracker, guard_);
    gh.setCanceled(
      Result(),
      "This goal handle was canceled by the action server because its timestamp is before the timestamp of the last cancel request");
  } else {
    GoalHandle gh = GoalHandle(it, this, handle_tracker, guard_);

    // make sure that we unlock before calling the users callback
    boost::reverse_lock<boost::recursive_mutex::scoped_lock> unlocker(lock);

    // now, we need to create a goal handle and call the user's callback
    goal_callback_(gh);
  }
}

template<class ActionSpec>
void ActionServerBase<ActionSpec>::cancelCallback(
  const boost::shared_ptr<const actionlib_msgs::GoalID> & goal_id)
{
  boost::recursive_mutex::scoped_lock lock(lock_);

  // if we're not started... then we're not actually going to do anything
  if (!started_) {
    return;
  }

  // we need to handle a cancel for the user
  ROS_DEBUG_NAMED("actionlib", "The action server has received a new cancel request");
  bool goal_id_found = false;
  for (typename std::list<StatusTracker<ActionSpec> >::iterator it = status_list_.begin();
    it != status_list_.end(); ++it)
  {
    // 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
    if (
      (goal_id->id == "" && goal_id->stamp == ros::Time()) ||  // id and stamp 0 --> cancel everything
      goal_id->id == (*it).status_.goal_id.id ||    // ids match... cancel that goal
      (goal_id->stamp != ros::Time() && (*it).status_.goal_id.stamp <= goal_id->stamp)       // stamp != 0 --> cancel everything before stamp
    )
    {
      // we need to check if we need to store this cancel request for later
      if (goal_id->id == (*it).status_.goal_id.id) {
        goal_id_found = true;
      }

      // attempt to get the handle_tracker for the list item if it exists
      boost::shared_ptr<void> handle_tracker = (*it).handle_tracker_.lock();

      if ((*it).handle_tracker_.expired()) {
        // if the handle tracker is expired, then we need to create a new one
        HandleTrackerDeleter<ActionSpec> d(this, it, guard_);
        handle_tracker = boost::shared_ptr<void>((void *)NULL, d);
        (*it).handle_tracker_ = handle_tracker;

        // we also need to reset the time that the status is supposed to be removed from the list
        (*it).handle_destruction_time_ = ros::Time();
      }

      // set the status of the goal to PREEMPTING or RECALLING as approriate
      // and check if the request should be passed on to the user
      GoalHandle gh(it, this, handle_tracker, guard_);
      if (gh.setCancelRequested()) {
        // make sure that we're unlocked before we call the users callback
        boost::reverse_lock<boost::recursive_mutex::scoped_lock> unlocker(lock);

        // call the user's cancel callback on the relevant goal
        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 != "" && !goal_id_found) {
    typename std::list<StatusTracker<ActionSpec> >::iterator it = status_list_.insert(
      status_list_.end(),
      StatusTracker<ActionSpec>(*goal_id, actionlib_msgs::GoalStatus::RECALLING));
    // start the timer for how long the status will live in the list without a goal handle to it
    (*it).handle_destruction_time_ = goal_id->stamp;
  }

  // make sure to set last_cancel_ based on the stamp associated with this cancel request
  if (goal_id->stamp > last_cancel_) {
    last_cancel_ = goal_id->stamp;
  }
}

}  // namespace actionlib
#endif  // ACTIONLIB__SERVER__ACTION_SERVER_BASE_H_