Go to the documentation of this file.00001 #include "monitoring.h"
00002 #include "state.h"
00003 #include "operator.h"
00004 #include "globals.h"
00005 #include "axioms.h"
00006 #include "plannerParameters.h"
00007 #include "ros_printouts.h"
00008 #if ROS_BUILD
00009 #include <ros/ros.h>
00010 #endif
00011 #include <iostream>
00012 #include <iomanip>
00013 #include <sstream>
00014 #include <iomanip>
00015 #include <cassert>
00016 #include <deque>
00017 #include <algorithm>
00018 #include <fstream>
00019
00020 using namespace std;
00021
00022 MonitorEngine* MonitorEngine::instance = 0;
00023
00024 MonitorEngine* MonitorEngine::getInstance()
00025 {
00026 if (instance == 0)
00027 instance = new MonitorEngine();
00028
00029 return instance;
00030 }
00031
00032 void MonitorEngine::readPlanFromFile(const string& filename, vector<string>& plan)
00033 {
00034 ifstream fin(filename.c_str(), ifstream::in);
00035 string buffer;
00036 while(fin.good()) {
00037 getline(fin, buffer, '\n');
00038 if(!buffer.empty())
00039 plan.push_back(buffer);
00040 }
00041 fin.close();
00042 }
00043
00044 bool MonitorEngine::validatePlan(const string & filename)
00045 {
00046 vector<string> plan;
00047 MonitorEngine::readPlanFromFile(filename, plan);
00048
00049 MonitorEngine* mon = MonitorEngine::getInstance();
00050 bool monitor = mon->validatePlan(plan);
00051 return monitor;
00052 }
00053
00054
00055 MonitorEngine::MonitorEngine()
00056 {
00057 }
00058
00059 MonitorEngine::~MonitorEngine()
00060 {
00061 }
00062
00063 int lower_case(int c)
00064 {
00065 return tolower(c);
00066 }
00067
00068 bool MonitorEngine::validatePlan(vector<string>& plan)
00069 {
00070 std::vector< std::vector<PlanStep> > p;
00071 for (unsigned int i = 0; i < plan.size(); i++) {
00072 if(plan[i].length() == 0)
00073 continue;
00074 if((plan[i])[0] == ';')
00075 continue;
00076
00077 if(plan[i].find(":") == string::npos) {
00078 ROS_FATAL("%s - could not find ':' in step: %s.", __func__, plan[i].c_str());
00079 return false;
00080 }
00081 const char* startStr = plan[i].substr(0, plan[i].find(":")).c_str();
00082 char* check = NULL;
00083 double start = strtod(startStr, &check);
00084 if(check == startStr) {
00085 ROS_FATAL("%s: Start parsing failed in step: %s.", __func__, plan[i].c_str());
00086 return false;
00087 }
00088
00089
00090 if(plan[i].find("(") == string::npos || plan[i].find(")") == string::npos) {
00091 ROS_FATAL("%s - could not find '(' and ')' in step: %s.", __func__, plan[i].c_str());
00092 return false;
00093 }
00094 string name = plan[i].substr(plan[i].find("(") + 1, plan[i].find(")")
00095 - plan[i].find("(") - 1);
00096 transform(name.begin(), name.end(), name.begin(), lower_case);
00097
00098
00099 if(plan[i].find("[") == string::npos || plan[i].find("]") == string::npos) {
00100 ROS_FATAL("%s - could not find '[' and ']' in step: %s.", __func__, plan[i].c_str());
00101 return false;
00102 }
00103 const char* durStr = plan[i].substr(plan[i].find("[") + 1, plan[i].length() - plan[i].find("[")).c_str();
00104 check = NULL;
00105 double duration = strtod(durStr, &check);
00106 if(check == durStr) {
00107 ROS_FATAL("%s: Duration parsing failed in step: %s.", __func__, plan[i].c_str());
00108 return false;
00109 }
00110
00111
00112 bool opFound = false;
00113 p.push_back(std::vector<PlanStep>());
00114 for(unsigned int j = 0; j < g_operators.size(); j++) {
00115 if(g_operators[j].get_name() == name) {
00116
00117 p.back().push_back(PlanStep(start,duration,&g_operators[j], NULL));
00118 opFound = true;
00119 }
00120 }
00121 if(!opFound) {
00122 ROS_FATAL("%s: Could not find matching operator for plan step: \"%s\"", __func__, name.c_str());
00123 return false;
00124 }
00125 ROS_ASSERT(!p.back().empty());
00126 }
00127
00128
00129 stable_sort(p.begin(), p.end(), PlanStepCompareStartTime());
00130
00131 ROS_DEBUG_STREAM("Validating Plan:");
00132 unsigned int count = 0;
00133 for(std::vector< std::vector<PlanStep> >::iterator it = p.begin(); it != p.end(); it++) {
00134
00135 ROS_DEBUG_STREAM("[" << count << "] \t" << setprecision(2) << fixed << it->front().start_time
00136 << " \"" << it->front().op->get_name() << "\" " << it->front().duration);
00137 count++;
00138 }
00139
00140 return validatePlan(p);
00141 }
00142
00162 bool MonitorEngine::validatePlan(const std::vector< std::vector<PlanStep> > & plan)
00163 {
00164 if(plan.empty()) {
00165 return g_initial_state->satisfies(g_goal);
00166 }
00167
00168 deque< FullPlanTrace > currentTraces;
00169
00170 currentTraces.push_back(FullPlanTrace(*g_initial_state));
00171
00172 for(int i = 0; i < plan.size(); i++) {
00173
00174
00175
00176
00177
00178 deque< FullPlanTrace > nextTraces;
00179
00180 for(deque<FullPlanTrace>::iterator it = currentTraces.begin(); it != currentTraces.end(); it++) {
00181
00182
00183
00184
00185 FullPlanTrace curTrace = *it;
00186
00187 if(g_parameters.monitoring_verify_timestamps) {
00188
00189 double dt = fabs(plan[i].front().start_time - curTrace.lastTimestamp());
00190 if(dt > EPS_TIME) {
00191 continue;
00192 }
00193 }
00194
00195
00196
00197 for(std::vector<PlanStep>::const_iterator stepOpsIt = plan[i].begin();
00198 stepOpsIt != plan[i].end(); stepOpsIt++) {
00199 if(curTrace.isApplicable(stepOpsIt->op)) {
00200 FullPlanTrace nextTrace = curTrace.applyOperator(stepOpsIt->op);
00201 nextTraces.push_back(nextTrace);
00202
00203
00204
00205 while(nextTraces.back().canLetTimePass()) {
00206 nextTraces.push_back(nextTraces.back().letTimePass());
00207 }
00208 }
00209 }
00210 }
00211
00212
00213
00214 if(nextTraces.empty()) {
00215 for(deque<FullPlanTrace>::iterator it = currentTraces.begin(); it != currentTraces.end(); it++) {
00216 printf("Trace State was:\n");
00217 it->dumpLastState();
00218 }
00219 }
00220 currentTraces = nextTraces;
00221 if(currentTraces.empty()) {
00222 ROS_INFO("Step [% 6d]: Could not apply operator: \"%s\" to any state.", i, plan[i].front().op->get_name().c_str());
00223 return false;
00224 }
00225 ROS_DEBUG("Step [% 6d]: Applied operator: \"%s\"", i, plan[i].front().op->get_name().c_str());
00226 }
00227
00228
00229 deque<FullPlanTrace> final_traces;
00230 for(deque<FullPlanTrace>::iterator it = currentTraces.begin(); it != currentTraces.end(); it++) {
00231 FullPlanTrace finalTrace = *it;
00232 while(finalTrace.canLetTimePass())
00233 finalTrace = finalTrace.letTimePass();
00234 final_traces.push_back(finalTrace);
00235 }
00236
00237
00238 const FullPlanTrace* best = NULL;
00239 double bestMakespan = HUGE_VAL;
00240 bool ret = false;
00241 for(deque<FullPlanTrace>::iterator it = final_traces.begin(); it != final_traces.end(); it++) {
00242 const FullPlanTrace & t = *it;
00243 if(t.satisfiesGoal()) {
00244 ret = true;
00245 if(t.getLastTimestamp() < bestMakespan) {
00246 bestMakespan = t.getLastTimestamp();
00247 best = &t;
00248 }
00249 }
00250 }
00251
00252 if(!ret)
00253 ROS_INFO("Applied all operators but final plan doesn't fulfill goal.");
00254
00255 if(best != NULL) {
00256 stringstream os;
00257 best->outputPlan(os);
00258 ROS_INFO_STREAM("Monitored Plan:" << endl << os.str());
00259 if(!g_parameters.plan_name.empty()) {
00260 string monitorPlanName = g_parameters.plan_name + ".monitored";
00261 ofstream of(monitorPlanName.c_str());
00262 if(!of.good()) {
00263 ROS_ERROR("Could not open monitoring output at \"%s\"", monitorPlanName.c_str());
00264 } else {
00265 best->outputPlan(of);
00266 of.close();
00267 }
00268 }
00269 }
00270
00271 return ret;
00272 }
00273
00274
00275 bool MonitorEngine::validatePlanOld(const std::vector< std::vector<PlanStep> >& plan)
00276 {
00277 TimeStampedState current = *g_initial_state;
00278
00279 for (int i = 0; i < plan.size(); i++) {
00280 while (plan[i].front().start_time > current.timestamp) {
00281 double curren_time = current.timestamp;
00282 if (plan[i].front().start_time - 2 * EPS_TIME - EPSILON
00283 <= current.timestamp) {
00284 current.timestamp += EPS_TIME;
00285 } else {
00286 current = current.let_time_pass();
00287 }
00288 if (double_equals(current.timestamp, curren_time)) {
00289 current.timestamp += EPS_TIME;
00290 }
00291 }
00292 cout << "Current time_stamp: " << current.timestamp << endl;
00293 cout << "Next op: " << plan[i].front().op->get_name() << " ";
00294
00295
00296 if(!plan[i].front().op->is_applicable(current)) {
00297 cout << "is not applicable!" << endl;
00298 return false;
00299 } else {
00300 cout << "is applicable!" << endl;
00301 current = TimeStampedState(current,(*plan[i].front().op));
00302 }
00303 }
00304
00305 while (!current.operators.empty())
00306 current = current.let_time_pass();
00307
00308 return current.satisfies(g_goal);
00309 }
00310
00311
00312 FullPlanTrace::FullPlanTrace(const TimeStampedState & s)
00313 {
00314
00315 FullPlanStep fps(s, NULL, s);
00316 plan.push_back(fps);
00317 }
00318
00319 bool FullPlanTrace::isApplicable(const Operator* op) const
00320 {
00321 if(plan.empty())
00322 return false;
00323
00324 return op->is_applicable(plan.back().state, false);
00325 }
00326
00327 FullPlanTrace FullPlanTrace::applyOperator(const Operator* op) const
00328 {
00329
00330 FullPlanTrace ret = *this;
00331 if(ret.plan.empty())
00332 return ret;
00333
00334 FullPlanStep fps(plan.back().state, op, TimeStampedState(plan.back().state, *op));
00335 ret.plan.push_back(fps);
00336 return ret;
00337 }
00338
00339 bool FullPlanTrace::canLetTimePass() const
00340 {
00341 if(plan.empty())
00342 return false;
00343
00344 return !plan.back().state.operators.empty();
00345 }
00346
00347 FullPlanTrace FullPlanTrace::letTimePass() const
00348 {
00349
00350 FullPlanTrace ret = *this;
00351 if(ret.plan.empty())
00352 return ret;
00353
00354 ret.plan.back().state = ret.plan.back().state.let_time_pass();
00355 return ret;
00356 }
00357
00358 bool FullPlanTrace::satisfiesGoal() const
00359 {
00360 if(plan.empty())
00361 return false;
00362
00363 return plan.back().state.satisfies(g_goal);
00364 }
00365
00366 double FullPlanTrace::lastTimestamp() const
00367 {
00368 if(plan.empty())
00369 return -1;
00370 return plan.back().state.timestamp;
00371 }
00372
00373 void FullPlanTrace::outputPlan(ostream & os) const
00374 {
00375 if(plan.empty())
00376 return;
00377 os << fixed << setprecision(8);
00378 bool first = true;
00379 for(deque<FullPlanStep>::const_iterator it = plan.begin(); it != plan.end(); it++) {
00380 if(it->op == NULL) {
00381 ROS_ASSERT(first);
00382 first = false;
00383 continue;
00384 }
00385 first = false;
00386
00387 os << it->predecessor.timestamp << ": (" << it->op->get_name() << ") ["
00388 << it->op->get_duration(&(it->predecessor)) << "]" << endl;
00389 }
00390 }
00391
00392 void FullPlanTrace::dumpLastState() const
00393 {
00394 if(plan.empty()) {
00395 ROS_WARN("%s: plan is empty.", __PRETTY_FUNCTION__);
00396 return;
00397 }
00398 plan.back().state.dump(true);
00399 }
00400
00401 double FullPlanTrace::getLastTimestamp() const
00402 {
00403 if(plan.empty())
00404 return HUGE_VAL;
00405
00406 return plan.back().state.timestamp;
00407 }
00408