monitoring.cpp
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)   // empty line
00073             continue;
00074         if((plan[i])[0] == ';')   // comment
00075             continue;
00076         // start
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         // op
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         // duration
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         // lookup op
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                 // forward state? FIXME no state here, determined by next step
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     // sort the PlanSteps according to their start time (usually they are already).
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         // only output the first op per step, they should all have the same params
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     // seed with init.
00170     currentTraces.push_back(FullPlanTrace(*g_initial_state));
00171 
00172     for(int i = 0; i < plan.size(); i++) {
00173         //printf("CURRENT STATES ARE:\n");
00174         /*for(deque<FullPlanTrace>::iterator it = currentTraces.begin(); it != currentTraces.end(); it++) {
00175         // it->dumpLastState();
00176         }*/
00177 
00178         deque< FullPlanTrace > nextTraces;
00179 
00180         for(deque<FullPlanTrace>::iterator it = currentTraces.begin(); it != currentTraces.end(); it++) {
00181             //printf("Checking STATE:\n");
00182             //it->dumpLastState();
00183 
00184             // if applicable apply the operator to every state in the queue
00185             FullPlanTrace curTrace = *it;
00186 
00187             if(g_parameters.monitoring_verify_timestamps) {
00188                 // this plan trace should now be at the start time of plan[i]
00189                 double dt = fabs(plan[i].front().start_time - curTrace.lastTimestamp());
00190                 if(dt > EPS_TIME) {     // timestamp doesn't match -> stop this trace
00191                     continue;
00192                 }
00193             }
00194 
00195             // add all applicable versions of this operator
00196             // FIXME use this plan[i] stuff as ref, better
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                     // for every state in the queue produce as many versions of let_time_pass
00204                     // as possible and construct the next queue from those
00205                     while(nextTraces.back().canLetTimePass()) {
00206                         nextTraces.push_back(nextTraces.back().letTimePass());
00207                     }
00208                 }
00209             }
00210         }
00211 
00212         // No ops applicable in any trace, so output the current ones (in the state before the op)
00213         // to set what might have gone wrong.
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     // final step: Finalize all ops in queue by letting time pass to finish all ops
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     // final_traces contains no final state with running ops
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 // TODO check this and use as verify-timestamps version if we can produce the plan for output
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         // replace with correct function
00295         // FIXME: at this point the front() stuff is wrong
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     // pred s is only dummy
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     // copy
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();    // are there running ops in the last state
00345 }
00346 
00347 FullPlanTrace FullPlanTrace::letTimePass() const
00348 {
00349     // copy
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);      // op == NULL is OK for the first state (from seed) - we skip that.
00382             first = false;
00383             continue;
00384         }
00385         first = false;
00386         //printf("DV: %d\n", it->op->get_duration_var());
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 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines


tfd_modules
Author(s): Maintained by Christian Dornhege (see AUTHORS file).
autogenerated on Tue Jan 22 2013 12:25:03