simple_action_client.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 the Willow Garage 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.
*********************************************************************/

#ifndef ACTIONLIB_SIMPLE_ACTION_CLIENT_H_
#define ACTIONLIB_SIMPLE_ACTION_CLIENT_H_

#include <boost/thread/condition.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/scoped_ptr.hpp>

#include "ros/ros.h"
#include "ros/callback_queue.h"
#include "actionlib/client/action_client.h"
#include "actionlib/client/simple_goal_state.h"
#include "actionlib/client/simple_client_goal_state.h"
#include "actionlib/client/terminal_state.h"


#if defined(__GNUC__)
#define DEPRECATED __attribute__((deprecated))
#else
#define DEPRECATED
#endif


namespace actionlib
{

template <class ActionSpec>
class SimpleActionClient
{
private:
  ACTION_DEFINITION(ActionSpec);
  typedef ClientGoalHandle<ActionSpec> GoalHandleT;
  typedef SimpleActionClient<ActionSpec> SimpleActionClientT;

public:
  typedef boost::function<void (const SimpleClientGoalState& state,  const ResultConstPtr& result) > SimpleDoneCallback;
  typedef boost::function<void () > SimpleActiveCallback;
  typedef boost::function<void (const FeedbackConstPtr& feedback) > SimpleFeedbackCallback;

  SimpleActionClient(const std::string& name, bool spin_thread = true) : cur_simple_state_(SimpleGoalState::PENDING)
  {
    initSimpleClient(nh_, name, spin_thread);
  }

  SimpleActionClient(ros::NodeHandle& n, const std::string& name, bool spin_thread = true) : cur_simple_state_(SimpleGoalState::PENDING)
  {
    initSimpleClient(n, name, spin_thread);

  }

  ~SimpleActionClient();

  bool waitForServer(const ros::Duration& timeout = ros::Duration(0,0) ) { return ac_->waitForActionServerToStart(timeout); }

  bool isServerConnected()
  {
    return ac_->isServerConnected();
  }

  void sendGoal(const Goal& goal,
                SimpleDoneCallback     done_cb     = SimpleDoneCallback(),
                SimpleActiveCallback   active_cb   = SimpleActiveCallback(),
                SimpleFeedbackCallback feedback_cb = SimpleFeedbackCallback());

  SimpleClientGoalState sendGoalAndWait(const Goal& goal,
                                        const ros::Duration& execute_timeout = ros::Duration(0,0),
                                        const ros::Duration& preempt_timeout = ros::Duration(0,0));

  bool waitForResult(const ros::Duration& timeout = ros::Duration(0,0) );

  ResultConstPtr getResult();

  SimpleClientGoalState getState();

  void cancelAllGoals();

  void cancelGoalsAtAndBeforeTime(const ros::Time& time);

  void cancelGoal();

  void stopTrackingGoal();

private:
  typedef ActionClient<ActionSpec> ActionClientT;
  ros::NodeHandle nh_;
  GoalHandleT gh_;

  SimpleGoalState cur_simple_state_;

  // Signalling Stuff
  boost::condition done_condition_;
  boost::mutex done_mutex_;

  // User Callbacks
  SimpleDoneCallback done_cb_;
  SimpleActiveCallback active_cb_;
  SimpleFeedbackCallback feedback_cb_;

  // Spin Thread Stuff
  boost::mutex terminate_mutex_;
  bool need_to_terminate_;
  boost::thread* spin_thread_;
  ros::CallbackQueue callback_queue;

  boost::scoped_ptr<ActionClientT> ac_;  // Action client depends on callback_queue, so it must be destroyed before callback_queue

  // ***** Private Funcs *****
  void initSimpleClient(ros::NodeHandle& n, const std::string& name, bool spin_thread);
  void handleTransition(GoalHandleT gh);
  void handleFeedback(GoalHandleT gh, const FeedbackConstPtr& feedback);
  void setSimpleState(const SimpleGoalState::StateEnum& next_state);
  void setSimpleState(const SimpleGoalState& next_state);
  void spinThread();
};



template<class ActionSpec>
void SimpleActionClient<ActionSpec>::initSimpleClient(ros::NodeHandle& n, const std::string& name, bool spin_thread)
{
  if (spin_thread)
  {
    ROS_DEBUG_NAMED("actionlib", "Spinning up a thread for the SimpleActionClient");
    need_to_terminate_ = false;
    spin_thread_ = new boost::thread(boost::bind(&SimpleActionClient<ActionSpec>::spinThread, this));
    ac_.reset(new ActionClientT(n, name, &callback_queue));
  }
  else
  {
    spin_thread_ = NULL;
    ac_.reset(new ActionClientT(n, name));
  }
}

template<class ActionSpec>
SimpleActionClient<ActionSpec>::~SimpleActionClient()
{
  if (spin_thread_)
  {
    {
      boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
      need_to_terminate_ = true;
    }
    spin_thread_->join();
    delete spin_thread_;
  }
  gh_.reset();
  ac_.reset();
}

template<class ActionSpec>
void SimpleActionClient<ActionSpec>::spinThread()
{
  while (nh_.ok())
  {
    {
      boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
      if (need_to_terminate_)
        break;
    }
    callback_queue.callAvailable(ros::WallDuration(0.1f));
  }
}

template<class ActionSpec>
void SimpleActionClient<ActionSpec>::setSimpleState(const SimpleGoalState::StateEnum& next_state)
{
  setSimpleState( SimpleGoalState(next_state) );
}

template<class ActionSpec>
void SimpleActionClient<ActionSpec>::setSimpleState(const SimpleGoalState& next_state)
{
  ROS_DEBUG_NAMED("actionlib", "Transitioning SimpleState from [%s] to [%s]",
            cur_simple_state_.toString().c_str(),
            next_state.toString().c_str());
  cur_simple_state_ = next_state;
}

template<class ActionSpec>
void SimpleActionClient<ActionSpec>::sendGoal(const Goal& goal,
                                              SimpleDoneCallback     done_cb,
                                              SimpleActiveCallback   active_cb,
                                              SimpleFeedbackCallback feedback_cb)
{
  // Reset the old GoalHandle, so that our callbacks won't get called anymore
  gh_.reset();

  // Store all the callbacks
  done_cb_     = done_cb;
  active_cb_   = active_cb;
  feedback_cb_ = feedback_cb;

  cur_simple_state_ = SimpleGoalState::PENDING;

  // Send the goal to the ActionServer
  gh_ = ac_->sendGoal(goal, boost::bind(&SimpleActionClientT::handleTransition, this, _1),
                            boost::bind(&SimpleActionClientT::handleFeedback, this, _1, _2));
}

template<class ActionSpec>
SimpleClientGoalState SimpleActionClient<ActionSpec>::getState()
{
  if (gh_.isExpired())
  {
    ROS_ERROR_NAMED("actionlib", "Trying to getState() when no goal is running. You are incorrectly using SimpleActionClient");
    return SimpleClientGoalState(SimpleClientGoalState::LOST);
  }

  CommState comm_state_ = gh_.getCommState();

  switch( comm_state_.state_)
  {
    case CommState::WAITING_FOR_GOAL_ACK:
    case CommState::PENDING:
    case CommState::RECALLING:
      return SimpleClientGoalState(SimpleClientGoalState::PENDING);
    case CommState::ACTIVE:
    case CommState::PREEMPTING:
      return SimpleClientGoalState(SimpleClientGoalState::ACTIVE);
    case CommState::DONE:
    {
      switch(gh_.getTerminalState().state_)
      {
        case TerminalState::RECALLED:
          return SimpleClientGoalState(SimpleClientGoalState::RECALLED, gh_.getTerminalState().text_);
        case TerminalState::REJECTED:
          return SimpleClientGoalState(SimpleClientGoalState::REJECTED, gh_.getTerminalState().text_);
        case TerminalState::PREEMPTED:
          return SimpleClientGoalState(SimpleClientGoalState::PREEMPTED, gh_.getTerminalState().text_);
        case TerminalState::ABORTED:
          return SimpleClientGoalState(SimpleClientGoalState::ABORTED, gh_.getTerminalState().text_);
        case TerminalState::SUCCEEDED:
          return SimpleClientGoalState(SimpleClientGoalState::SUCCEEDED, gh_.getTerminalState().text_);
        case TerminalState::LOST:
          return SimpleClientGoalState(SimpleClientGoalState::LOST, gh_.getTerminalState().text_);
        default:
          ROS_ERROR_NAMED("actionlib", "Unknown terminal state [%u]. This is a bug in SimpleActionClient", gh_.getTerminalState().state_);
          return SimpleClientGoalState(SimpleClientGoalState::LOST, gh_.getTerminalState().text_);
      }
    }
    case CommState::WAITING_FOR_RESULT:
    case CommState::WAITING_FOR_CANCEL_ACK:
    {
      switch (cur_simple_state_.state_)
      {
        case SimpleGoalState::PENDING:
          return SimpleClientGoalState(SimpleClientGoalState::PENDING);
        case SimpleGoalState::ACTIVE:
          return SimpleClientGoalState(SimpleClientGoalState::ACTIVE);
        case SimpleGoalState::DONE:
          ROS_ERROR_NAMED("actionlib", "In WAITING_FOR_RESULT or WAITING_FOR_CANCEL_ACK, yet we are in SimpleGoalState DONE. This is a bug in SimpleActionClient");
          return SimpleClientGoalState(SimpleClientGoalState::LOST);
        default:
          ROS_ERROR_NAMED("actionlib", "Got a SimpleGoalState of [%u]. This is a bug in SimpleActionClient", cur_simple_state_.state_);
      }
    }
    default:
      break;
  }
  ROS_ERROR_NAMED("actionlib", "Error trying to interpret CommState - %u", comm_state_.state_);
  return SimpleClientGoalState(SimpleClientGoalState::LOST);
}

template<class ActionSpec>
typename SimpleActionClient<ActionSpec>::ResultConstPtr SimpleActionClient<ActionSpec>::getResult()
{
  if (gh_.isExpired())
    ROS_ERROR_NAMED("actionlib", "Trying to getResult() when no goal is running. You are incorrectly using SimpleActionClient");

  if (gh_.getResult())
    return gh_.getResult();

  return ResultConstPtr(new Result);
}


template<class ActionSpec>
void SimpleActionClient<ActionSpec>::cancelAllGoals()
{
  ac_->cancelAllGoals();
}

template<class ActionSpec>
void SimpleActionClient<ActionSpec>::cancelGoalsAtAndBeforeTime(const ros::Time& time)
{
  ac_->cancelGoalsAtAndBeforeTime(time);
}

template<class ActionSpec>
void SimpleActionClient<ActionSpec>::cancelGoal()
{
  if (gh_.isExpired())
    ROS_ERROR_NAMED("actionlib", "Trying to cancelGoal() when no goal is running. You are incorrectly using SimpleActionClient");

  gh_.cancel();
}

template<class ActionSpec>
void SimpleActionClient<ActionSpec>::stopTrackingGoal()
{
  if (gh_.isExpired())
    ROS_ERROR_NAMED("actionlib", "Trying to stopTrackingGoal() when no goal is running. You are incorrectly using SimpleActionClient");
  gh_.reset();
}

template<class ActionSpec>
void SimpleActionClient<ActionSpec>::handleFeedback(GoalHandleT gh, const FeedbackConstPtr& feedback)
{
  if (gh_ != gh)
    ROS_ERROR_NAMED("actionlib", "Got a callback on a goalHandle that we're not tracking.  \
               This is an internal SimpleActionClient/ActionClient bug.  \
               This could also be a GoalID collision");
  if (feedback_cb_)
    feedback_cb_(feedback);
}

template<class ActionSpec>
void SimpleActionClient<ActionSpec>::handleTransition(GoalHandleT gh)
{
  CommState comm_state_ = gh.getCommState();
  switch (comm_state_.state_)
  {
    case CommState::WAITING_FOR_GOAL_ACK:
      ROS_ERROR_NAMED("actionlib", "BUG: Shouldn't ever get a transition callback for WAITING_FOR_GOAL_ACK");
      break;
    case CommState::PENDING:
      ROS_ERROR_COND( cur_simple_state_ != SimpleGoalState::PENDING,
                      "BUG: Got a transition to CommState [%s] when our in SimpleGoalState [%s]",
                      comm_state_.toString().c_str(), cur_simple_state_.toString().c_str());
      break;
    case CommState::ACTIVE:
      switch (cur_simple_state_.state_)
      {
        case SimpleGoalState::PENDING:
          setSimpleState(SimpleGoalState::ACTIVE);
          if (active_cb_)
            active_cb_();
          break;
        case SimpleGoalState::ACTIVE:
          break;
        case SimpleGoalState::DONE:
          ROS_ERROR_NAMED("actionlib", "BUG: Got a transition to CommState [%s] when in SimpleGoalState [%s]",
                    comm_state_.toString().c_str(), cur_simple_state_.toString().c_str());
          break;
        default:
          ROS_FATAL("Unknown SimpleGoalState %u", cur_simple_state_.state_);
          break;
      }
      break;
    case CommState::WAITING_FOR_RESULT:
      break;
    case CommState::WAITING_FOR_CANCEL_ACK:
      break;
    case CommState::RECALLING:
      ROS_ERROR_COND( cur_simple_state_ != SimpleGoalState::PENDING,
                      "BUG: Got a transition to CommState [%s] when our in SimpleGoalState [%s]",
                      comm_state_.toString().c_str(), cur_simple_state_.toString().c_str());
      break;
    case CommState::PREEMPTING:
      switch (cur_simple_state_.state_)
      {
        case SimpleGoalState::PENDING:
          setSimpleState(SimpleGoalState::ACTIVE);
          if (active_cb_)
            active_cb_();
          break;
        case SimpleGoalState::ACTIVE:
          break;
        case SimpleGoalState::DONE:
          ROS_ERROR_NAMED("actionlib", "BUG: Got a transition to CommState [%s] when in SimpleGoalState [%s]",
                     comm_state_.toString().c_str(), cur_simple_state_.toString().c_str());
          break;
        default:
          ROS_FATAL("Unknown SimpleGoalState %u", cur_simple_state_.state_);
          break;
      }
      break;
    case CommState::DONE:
      switch (cur_simple_state_.state_)
      {
        case SimpleGoalState::PENDING:
        case SimpleGoalState::ACTIVE:
          done_mutex_.lock();
          setSimpleState(SimpleGoalState::DONE);
          done_mutex_.unlock();

          if (done_cb_)
            done_cb_(getState(), gh.getResult());

          done_condition_.notify_all();
          break;
        case SimpleGoalState::DONE:
          ROS_ERROR_NAMED("actionlib", "BUG: Got a second transition to DONE");
          break;
        default:
          ROS_FATAL("Unknown SimpleGoalState %u", cur_simple_state_.state_);
          break;
      }
      break;
    default:
      ROS_ERROR_NAMED("actionlib", "Unknown CommState received [%u]", comm_state_.state_);
      break;
  }
}

template<class ActionSpec>
bool SimpleActionClient<ActionSpec>::waitForResult(const ros::Duration& timeout )
{
  if (gh_.isExpired())
  {
    ROS_ERROR_NAMED("actionlib", "Trying to waitForGoalToFinish() when no goal is running. You are incorrectly using SimpleActionClient");
    return false;
  }

  if (timeout < ros::Duration(0,0))
    ROS_WARN_NAMED("actionlib", "Timeouts can't be negative. Timeout is [%.2fs]", timeout.toSec());

  ros::Time timeout_time = ros::Time::now() + timeout;

  boost::mutex::scoped_lock lock(done_mutex_);

  // Hardcode how often we check for node.ok()
  ros::Duration loop_period = ros::Duration().fromSec(.1);

  while (nh_.ok())
  {
    // Determine how long we should wait
    ros::Duration time_left = timeout_time - ros::Time::now();

    // Check if we're past the timeout time
    if (timeout > ros::Duration(0,0) && time_left <= ros::Duration(0,0) )
    {
      break;
    }
    
    if (cur_simple_state_ == SimpleGoalState::DONE)
    {
      break;
    }


    // Truncate the time left
    if (time_left > loop_period || timeout == ros::Duration())
      time_left = loop_period;

    done_condition_.timed_wait(lock, boost::posix_time::milliseconds(time_left.toSec() * 1000.0f));
  }
  
  return (cur_simple_state_ == SimpleGoalState::DONE);
}

template<class ActionSpec>
SimpleClientGoalState SimpleActionClient<ActionSpec>::sendGoalAndWait(const Goal& goal,
                                                                      const ros::Duration& execute_timeout,
                                                                      const ros::Duration& preempt_timeout)
{
  sendGoal(goal);

  // See if the goal finishes in time
  if (waitForResult(execute_timeout))
  {
    ROS_DEBUG_NAMED("actionlib", "Goal finished within specified execute_timeout [%.2f]", execute_timeout.toSec());
    return getState();
  }

  ROS_DEBUG_NAMED("actionlib", "Goal didn't finish within specified execute_timeout [%.2f]", execute_timeout.toSec());

  // It didn't finish in time, so we need to preempt it
  cancelGoal();

  // Now wait again and see if it finishes
  if (waitForResult(preempt_timeout))
    ROS_DEBUG_NAMED("actionlib", "Preempt finished within specified preempt_timeout [%.2f]", preempt_timeout.toSec());
  else
    ROS_DEBUG_NAMED("actionlib", "Preempt didn't finish specified preempt_timeout [%.2f]", preempt_timeout.toSec());
  return getState();
}

}

#undef DEPRECATED

#endif // ACTIONLIB_SINGLE_GOAL_ACTION_CLIENT_H_