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;
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.");
00021 return Running;
00022 }
00023
00024 if(_allowDirectGoalCheck && isGoalFulfilled()) {
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;
00041 }
00042
00043
00044 _currentPlan = newPlan;
00045 _status.updateCurrentPlan(_currentPlan);
00046
00047
00048
00049 std::set<DurativeAction> executedActions;
00050
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;
00056 ros::WallDuration sleep(10.0);
00057 sleep.sleep();
00058 } else {
00059 _status.finishedExecution(true, _currentPlan.actions.front());
00060 }
00061
00062
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
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);
00128
00129
00130 Plan plan;
00131 _status.startedPlanning();
00132 continual_planning_executive::PlannerInterface::PlannerResult result =
00133 _planner->plan(_currentState, _goal, plan);
00134 _forceReplan = false;
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
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
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;
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
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