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
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
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