actionExecutorActionlib.hpp
Go to the documentation of this file.
00001 #ifndef ACTION_EXECUTOR_ACTIONLIB_H
00002 #define ACTION_EXECUTOR_ACTIONLIB_H
00003 
00004 #include "continual_planning_executive/actionExecutorInterface.h"
00005 #include "continual_planning_executive/symbolicState.h"
00006 #include <actionlib/client/simple_action_client.h>
00007 
00009 
00020 template <class Action, class ActionGoal, class ActionResult>
00021 class ActionExecutorActionlib : public continual_planning_executive::ActionExecutorInterface
00022 {
00023    public:
00024       ActionExecutorActionlib();
00025       ~ActionExecutorActionlib();
00026 
00027 
00029 
00034       virtual void initialize(const std::deque<std::string> & arguments);
00035 
00037 
00040       virtual bool canExecute(const DurativeAction & a, const SymbolicState & current) const;
00041 
00043       virtual bool executeBlocking(const DurativeAction & a, SymbolicState & current);
00044 
00046 
00051       virtual bool fillGoal(ActionGoal & goal, const DurativeAction & a, const SymbolicState & current) = 0;
00052 
00054 
00059       virtual void updateState(const actionlib::SimpleClientGoalState & actionReturnState, const ActionResult & result,
00060               const DurativeAction & a, SymbolicState & current) {}
00061 
00063       virtual void cancelAction();
00064 
00065    protected:
00066       typedef actionlib::SimpleActionClient<Action> ActionClient;
00067       ActionClient* _actionClient;
00068 
00069       std::string _actionName;
00070 };
00071 
00072 
00073 template <class Action, class ActionGoal, class ActionResult>
00074 ActionExecutorActionlib<Action, ActionGoal, ActionResult>::ActionExecutorActionlib() : _actionClient(NULL)
00075 {
00076 }
00077 
00078 template <class Action, class ActionGoal, class ActionResult>
00079 ActionExecutorActionlib<Action, ActionGoal, ActionResult>::~ActionExecutorActionlib()
00080 {
00081     _actionClient->cancelAllGoals();
00082 
00083     delete _actionClient;
00084 }
00085 
00086 
00087 template <class Action, class ActionGoal, class ActionResult>
00088 void ActionExecutorActionlib<Action, ActionGoal, ActionResult>::initialize(const std::deque<std::string> & arguments)
00089 {
00090     ROS_ASSERT(arguments.size() >= 2);
00091     _actionName = arguments.at(0);
00092     ROS_INFO("Initializing ActionExecutor for action %s...", _actionName.c_str());
00093 
00094     std::string actionServeName = arguments.at(1);
00095 
00096     _actionClient = new ActionClient(actionServeName, true);
00097     ROS_INFO("Created action client.");
00098     // wait for the action server to come up
00099     while(!_actionClient->waitForServer(ros::Duration(5.0))){
00100         ROS_INFO("Waiting for the %s action server to come up...", actionServeName.c_str());
00101     }
00102     ROS_INFO("Initialized ActionExecutor for action %s.", _actionName.c_str());
00103 }
00104 
00105 template <class Action, class ActionGoal, class ActionResult>
00106 bool ActionExecutorActionlib<Action, ActionGoal, ActionResult>::canExecute(
00107         const DurativeAction & a, const SymbolicState & current) const
00108 {
00109     return (a.name == _actionName);
00110 }
00111 
00112 template <class Action, class ActionGoal, class ActionResult>
00113 bool ActionExecutorActionlib<Action, ActionGoal, ActionResult>::executeBlocking(const DurativeAction & a, SymbolicState & current)
00114 {
00115     ActionGoal goal;
00116     if(!fillGoal(goal, a, current)) {
00117         return false;
00118     }
00119 
00120     ROS_INFO("Sending goal for action: %s.", _actionName.c_str());
00121     _actionClient->sendGoal(goal);
00122 
00123     // blocking call
00124     _actionClient->waitForResult();
00125 
00126     updateState(_actionClient->getState(), *(_actionClient->getResult()), a, current);
00127 
00128     if(_actionClient->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) {
00129         ROS_INFO("Reached goal for action: %s.", _actionName.c_str());
00130         return true;
00131     }
00132 
00133     ROS_INFO("Could not reach goal for action %s! Resulting action state: %s.",
00134            _actionName.c_str(), _actionClient->getState().toString().c_str());
00135     return false;
00136 }
00137 
00138 template <class Action, class ActionGoal, class ActionResult>
00139 void ActionExecutorActionlib<Action, ActionGoal, ActionResult>::cancelAction()
00140 {
00141     _actionClient->cancelAllGoals();
00142 }
00143 
00144 #endif
00145 


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