actionExecutorService.hpp
Go to the documentation of this file.
00001 #ifndef ACTION_EXECUTOR_SERVICE_H
00002 #define ACTION_EXECUTOR_SERVICE_H
00003 
00004 #include "continual_planning_executive/actionExecutorInterface.h"
00005 #include "continual_planning_executive/symbolicState.h"
00006 #include <ros/ros.h>
00007 
00009 
00019 template <class Service>
00020 class ActionExecutorService : public continual_planning_executive::ActionExecutorInterface
00021 {
00022    public:
00023       ActionExecutorService();
00024       ~ActionExecutorService();
00025 
00027 
00031       virtual void initialize(const std::deque<std::string> & arguments);
00032 
00034 
00037       virtual bool canExecute(const DurativeAction & a, const SymbolicState & current) const;
00038 
00040       virtual bool executeBlocking(const DurativeAction & a, SymbolicState & current);
00041 
00043 
00048       virtual bool fillGoal(typename Service::Request & goal,
00049              const DurativeAction & a, const SymbolicState & current) = 0;
00050 
00052 
00058       virtual void updateState(bool success, typename Service::Response & response,
00059               const DurativeAction & a, SymbolicState & current) {}
00060 
00062       virtual void cancelAction() {}
00063 
00064    protected:
00065       ros::NodeHandle* _nh;
00066       ros::ServiceClient _serviceClient;
00067 
00068       std::string _actionName;
00069 };
00070 
00071 
00072 template <class Service>
00073 ActionExecutorService<Service>::ActionExecutorService() : _nh(NULL)
00074 {
00075 }
00076 
00077 template <class Service>
00078 ActionExecutorService<Service>::~ActionExecutorService()
00079 {
00080     delete _nh;
00081 }
00082 
00083 
00084 template <class Service>
00085 void ActionExecutorService<Service>::initialize(const std::deque<std::string> & arguments)
00086 {
00087     ROS_ASSERT(arguments.size() >= 2);
00088     _actionName = arguments.at(0);
00089     ROS_INFO("Initializing ActionExecutor for action %s...", _actionName.c_str());
00090 
00091     std::string serviceName = arguments.at(1);
00092     _nh = new ros::NodeHandle();
00093 
00094     // wait for the service to come up
00095     while(!ros::service::waitForService(serviceName, ros::Duration(5.0))) {
00096         ROS_WARN("Service %s not available - waiting.", serviceName.c_str());
00097     }
00098 
00099     _serviceClient = _nh->serviceClient<Service>(serviceName);
00100     if(!_serviceClient) {
00101         ROS_FATAL("Could not initialize service from %s (client name: %s)",
00102                 serviceName.c_str(), _serviceClient.getService().c_str());
00103     }
00104 
00105     ROS_INFO("Initialized ActionExecutor for action %s.", _actionName.c_str());
00106 }
00107 
00108 template <class Service>
00109 bool ActionExecutorService<Service>::canExecute(
00110         const DurativeAction & a, const SymbolicState & current) const
00111 {
00112     return (a.name == _actionName);
00113 }
00114 
00115 template <class Service>
00116 bool ActionExecutorService<Service>::executeBlocking(const DurativeAction & a, SymbolicState & current)
00117 {
00118     Service service;
00119     if(!fillGoal(service.request, a, current)) {
00120         return false;
00121     }
00122 
00123     if(!_serviceClient) {
00124         ROS_ERROR("Persistent service connection to %s failed.", _serviceClient.getService().c_str());
00125 
00126         // try to reconnect - this shouldn't happen.
00127         _serviceClient = _nh->serviceClient<Service>(_serviceClient.getService(), true);
00128         if(!_serviceClient) {
00129             ROS_FATAL("Could not reconnect service from %s", _serviceClient.getService().c_str());
00130             return false;
00131         }
00132     }
00133 
00134     ROS_INFO("Calling service for action: %s.", _actionName.c_str());
00135 
00136     // blocking call
00137     bool success = _serviceClient.call(service);
00138 
00139     updateState(success, service.response, a, current);
00140 
00141     if(success) {
00142         ROS_INFO("Reached goal for action: %s.", _actionName.c_str());
00143         return true;
00144     }
00145 
00146     ROS_INFO("Could not reach goal for action %s!", _actionName.c_str());
00147     return false;
00148 }
00149 
00150 #endif
00151 


continual_planning_executive
Author(s): Christian Dornhege
autogenerated on Mon Oct 6 2014 07:51:56