actionExecutorService.hpp

Go to the documentation of this file.

#ifndef ACTION_EXECUTOR_SERVICE_H
#define ACTION_EXECUTOR_SERVICE_H

#include "continual_planning_executive/actionExecutorInterface.h"
#include "continual_planning_executive/symbolicState.h"
#include <ros/ros.h>


template <class Service>
class ActionExecutorService : public continual_planning_executive::ActionExecutorInterface
{
   public:
      ActionExecutorService();
      ~ActionExecutorService();


      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(typename Service::Request & goal,
             const DurativeAction & a, const SymbolicState & current) = 0;


      virtual void updateState(bool success, typename Service::Response & response,
              const DurativeAction & a, SymbolicState & current) {}

      virtual void cancelAction() {}

   protected:
      ros::NodeHandle* _nh;
      ros::ServiceClient _serviceClient;

      std::string _actionName;
};


template <class Service>
ActionExecutorService<Service>::ActionExecutorService() : _nh(NULL)
{
}

template <class Service>
ActionExecutorService<Service>::~ActionExecutorService()
{
    delete _nh;
}


template <class Service>
void ActionExecutorService<Service>::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 serviceName = arguments.at(1);
    _nh = new ros::NodeHandle();

    // wait for the service to come up
    while(!ros::service::waitForService(serviceName, ros::Duration(5.0))) {
        ROS_WARN("Service %s not available - waiting.", serviceName.c_str());
    }

    _serviceClient = _nh->serviceClient<Service>(serviceName);
    if(!_serviceClient) {
        ROS_FATAL("Could not initialize service from %s (client name: %s)",
                serviceName.c_str(), _serviceClient.getService().c_str());
    }

    ROS_INFO("Initialized ActionExecutor for action %s.", _actionName.c_str());
}

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

template <class Service>
bool ActionExecutorService<Service>::executeBlocking(const DurativeAction & a, SymbolicState & current)
{
    Service service;
    if(!fillGoal(service.request, a, current)) {
        return false;
    }

    if(!_serviceClient) {
        ROS_ERROR("Persistent service connection to %s failed.", _serviceClient.getService().c_str());

        // try to reconnect - this shouldn't happen.
        _serviceClient = _nh->serviceClient<Service>(_serviceClient.getService(), true);
        if(!_serviceClient) {
            ROS_FATAL("Could not reconnect service from %s", _serviceClient.getService().c_str());
            return false;
        }
    }

    ROS_INFO("Calling service for action: %s.", _actionName.c_str());

    // blocking call
    bool success = _serviceClient.call(service);

    updateState(success, service.response, a, current);

    if(success) {
        ROS_INFO("Reached goal for action: %s.", _actionName.c_str());
        return true;
    }

    ROS_INFO("Could not reach goal for action %s!", _actionName.c_str());
    return false;
}

#endif