Go to the documentation of this file.00001 #include "planExecutor.h"
00002 #include <ros/ros.h>
00003 #include <iostream>
00004
00005 PlanExecutor::PlanExecutor()
00006 {
00007 _onlyExecuteActionAtZeroTime = true;
00008 }
00009
00010 PlanExecutor::~PlanExecutor()
00011 {
00012 }
00013
00014 void PlanExecutor::addActionExecutor(continual_planning_executive::ActionExecutorInterface* ae)
00015 {
00016 _actionExecutors.push_back(ae);
00017 }
00018
00019 bool PlanExecutor::executeBlocking(const Plan & p, SymbolicState & currentState,
00020 std::set<DurativeAction> & executedActions)
00021 {
00022 int actionsExectued = 0;
00023 forEach(const DurativeAction & da, p.actions) {
00024 if(_onlyExecuteActionAtZeroTime && da.startTime > 0.01)
00025 continue;
00026
00027 bool count = 0;
00028 forEach(continual_planning_executive::ActionExecutorInterface* ai, _actionExecutors) {
00029 if(ai->canExecute(da, currentState))
00030 count++;
00031 }
00032 if(count == 0) {
00033 std::cerr << "WARNING: No ActionExecutor for action: " << da << std::endl;
00034 }
00035 if(count > 1) {
00036 std::cerr << "WARNING: " << count << " ActionExecutors for action: " << da << std::endl;
00037 }
00038 forEach(continual_planning_executive::ActionExecutorInterface* ai, _actionExecutors) {
00039 if(ai->canExecute(da, currentState)) {
00040 ROS_INFO_STREAM("Trying to execute action: \"" << da << "\"");
00041 if(ai->executeBlocking(da, currentState)) {
00042 actionsExectued++;
00043 ROS_INFO_STREAM("Successfully executed action: \"" << da << "\"");
00044 } else {
00045 ROS_ERROR_STREAM("Action execution failed for action: \"" << da << "\"");
00046 }
00047
00048 executedActions.insert(da);
00049 }
00050 }
00051 }
00052
00053 if(actionsExectued > 1)
00054 ROS_WARN("Executed %d actions in one step.", actionsExectued);
00055
00056 return actionsExectued > 0;
00057 }
00058
00059 void PlanExecutor::cancelAllActions()
00060 {
00061 forEach(continual_planning_executive::ActionExecutorInterface* ai, _actionExecutors) {
00062 ai->cancelAction();
00063 }
00064 }
00065