00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "ompl/benchmark/Benchmark.h"
00038 #include "ompl/util/Time.h"
00039 #include "ompl/util/Memory.h"
00040 #include <boost/lexical_cast.hpp>
00041 #include <boost/progress.hpp>
00042 #include <fstream>
00043 #include <sstream>
00044
00045 #if defined _WIN32
00046 # include <winsock2.h>
00047 #else
00048 # include <unistd.h>
00049 #endif
00050
00051 namespace ompl
00052 {
00053
00054 static std::string getHostname(void)
00055 {
00056 char buffer[1024];
00057 int len = gethostname(buffer, sizeof(buffer));
00058 if (len != 0)
00059 return std::string();
00060 else
00061 return std::string(buffer);
00062 }
00063 }
00064
00065 std::string ompl::Benchmark::getResultsFilename(void) const
00066 {
00067 return "ompl_" + exp_.host + "_" + boost::posix_time::to_iso_extended_string(exp_.startTime) + ".log";
00068 }
00069
00070 bool ompl::Benchmark::saveResultsToFile(const char *filename) const
00071 {
00072 bool result = false;
00073
00074 std::ofstream fout(filename);
00075 if (fout.good())
00076 {
00077 result = saveResultsToStream(fout);
00078 msg_.inform("Results saved to '%s'", filename);
00079 }
00080 else
00081 {
00082
00083 if (getResultsFilename() != std::string(filename))
00084 result = saveResultsToFile();
00085
00086 msg_.error("Unable to write results to '%s'", filename);
00087 }
00088 return result;
00089 }
00090
00091 bool ompl::Benchmark::saveResultsToFile(void) const
00092 {
00093 std::string filename = getResultsFilename();
00094 return saveResultsToFile(filename.c_str());
00095 }
00096
00097 bool ompl::Benchmark::saveResultsToStream(std::ostream &out) const
00098 {
00099 if (exp_.planners.empty())
00100 {
00101 msg_.warn("There is no experimental data to save");
00102 return false;
00103 }
00104
00105 if (!out.good())
00106 {
00107 msg_.error("Unable to write to stream");
00108 return false;
00109 }
00110
00111 out << "Experiment " << (exp_.name.empty() ? "NO_NAME" : exp_.name) << std::endl;
00112 out << "Running on " << (exp_.host.empty() ? "UNKNOWN" : exp_.host) << std::endl;
00113 out << "Starting at " << boost::posix_time::to_iso_extended_string(exp_.startTime) << std::endl;
00114 out << "<<<|" << std::endl << exp_.setupInfo << "|>>>" << std::endl;
00115
00116 out << exp_.seed << " is the random seed" << std::endl;
00117 out << exp_.maxTime << " seconds per run" << std::endl;
00118 out << exp_.maxMem << " MB per run" << std::endl;
00119 out << exp_.totalDuration << " seconds spent to collect the data" << std::endl;
00120 out << exp_.planners.size() << " planners" << std::endl;
00121
00122 for (unsigned int i = 0 ; i < exp_.planners.size() ; ++i)
00123 {
00124 out << exp_.planners[i].name << std::endl;
00125
00126
00127 std::vector<std::string> properties;
00128 for (std::map<std::string, std::string>::const_iterator mit = exp_.planners[i].common.begin() ;
00129 mit != exp_.planners[i].common.end() ; ++mit)
00130 properties.push_back(mit->first);
00131 std::sort(properties.begin(), properties.end());
00132
00133
00134 out << properties.size() << " common properties" << std::endl;
00135 for (unsigned int k = 0 ; k < properties.size() ; ++k)
00136 {
00137 std::map<std::string, std::string>::const_iterator it = exp_.planners[i].common.find(properties[k]);
00138 out << it->first << " = " << it->second << std::endl;
00139 }
00140
00141
00142 std::map<std::string, bool> propSeen;
00143 for (unsigned int j = 0 ; j < exp_.planners[i].runs.size() ; ++j)
00144 for (std::map<std::string, std::string>::const_iterator mit = exp_.planners[i].runs[j].begin() ;
00145 mit != exp_.planners[i].runs[j].end() ; ++mit)
00146 propSeen[mit->first] = true;
00147
00148 properties.clear();
00149
00150 for (std::map<std::string, bool>::iterator it = propSeen.begin() ; it != propSeen.end() ; ++it)
00151 properties.push_back(it->first);
00152 std::sort(properties.begin(), properties.end());
00153
00154
00155 out << properties.size() << " properties for each run" << std::endl;
00156 for (unsigned int j = 0 ; j < properties.size() ; ++j)
00157 out << properties[j] << std::endl;
00158
00159
00160 out << exp_.planners[i].runs.size() << " runs" << std::endl;
00161 for (unsigned int j = 0 ; j < exp_.planners[i].runs.size() ; ++j)
00162 {
00163 for (unsigned int k = 0 ; k < properties.size() ; ++k)
00164 {
00165 std::map<std::string, std::string>::const_iterator it = exp_.planners[i].runs[j].find(properties[k]);
00166 if (it != exp_.planners[i].runs[j].end())
00167 out << it->second;
00168 out << "; ";
00169 }
00170 out << std::endl;
00171 }
00172
00173 out << '.' << std::endl;
00174 }
00175 return true;
00176 }
00177
00178 namespace ompl
00179 {
00180
00181 static bool terminationCondition(MemUsage_t maxMem, const time::point &endTime)
00182 {
00183 if (time::now() < endTime && getProcessMemoryUsage() < maxMem)
00184 return false;
00185 return true;
00186 }
00187
00188 }
00189
00190 void ompl::Benchmark::benchmark(double maxTime, double maxMem, unsigned int runCount, bool displayProgress)
00191 {
00192
00193 if (gsetup_)
00194 gsetup_->setup();
00195 else
00196 csetup_->setup();
00197
00198 if (!(gsetup_ ? gsetup_->getGoal() : csetup_->getGoal()))
00199 {
00200 msg_.error("No goal defined");
00201 return;
00202 }
00203
00204 if (planners_.empty())
00205 {
00206 msg_.error("There are no planners to benchmark");
00207 return;
00208 }
00209
00210 status_.running = true;
00211 exp_.totalDuration = 0.0;
00212 exp_.maxTime = maxTime;
00213 exp_.maxMem = maxMem;
00214 exp_.host = getHostname();
00215 exp_.seed = RNG::getSeed();
00216
00217 exp_.startTime = time::now();
00218
00219
00220 exp_.planners.clear();
00221 exp_.planners.resize(planners_.size());
00222
00223 const base::ProblemDefinitionPtr &pdef = gsetup_ ? gsetup_->getProblemDefinition() : csetup_->getProblemDefinition();
00224
00225 for (unsigned int i = 0 ; i < planners_.size() ; ++i)
00226 {
00227
00228 planners_[i]->setProblemDefinition(pdef);
00229 if (!planners_[i]->isSetup())
00230 planners_[i]->setup();
00231 exp_.planners[i].name = (gsetup_ ? "geometric_" : "control_") + planners_[i]->getName();
00232 }
00233
00234 std::stringstream setupInfo;
00235 if (gsetup_)
00236 gsetup_->print(setupInfo);
00237 else
00238 csetup_->print(setupInfo);
00239 exp_.setupInfo = setupInfo.str();
00240
00241 msg_.inform("Beginning benchmark");
00242
00243 msg::OutputHandler *oh = msg::getOutputHandler();
00244 msg::noOutputHandler();
00245
00246 boost::shared_ptr<boost::progress_display> progress;
00247 if (displayProgress)
00248 {
00249 std::cout << "Running experiment " << exp_.name << std::endl;
00250 progress.reset(new boost::progress_display(100, std::cout));
00251 }
00252
00253 MemUsage_t memStart = getProcessMemoryUsage();
00254
00255 for (unsigned int i = 0 ; i < planners_.size() ; ++i)
00256 {
00257 status_.activePlanner = exp_.planners[i].name;
00258
00259
00260 for (unsigned int j = 0 ; j < runCount ; ++j)
00261 {
00262 status_.activeRun = j;
00263 status_.progressPercentage = (double)(100 * (runCount * i + j)) / (double)(planners_.size() * runCount);
00264
00265 if (displayProgress)
00266 while (status_.progressPercentage > progress->count())
00267 ++(*progress);
00268
00269
00270 planners_[i]->clear();
00271 if (gsetup_)
00272 {
00273 gsetup_->getGoal()->clearSolutionPath();
00274 gsetup_->getSpaceInformation()->getMotionValidator()->resetMotionCounter();
00275 }
00276 else
00277 {
00278 csetup_->getGoal()->clearSolutionPath();
00279 csetup_->getSpaceInformation()->getMotionValidator()->resetMotionCounter();
00280 }
00281
00282 time::point timeStart = time::now();
00283
00284 bool solved = false;
00285
00286 try
00287 {
00288 solved = planners_[i]->solve(boost::bind(&terminationCondition,
00289 memStart + (MemUsage_t)(maxMem * 1024 * 1024),
00290 time::now() + time::seconds(maxTime)), 0.1);
00291 }
00292 catch(std::runtime_error &e)
00293 {
00294 std::cerr << std::endl << "There was an error executing planner " << status_.activePlanner << ", run = " << j << std::endl;
00295 std::cerr << "*** " << e.what() << std::endl;
00296 }
00297
00298 double timeUsed = time::seconds(time::now() - timeStart);
00299 MemUsage_t memUsed = getProcessMemoryUsage();
00300 if (memStart < memUsed)
00301 memUsed -= memStart;
00302 else
00303 memUsed = 0;
00304
00305
00306 try
00307 {
00308 RunProperties run;
00309 run["solved BOOLEAN"] = boost::lexical_cast<std::string>(solved);
00310 run["time REAL"] = boost::lexical_cast<std::string>(timeUsed);
00311 run["memory REAL"] = boost::lexical_cast<std::string>((double)memUsed / (1024.0 * 1024.0));
00312 if (gsetup_)
00313 {
00314 run["valid segment fraction REAL"] = boost::lexical_cast<std::string>(gsetup_->getSpaceInformation()->getMotionValidator()->getValidMotionFraction());
00315 }
00316 else
00317 {
00318 run["valid segment fraction REAL"] = boost::lexical_cast<std::string>(csetup_->getSpaceInformation()->getMotionValidator()->getValidMotionFraction());
00319 }
00320
00321 if (solved)
00322 {
00323 if (gsetup_)
00324 {
00325 run["approximate solution BOOLEAN"] = boost::lexical_cast<std::string>(gsetup_->getGoal()->isApproximate());
00326 run["solution difference REAL"] = boost::lexical_cast<std::string>(gsetup_->getGoal()->getDifference());
00327 run["solution length REAL"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().length());
00328 run["solution smoothness REAL"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().smoothness());
00329 run["solution clearance REAL"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().clearance());
00330 run["solution segments INTEGER"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().states.size() - 1);
00331 run["correct solution BOOLEAN"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().check());
00332
00333 unsigned int factor = gsetup_->getStateManifold()->getValidSegmentCountFactor();
00334 gsetup_->getStateManifold()->setValidSegmentCountFactor(factor * 4);
00335 run["correct solution strict BOOLEAN"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().check());
00336 gsetup_->getStateManifold()->setValidSegmentCountFactor(factor);
00337
00338
00339 timeStart = time::now();
00340 gsetup_->simplifySolution();
00341 timeUsed = time::seconds(time::now() - timeStart);
00342 run["simplification time REAL"] = boost::lexical_cast<std::string>(timeUsed);
00343 run["simplified solution length REAL"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().length());
00344 run["simplified solution smoothness REAL"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().smoothness());
00345 run["simplified solution clearance REAL"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().clearance());
00346 run["simplified solution segments INTEGER"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().states.size() - 1);
00347 run["simplified correct solution BOOLEAN"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().check());
00348 gsetup_->getStateManifold()->setValidSegmentCountFactor(factor * 4);
00349 run["simplified correct solution strict BOOLEAN"] = boost::lexical_cast<std::string>(gsetup_->getSolutionPath().check());
00350 gsetup_->getStateManifold()->setValidSegmentCountFactor(factor);
00351 }
00352 else
00353 {
00354 run["approximate solution BOOLEAN"] = boost::lexical_cast<std::string>(csetup_->getGoal()->isApproximate());
00355 run["solution difference REAL"] = boost::lexical_cast<std::string>(csetup_->getGoal()->getDifference());
00356 run["solution length REAL"] = boost::lexical_cast<std::string>(csetup_->getSolutionPath().length());
00357 run["solution clearance REAL"] = boost::lexical_cast<std::string>(csetup_->getSolutionPath().asGeometric().clearance());
00358 run["solution segments INTEGER"] = boost::lexical_cast<std::string>(csetup_->getSolutionPath().states.size() - 1);
00359 run["correct solution BOOLEAN"] = boost::lexical_cast<std::string>(csetup_->getSolutionPath().check());
00360 }
00361 }
00362
00363 base::PlannerData pd;
00364 planners_[i]->getPlannerData(pd);
00365 run["graph states INTEGER"] = boost::lexical_cast<std::string>(pd.states.size());
00366 unsigned long edges = 0;
00367 for (unsigned int k = 0 ; k < pd.edges.size() ; ++k)
00368 edges += pd.edges[k].size();
00369 run["graph motions INTEGER"] = boost::lexical_cast<std::string>(edges);
00370
00371 for (std::map<std::string, std::string>::const_iterator it = pd.properties.begin() ; it != pd.properties.end() ; ++it)
00372 run[it->first] = it->second;
00373
00374 exp_.planners[i].runs.push_back(run);
00375 }
00376 catch(std::runtime_error &e)
00377 {
00378 std::cerr << std::endl << "There was an error in the extraction of planner results: planner = " << status_.activePlanner << ", run = " << j << std::endl;
00379 std::cerr << "*** " << e.what() << std::endl;
00380 }
00381 }
00382 }
00383
00384 status_.running = false;
00385 status_.progressPercentage = 100.0;
00386 if (displayProgress)
00387 {
00388 while (status_.progressPercentage > progress->count())
00389 ++(*progress);
00390 std::cout << std::endl;
00391 }
00392
00393 exp_.totalDuration = time::seconds(time::now() - exp_.startTime);
00394 msg::useOutputHandler(oh);
00395
00396 msg_.inform("Benchmark complete");
00397 }