planner.cpp
Go to the documentation of this file.
00001 #include "best_first_search.h"
00002 #include "cyclic_cg_heuristic.h"
00003 #include "no_heuristic.h"
00004 #include "monitoring.h"
00005 
00006 #include "globals.h"
00007 #include "operator.h"
00008 #include "partial_order_lifter.h"
00009 
00010 #include <cstdlib>
00011 #include <cstring>
00012 #include <iostream>
00013 #include <fstream>
00014 #include <vector>
00015 #include <sstream>
00016 
00017 #include <cstdio>
00018 #include <math.h>
00019 #include "plannerParameters.h"
00020 #include "ros_printouts.h"
00021 
00022 #if ROS_BUILD
00023 #include <ros/ros.h>
00024 #endif
00025 
00026 #include "pddlModuleLoaderLDL.h"
00027 
00028 using namespace std;
00029 
00030 #include <sys/times.h>
00031 #include <sys/time.h>
00032 
00033 double save_plan(BestFirstSearchEngine& engine, double best_makespan, int &plan_number, string &plan_name);
00034 std::string getTimesName(const string & plan_name);    
00035 double getCurrentTime();            
00036 
00037 int main(int argc, char **argv)
00038 {
00039 #if ROS_BUILD
00040     ros::init(argc, argv, "tfd_modules", ros::init_options::NoSigintHandler);
00041     ros::NodeHandle nh;
00042 #endif
00043 
00044     ifstream file("../preprocess/output");
00045     if(strcmp(argv[argc - 1], "-eclipserun") == 0) {
00046         cin.rdbuf(file.rdbuf());
00047         cerr.rdbuf(cout.rdbuf());
00048         argc--;
00049     }
00050 
00051     struct tms start, search_start, search_end;
00052     times(&start);
00053     double start_walltime, search_start_walltime, search_end_walltime;
00054     start_walltime = getCurrentTime();
00055 
00056     if(!g_parameters.readParameters(argc, argv)) {
00057         cerr << "Error in reading parameters.\n";
00058         return 2;
00059     }
00060     g_parameters.dump();
00061 
00062     bool poly_time_method = false;
00063     cin >> poly_time_method;
00064     if(poly_time_method) {
00065         cout << "Poly-time method not implemented in this branch." << endl;
00066         cout << "Starting normal solver." << endl;
00067     }
00068 
00069     g_module_loader = new PDDLModuleLoaderLDL();
00070 
00071     read_everything(cin);
00072 
00073     g_let_time_pass = new Operator(false);
00074     g_wait_operator = new Operator(true);
00075 
00076     // init modules
00077     for (vector<InitModule*>::iterator it = g_init_modules.begin(); it
00078             != g_init_modules.end(); it++) {
00079         (*it)->execInit();
00080     }
00081 
00082     // init opl callback interface
00083     if (g_oplinit != NULL)
00084     {
00085         g_OplModuleCallback = g_oplinit->execInit(g_objectTypes, g_pred_mapping, g_func_mapping, g_pred_constants, g_func_constants);
00086         g_OplModuleCallback->setCurrentState(g_initial_state);
00087     }
00088 
00089     if(g_parameters.lazy_state_module_evaluation < 0) {
00090         if(g_condition_modules.empty() && g_cost_modules.empty())
00091             g_parameters.lazy_state_module_evaluation = 0;
00092         else
00093             g_parameters.lazy_state_module_evaluation = 1;
00094         cout << "Auto-Set Lazy State Module Evaluation to "
00095             << (g_parameters.lazy_state_module_evaluation ? "Enabled" : "Disabled") << endl;
00096     }
00097 
00098     FILE* timeDebugFile = NULL;
00099     if(!getTimesName(g_parameters.plan_name).empty()) {
00100         timeDebugFile = fopen(getTimesName(g_parameters.plan_name).c_str(), "w");
00101         if(!timeDebugFile) {
00102             cout << "WARNING: Could not open time debug file at: " << getTimesName(g_parameters.plan_name) << endl;
00103         } else {
00104             fprintf(timeDebugFile, "# Makespans for created plans and the time it took to create the plan\n");
00105             fprintf(timeDebugFile, "# The special makespan: -1 "
00106                     "indicates the total runtime and not the generation of a plan\n");
00107             fprintf(timeDebugFile, "# makespan search_time(s) total_time(s) search_walltime(s) total_walltime(s)\n");
00108             fflush(timeDebugFile);
00109         }
00110     }
00111 
00112 
00113     // Monitoring mode
00114     if (!g_parameters.planMonitorFileName.empty()) {
00115         bool ret = MonitorEngine::validatePlan(g_parameters.planMonitorFileName);
00116         ROS_INFO_STREAM("Monitoring: Plan is valid: " << ret);
00117         if(ret)
00118             exit(0);
00119         exit(1);
00120     }
00121 
00122     // Initialize search engine and heuristics
00123     BestFirstSearchEngine* engine = new BestFirstSearchEngine(g_parameters.queueManagementMode);
00124 
00125     if(g_parameters.makespan_heuristic || g_parameters.makespan_heuristic_preferred_operators)
00126         engine->add_heuristic(new CyclicCGHeuristic(CyclicCGHeuristic::SUFFIX_MAKESPAN),
00127             g_parameters.makespan_heuristic, g_parameters.makespan_heuristic_preferred_operators);
00128     if(g_parameters.cyclic_cg_heuristic || g_parameters.cyclic_cg_preferred_operators)
00129         engine->add_heuristic(new CyclicCGHeuristic(CyclicCGHeuristic::CEA), g_parameters.cyclic_cg_heuristic,
00130             g_parameters.cyclic_cg_preferred_operators);
00131     if(g_parameters.no_heuristic)
00132         engine->add_heuristic(new NoHeuristic, g_parameters.no_heuristic, false);
00133 
00134     double best_makespan = REALLYBIG;
00135     times(&search_start);
00136     search_start_walltime = getCurrentTime();
00137     int plan_number = 1;
00138 
00139     SearchEngine::status search_result = SearchEngine::IN_PROGRESS;
00140     while(true) {
00141         engine->initialize();
00142         search_result = engine->search();
00143 
00144         times(&search_end);
00145         search_end_walltime = getCurrentTime();
00146         if(engine->found_solution()) {
00147             if(search_result == SearchEngine::SOLVED) {
00148                 // FIXME only save_plan if return value is SOLVED, otherwise no new plan was found
00149                 best_makespan = save_plan(*engine, best_makespan, plan_number, g_parameters.plan_name);
00150                 // write plan length and search time to file
00151                 if(timeDebugFile && search_result == SearchEngine::SOLVED) {    // don't write info for timeout
00152                     int search_ms = (search_end.tms_utime - search_start.tms_utime) * 10;
00153                     int total_ms = (search_end.tms_utime - start.tms_utime) * 10;
00154                     double search_time = 0.001 * (double)search_ms;
00155                     double total_time = 0.001 * (double)total_ms;
00156                     double search_time_wall = search_end_walltime - search_start_walltime;
00157                     double total_time_wall = search_end_walltime - start_walltime;
00158 
00159                     fprintf(timeDebugFile, "%f %f %f %f %f\n", best_makespan, search_time, total_time, 
00160                             search_time_wall, total_time_wall);
00161                     fflush(timeDebugFile);
00162                 }
00163                 engine->bestMakespan = best_makespan;
00164             }
00165             // to continue searching we need to be in anytime search and the ret value is SOLVED
00166             // all other possibilities are either a timeout or completely explored search space
00167             if(g_parameters.anytime_search) {
00168                 if (search_result == SearchEngine::SOLVED) {
00169                     engine->fetch_next_state();
00170                 } else {
00171                     break;
00172                 }
00173             } else {
00174                 break;
00175             }
00176         } else {
00177             break;
00178         }
00179     }
00180 
00181     double search_time_wall = search_end_walltime - search_start_walltime;
00182     double total_time_wall = search_end_walltime - start_walltime;
00183 
00184     int search_ms = (search_end.tms_utime - search_start.tms_utime) * 10;
00185     int total_ms = (search_end.tms_utime - start.tms_utime) * 10;
00186     double search_time = 0.001 * (double)search_ms;
00187     double total_time = 0.001 * (double)total_ms;
00188     cout << "Search time: " << search_time << " seconds - Walltime: "
00189         << search_time_wall << " seconds" << endl;
00190     cout << "Total time: " << total_time << " seconds - Walltime: " 
00191         << total_time_wall << " seconds" << endl;
00192 
00193     if(timeDebugFile) {
00194         fprintf(timeDebugFile, "%f %f %f %f %f\n", -1.0, search_time, total_time, 
00195                 search_time_wall, total_time_wall);
00196         fclose(timeDebugFile);
00197     }
00198 
00199     switch(search_result) {
00200         case SearchEngine::SOLVED_TIMEOUT:
00201         case SearchEngine::FAILED_TIMEOUT:
00202             return 137;
00203         case SearchEngine::SOLVED:
00204         case SearchEngine::SOLVED_COMPLETE:
00205             return 0;
00206         case SearchEngine::FAILED:
00207             assert (!engine->found_at_least_one_solution());
00208             return 1;
00209         default:
00210             cerr << "Invalid Search return value: " << search_result << endl;
00211     }
00212 
00213     return 2;
00214 }
00215 
00217 
00225 bool epsilonize_plan(const std::string & filename, bool keepOriginalPlan = true)
00226 {
00227     // get a temp file name
00228     std::string orig_file = filename + ".orig";
00229 
00230     // first move filename to ...orig file, FIXME copy better?
00231     int retMove = rename(filename.c_str(), orig_file.c_str());
00232     if(retMove != 0) {
00233         cerr << __func__ << ": Error moving to orig file: " << orig_file << " from: " << filename << endl;
00234         return false;
00235     }
00236 
00237     // create the syscall to write the epsilonized plan
00238 #if ROS_BUILD
00239     std::string syscall = "rosrun tfd_modules epsilonize_plan.py";
00240 #else
00241     std::string syscall = "epsilonize_plan.py";
00242 #endif
00243     // read in the orig plan filename and output to filename
00244     syscall += " < " + orig_file + " > " + filename;  
00245 
00246     // execute epsilonize
00247     int ret = system(syscall.c_str());
00248     if(ret != 0) {
00249         cerr << __func__ << ": Error executing epsilonize_plan as: " << syscall << endl;
00250         // move back instead of copying/retaining .orig although not epsilonized
00251         int retMove = rename(orig_file.c_str(), filename.c_str());
00252         if(retMove != 0) {
00253             cerr << __func__ << ": Error moving orig file: " << orig_file << " back to: " << filename << endl;
00254         }
00255         return false;
00256     }
00257 
00258     if(!keepOriginalPlan) {
00259         remove(orig_file.c_str());  // don't care about return value, can't do anything if it fails
00260     }
00261 
00262     return true;
00263 }
00264 
00265 double save_plan(BestFirstSearchEngine& engine, double best_makespan, int &plan_number, string &plan_name)
00266 {
00267     const vector<PlanStep> &plan = engine.get_plan();
00268     const PlanTrace &path = engine.get_path();
00269 
00270     PartialOrderLifter partialOrderLifter(plan, path);
00271 
00272     Plan rescheduled_plan = plan;
00273     if(g_parameters.reschedule_plans)
00274         rescheduled_plan = partialOrderLifter.lift();
00275 
00276     handleSubplans(plan);
00277 
00278     double makespan = 0;
00279     for(int i = 0; i < rescheduled_plan.size(); i++) {
00280         double end_time = rescheduled_plan[i].start_time + rescheduled_plan[i].duration;
00281         makespan = max(makespan, end_time);
00282     }
00283     double original_makespan = 0;
00284     for (int i = 0; i < plan.size(); i++) {
00285         double end_time = plan[i].start_time + rescheduled_plan[i].duration;
00286         original_makespan = max(original_makespan, end_time);
00287     }
00288 
00289     if(g_parameters.use_subgoals_to_break_makespan_ties) {
00290         if(makespan > best_makespan)
00291             return best_makespan;
00292 
00293         if(double_equals(makespan, best_makespan)) {
00294             double sumOfSubgoals = getSumOfSubgoals(rescheduled_plan);
00295             if(sumOfSubgoals < engine.bestSumOfGoals) {
00296                 cout
00297                     << "Found plan of equal makespan but with faster solved subgoals."
00298                     << endl;
00299                 cout << "Sum of subgoals = " << sumOfSubgoals << endl;
00300                 engine.bestSumOfGoals = sumOfSubgoals;
00301             } else {
00302                 return best_makespan;
00303             }
00304         } else {
00305             assert(makespan < best_makespan);
00306             double sumOfSubgoals = getSumOfSubgoals(rescheduled_plan);
00307             engine.bestSumOfGoals = sumOfSubgoals;
00308         }
00309     } else {
00310         if(makespan >= best_makespan)
00311             return best_makespan;
00312     }
00313 
00314     cout << endl << "Found new plan:" << endl;
00315     for(int i = 0; i < plan.size(); i++) {
00316         const PlanStep& step = plan[i];
00317         printf("%.8f: (%s) [%.8f]\n", step.start_time, step.op->get_name().c_str(), step.duration);
00318     }
00319     if(g_parameters.reschedule_plans) {
00320         cout << "Rescheduled Plan:" << endl;
00321         for (int i = 0; i < rescheduled_plan.size(); i++) {
00322             const PlanStep& step = rescheduled_plan[i];
00323             printf("%.8f: (%s) [%.8f]\n", step.start_time, step.op->get_name().c_str(), step.duration);
00324         }
00325     }
00326     cout << "Solution with original makespan " << original_makespan
00327         << " found (ignoring no-moving-targets-rule)." << endl;
00328 
00329     // Determine filenames to write to
00330     FILE *file = 0;
00331     FILE *best_file = 0;
00332     std::string plan_filename;
00333     std::string best_plan_filename;
00334     if (plan_name != "-") {
00335         // Construct planNrStr to get 42 into "42".
00336         int nrLen = log10(plan_number) + 10;
00337         char* planNr = (char*)malloc(nrLen * sizeof(char));
00338         sprintf(planNr, "%d", plan_number);
00339         std::string planNrStr = planNr;
00340         free(planNr);
00341 
00342         // Construct filenames
00343         best_plan_filename = plan_name + ".best";
00344         if (g_parameters.anytime_search)
00345             plan_filename = plan_name + "." + planNrStr;
00346         else
00347             plan_filename = plan_name;
00348 
00349         // open files
00350         file = fopen(plan_filename.c_str(), "w");
00351         best_file = fopen(best_plan_filename.c_str(), "w");
00352 
00353         if(file == NULL) {
00354             fprintf(stderr, "%s:\n  Could not open plan file %s for writing.\n", 
00355                     __PRETTY_FUNCTION__, plan_filename.c_str());
00356             return makespan;
00357         }
00358     } else {
00359         file = stdout;
00360     }
00361 
00362     // Actually write the plan
00363     plan_number++;
00364     for(int i = 0; i < rescheduled_plan.size(); i++) {
00365         const PlanStep& step = rescheduled_plan[i];
00366         fprintf(file, "%.8f: (%s) [%.8f]\n", step.start_time, step.op->get_name().c_str(), step.duration);
00367         if (best_file) {
00368             fprintf(best_file, "%.8f: (%s) [%.8f]\n", 
00369                 step.start_time, step.op->get_name().c_str(), step.duration);
00370         }
00371     }
00372 
00373     if (plan_name != "-") {
00374         fclose(file);
00375         if(best_file)
00376             fclose(best_file);
00377     }
00378 
00379     cout << "Plan length: " << rescheduled_plan.size() << " step(s)." << endl;
00380     if(g_parameters.reschedule_plans)
00381         cout << "Rescheduled Makespan   : " << makespan << endl;
00382     else
00383         cout << "Makespan   : " << makespan << endl;
00384 
00385     if(g_parameters.epsilonize_externally) {
00386         // Perform epsilonize
00387         if(!plan_filename.empty()) {
00388             bool ret_of_epsilonize_plan = epsilonize_plan(plan_filename, g_parameters.keep_original_plans);
00389             if(!ret_of_epsilonize_plan)
00390                 cout << "Error while calling epsilonize plan! File: " << plan_filename << endl;
00391         }
00392         if(!best_plan_filename.empty()) {
00393             bool ret_of_epsilonize_best_plan = epsilonize_plan(best_plan_filename, g_parameters.keep_original_plans);
00394             if(!ret_of_epsilonize_best_plan)
00395                 cout << "Error while calling epsilonize best_plan! File: " << best_plan_filename << endl;
00396         }
00397     }
00398 
00399     return makespan;
00400 }
00401 
00402 std::string getTimesName(const string & plan_name)
00403 {
00404     if(plan_name.empty())
00405         return std::string();
00406     if(plan_name == "-")
00407         return std::string();
00408     return plan_name + ".times";
00409 }
00410 
00411 double getCurrentTime()
00412 {
00413     const double USEC_PER_SEC = 1000000;
00414 
00415     struct timeval tv; 
00416     gettimeofday(&tv, 0);
00417     return(tv.tv_sec + (double)tv.tv_usec / USEC_PER_SEC);
00418 }
00419 


tfd_modules
Author(s): Maintained by Christian Dornhege (see AUTHORS file).
autogenerated on Mon Oct 6 2014 07:52:06