continualPlanning.cpp
Go to the documentation of this file.
00001 #include "continualPlanning.h"
00002 #include <ros/ros.h>
00003 #include <iomanip>
00004 #include <iostream>
00005 
00006 ContinualPlanning::ContinualPlanning() : _planner(NULL)
00007 {
00008     _replanningTrigger = ReplanByMonitoring;
00009     _allowDirectGoalCheck = false;      // OK, we do this via monitoring
00010     _forceReplan = true;
00011 }
00012 
00013 ContinualPlanning::~ContinualPlanning()
00014 {
00015 }
00016 
00017 ContinualPlanning::ContinualPlanningState ContinualPlanning::loop()
00018 {
00019     if(!estimateCurrentState()) {
00020         ROS_WARN("State estimation failed.");     // FIXME: continue execution until this works.
00021         return Running;
00022     }
00023 
00024     if(_allowDirectGoalCheck && isGoalFulfilled()) { // done!
00025         _status.finishedContinualPlanning(true, "Goal fulfilled");
00026         return FinishedAtGoal;
00027     }
00028 
00029     bool atGoal = false;
00030     Plan newPlan = monitorAndReplan(atGoal);
00031     if(newPlan.empty()) {
00032         if(atGoal) {
00033             ROS_INFO("\n\nEmpty plan returned by monitorAndReplan - Reached Goal!\n\n");
00034             _status.finishedContinualPlanning(true, "Empty plan returned by monitorAndReplan - Reached Goal!");
00035             return FinishedAtGoal;
00036         }
00037 
00038         ROS_ERROR("\n\nEmpty plan returned by monitorAndReplan - not at goal.\n\n");
00039         _status.finishedContinualPlanning(false, "Empty plan returned by monitorAndReplan - not at goal.");
00040         return FinishedNoPlanToGoal;           // not at goal and no plan -> can't do anything besides fail
00041     }
00042 
00043     // exec plan
00044     _currentPlan = newPlan;
00045     _status.updateCurrentPlan(_currentPlan);
00046 
00047     // TODO_TP: just send and remember actions
00048     //          supervise those while running and estimating state.
00049     std::set<DurativeAction> executedActions;
00050     // should not be empty (see above), FIXME exec should only exec the first
00051     _status.startedExecution(_currentPlan.actions.front()); 
00052     if(!_planExecutor.executeBlocking(_currentPlan, _currentState, executedActions)) {
00053         _status.finishedExecution(false, _currentPlan.actions.front());
00054         ROS_ERROR_STREAM("No action was executed for current plan:\n" << _currentPlan << "\nWaiting for 10 sec...");
00055         _forceReplan = true;        // force here in the hope that it fixes something.
00056         ros::WallDuration sleep(10.0);
00057         sleep.sleep();
00058     } else {
00059         _status.finishedExecution(true, _currentPlan.actions.front());
00060     }
00061 
00062     // remove executedActions from plan
00063     for(std::set<DurativeAction>::iterator it = executedActions.begin(); it != executedActions.end(); it++) {
00064         _currentPlan.removeAction(*it);
00065     }
00066     _status.updateCurrentPlan(_currentPlan);
00067 
00068     return Running;
00069 }
00070 
00071 bool ContinualPlanning::executeActionDirectly(const DurativeAction & a, bool publishStatus)
00072 {
00073     _status.setEnabled(publishStatus);
00074 
00075     if(!estimateCurrentState()) {
00076         ROS_ERROR("State estimation failed.");
00077         _status.setEnabled(true);
00078         return false;
00079     }
00080 
00081     Plan plan;
00082     plan.actions.push_back(a);
00083 
00084     std::set<DurativeAction> executedActions;
00085     // should not be empty (see above), FIXME exec should only exec the first
00086     _status.startedExecution(plan.actions.front()); 
00087     if(!_planExecutor.executeBlocking(plan, _currentState, executedActions)) {
00088         _status.finishedExecution(false, plan.actions.front());
00089         ROS_ERROR_STREAM("No action was executed for current plan:\n" << plan);
00090         _status.setEnabled(true);
00091         return false;
00092     } else {
00093         _status.finishedExecution(true, plan.actions.front());
00094     }
00095 
00096     _status.setEnabled(true);
00097     return !executedActions.empty();
00098 }
00099 
00100 bool ContinualPlanning::isGoalFulfilled() const
00101 {
00102     return _goal.isFulfilledBy(_currentState);
00103 }
00104 
00105 bool ContinualPlanning::estimateCurrentState()
00106 {
00107     _status.startedStateEstimation();
00108     bool ret = true;
00109     for(std::vector<continual_planning_executive::StateCreator*>::iterator it = _stateCreators.begin();
00110             it != _stateCreators.end(); it++) {
00111         ret &= (*it)->fillState(_currentState);
00112     }
00113     ROS_INFO_STREAM("Current state is: " << _currentState << std::endl);
00114     _status.finishedStateEstimation(ret, _currentState);
00115     return ret;
00116 }
00117 
00118 Plan ContinualPlanning::monitorAndReplan(bool & atGoal)
00119 {
00120     _status.startedMonitoring();
00121     if(!needReplanning(atGoal)) {
00122         _status.finishedMonitoring(true);
00123         _status.publishStatus(continual_planning_executive::ContinualPlanningStatus::PLANNING,
00124                 continual_planning_executive::ContinualPlanningStatus::INACTIVE, "-");
00125         return _currentPlan;
00126     }
00127     _status.finishedMonitoring(false);  // need replanning -> monitoring false
00128 
00129     // REPLAN
00130     Plan plan;
00131     _status.startedPlanning();
00132     continual_planning_executive::PlannerInterface::PlannerResult result = 
00133         _planner->plan(_currentState, _goal, plan);
00134     _forceReplan = false;       // did replanning
00135 
00136     if(result == continual_planning_executive::PlannerInterface::PR_SUCCESS
00137             || result == continual_planning_executive::PlannerInterface::PR_SUCCESS_TIMEOUT) {
00138         ROS_INFO_STREAM("Planning successfull. Got plan:" << std::endl
00139                 << std::fixed << std::setprecision(2) << plan);
00140         _status.finishedPlanning(true, plan);
00141     } else {
00142         ROS_ERROR("Planning failed, result: %s",
00143                 continual_planning_executive::PlannerInterface::PlannerResultStr(result).c_str());
00144         _status.finishedPlanning(false, plan);
00145     }
00146     return plan;
00147 }
00148 
00149 bool ContinualPlanning::needReplanning(bool & atGoal) const
00150 {
00151     static SymbolicState lastReplanState;
00152 
00153     if(_forceReplan) {
00154         ROS_WARN("Replanning was forced.");
00155         return true;
00156     }
00157 
00158     // in monitoring it is OK to monitor the empty plan - only if that fails we need to replan
00159     if(_currentPlan.empty() && _replanningTrigger != ReplanByMonitoring) {
00160         lastReplanState = _currentState;
00161         return true;
00162     }
00163 
00164     switch(_replanningTrigger) {
00165         case ReplanAlways:
00166             return true;
00167             break;
00168         case ReplanIfLogicalStateChanged:
00169             if(_currentState.booleanEquals(lastReplanState))
00170                 return false;
00171 
00172             ROS_INFO("state changed since last replanning.");
00173             lastReplanState = _currentState;
00174             return true;
00175             break;
00176         case ReplanByMonitoring:
00177             // check that: app(currentState, currentPlan) reaches goal
00178             {
00179                 continual_planning_executive::PlannerInterface::PlannerResult result = 
00180                     _planner->monitor(_currentState, _goal, _currentPlan);
00181                 if(result == continual_planning_executive::PlannerInterface::PR_SUCCESS) {
00182                     if(_currentPlan.empty())
00183                         atGoal = true;
00184                     return false;   // success = _currentPlan leads to goal -> no replanning
00185                 } else if(result == continual_planning_executive::PlannerInterface::PR_FAILURE_UNREACHABLE) {
00186                     return true;
00187                 } else {
00188                     ROS_WARN("Unexpected monitoring result: %s",
00189                             continual_planning_executive::PlannerInterface::PlannerResultStr(result).c_str());
00190                     // there should be no timeouts in monitoring
00191                     if(result == continual_planning_executive::PlannerInterface::PR_SUCCESS_TIMEOUT) {
00192                         if(_currentPlan.empty())
00193                             atGoal = true;
00194                         return false;
00195                     }
00196                     return true;
00197                 }
00198             }
00199             break;
00200     }
00201 
00202     return true;
00203 }
00204 


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