actionExecutorActionlib.hpp

Go to the documentation of this file.

#ifndef ACTION_EXECUTOR_ACTIONLIB_H
#define ACTION_EXECUTOR_ACTIONLIB_H

#include "continual_planning_executive/actionExecutorInterface.h"
#include "continual_planning_executive/symbolicState.h"
#include <actionlib/client/simple_action_client.h>


template <class Action, class ActionGoal, class ActionResult>
class ActionExecutorActionlib : public continual_planning_executive::ActionExecutorInterface
{
   public:
      ActionExecutorActionlib();
      ~ActionExecutorActionlib();



      virtual void initialize(const std::deque<std::string> & arguments);


      virtual bool canExecute(const DurativeAction & a, const SymbolicState & current) const;

      virtual bool executeBlocking(const DurativeAction & a, SymbolicState & current);


      virtual bool fillGoal(ActionGoal & goal, const DurativeAction & a, const SymbolicState & current) = 0;


      virtual void updateState(const actionlib::SimpleClientGoalState & actionReturnState, const ActionResult & result,
              const DurativeAction & a, SymbolicState & current) {}

      virtual void cancelAction();

   protected:
      typedef actionlib::SimpleActionClient<Action> ActionClient;
      ActionClient* _actionClient;

      std::string _actionName;
};


template <class Action, class ActionGoal, class ActionResult>
ActionExecutorActionlib<Action, ActionGoal, ActionResult>::ActionExecutorActionlib() : _actionClient(NULL)
{
}

template <class Action, class ActionGoal, class ActionResult>
ActionExecutorActionlib<Action, ActionGoal, ActionResult>::~ActionExecutorActionlib()
{
    _actionClient->cancelAllGoals();

    delete _actionClient;
}


template <class Action, class ActionGoal, class ActionResult>
void ActionExecutorActionlib<Action, ActionGoal, ActionResult>::initialize(const std::deque<std::string> & arguments)
{
    ROS_ASSERT(arguments.size() >= 2);
    _actionName = arguments.at(0);
    ROS_INFO("Initializing ActionExecutor for action %s...", _actionName.c_str());

    std::string actionServeName = arguments.at(1);

    _actionClient = new ActionClient(actionServeName, true);
    ROS_INFO("Created action client.");
    // wait for the action server to come up
    while(!_actionClient->waitForServer(ros::Duration(5.0))){
        ROS_INFO("Waiting for the %s action server to come up...", actionServeName.c_str());
    }
    ROS_INFO("Initialized ActionExecutor for action %s.", _actionName.c_str());
}

template <class Action, class ActionGoal, class ActionResult>
bool ActionExecutorActionlib<Action, ActionGoal, ActionResult>::canExecute(
        const DurativeAction & a, const SymbolicState & current) const
{
    return (a.name == _actionName);
}

template <class Action, class ActionGoal, class ActionResult>
bool ActionExecutorActionlib<Action, ActionGoal, ActionResult>::executeBlocking(const DurativeAction & a, SymbolicState & current)
{
    ActionGoal goal;
    if(!fillGoal(goal, a, current)) {
        return false;
    }

    ROS_INFO("Sending goal for action: %s.", _actionName.c_str());
    _actionClient->sendGoal(goal);

    // blocking call
    _actionClient->waitForResult();

    updateState(_actionClient->getState(), *(_actionClient->getResult()), a, current);

    if(_actionClient->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) {
        ROS_INFO("Reached goal for action: %s.", _actionName.c_str());
        return true;
    }

    ROS_INFO("Could not reach goal for action %s! Resulting action state: %s.",
           _actionName.c_str(), _actionClient->getState().toString().c_str());
    return false;
}

template <class Action, class ActionGoal, class ActionResult>
void ActionExecutorActionlib<Action, ActionGoal, ActionResult>::cancelAction()
{
    _actionClient->cancelAllGoals();
}

#endif