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
00077 for (vector<InitModule*>::iterator it = g_init_modules.begin(); it
00078 != g_init_modules.end(); it++) {
00079 (*it)->execInit();
00080 }
00081
00082
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
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
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
00149 best_makespan = save_plan(*engine, best_makespan, plan_number, g_parameters.plan_name);
00150
00151 if(timeDebugFile && search_result == SearchEngine::SOLVED) {
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
00166
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
00228 std::string orig_file = filename + ".orig";
00229
00230
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
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
00244 syscall += " < " + orig_file + " > " + filename;
00245
00246
00247 int ret = system(syscall.c_str());
00248 if(ret != 0) {
00249 cerr << __func__ << ": Error executing epsilonize_plan as: " << syscall << endl;
00250
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());
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
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
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
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
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
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
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