planExecutor.cpp
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                 // FIXME: insert even if failed as we tried and the action is "used up"
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 


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