Loading...
Searching...
No Matches
Benchmark.cpp
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2010, Rice University
5* All rights reserved.
6*
7* Redistribution and use in source and binary forms, with or without
8* modification, are permitted provided that the following conditions
9* are met:
10*
11* * Redistributions of source code must retain the above copyright
12* notice, this list of conditions and the following disclaimer.
13* * Redistributions in binary form must reproduce the above
14* copyright notice, this list of conditions and the following
15* disclaimer in the documentation and/or other materials provided
16* with the distribution.
17* * Neither the name of the Rice University nor the names of its
18* contributors may be used to endorse or promote products derived
19* from this software without specific prior written permission.
20*
21* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32* POSSIBILITY OF SUCH DAMAGE.
33*********************************************************************/
34
35/* Author: Ioan Sucan, Luis G. Torres */
36
37#include "ompl/tools/benchmark/Benchmark.h"
38#include "ompl/tools/benchmark/MachineSpecs.h"
39#include "ompl/util/Time.h"
40#include "ompl/config.h"
41#include "ompl/util/String.h"
42#include <boost/scoped_ptr.hpp>
43#include <thread>
44#include <mutex>
45#include <condition_variable>
46#include <fstream>
47#include <sstream>
48
50namespace ompl
51{
52 namespace tools
53 {
56 static std::string getResultsFilename(const Benchmark::CompleteExperiment &exp)
57 {
58 return "ompl_" + exp.host + "_" + time::as_string(exp.startTime) + ".log";
59 }
60
63 static std::string getConsoleFilename(const Benchmark::CompleteExperiment &exp)
64 {
65 return "ompl_" + exp.host + "_" + time::as_string(exp.startTime) + ".console";
66 }
67
68 static bool terminationCondition(const machine::MemUsage_t maxMem, const time::point &endTime)
69 {
70 if (time::now() < endTime && machine::getProcessMemoryUsage() < maxMem)
71 return false;
72 return true;
73 }
74
75 class RunPlanner
76 {
77 public:
78 RunPlanner(const Benchmark *benchmark)
79 : benchmark_(benchmark), timeUsed_(0.0), memUsed_(0)
80 {
81 }
82
83 void run(const base::PlannerPtr &planner, const machine::MemUsage_t memStart,
84 const machine::MemUsage_t maxMem, const double maxTime, const double timeBetweenUpdates)
85 {
86 runThread(planner, memStart + maxMem, time::seconds(maxTime), time::seconds(timeBetweenUpdates));
87 }
88
89 double getTimeUsed() const
90 {
91 return timeUsed_;
92 }
93
94 machine::MemUsage_t getMemUsed() const
95 {
96 return memUsed_;
97 }
98
99 base::PlannerStatus getStatus() const
100 {
101 return status_;
102 }
103
104 const Benchmark::RunProgressData &getRunProgressData() const
105 {
106 return runProgressData_;
107 }
108
109 private:
110 void runThread(const base::PlannerPtr &planner, const machine::MemUsage_t maxMem,
111 const time::duration &maxDuration, const time::duration &timeBetweenUpdates)
112 {
113 time::point timeStart = time::now();
114
115 try
116 {
117 const time::point endtime = time::now() + maxDuration;
118 base::PlannerTerminationConditionFn ptc([maxMem, endtime]
119 {
120 return terminationCondition(maxMem, endtime);
121 });
122 solved_ = false;
123 // Only launch the planner progress property
124 // collector if there is any data for it to report
125 //
126 // \todo issue here is that at least one sample
127 // always gets taken before planner even starts;
128 // might be worth adding a short wait time before
129 // collector begins sampling
130 boost::scoped_ptr<std::thread> t;
131 if (planner->getPlannerProgressProperties().size() > 0)
132 t.reset(new std::thread([this, &planner, timeBetweenUpdates]
133 {
134 collectProgressProperties(planner->getPlannerProgressProperties(),
135 timeBetweenUpdates);
136 }));
137 status_ = planner->solve(ptc, 0.1);
138 solvedFlag_.lock();
139 solved_ = true;
140 solvedCondition_.notify_all();
141 solvedFlag_.unlock();
142 if (t)
143 t->join(); // maybe look into interrupting even if planner throws an exception
144 }
145 catch (std::runtime_error &e)
146 {
147 std::stringstream es;
148 es << "There was an error executing planner " << benchmark_->getStatus().activePlanner
149 << ", run = " << benchmark_->getStatus().activeRun << std::endl;
150 es << "*** " << e.what() << std::endl;
151 std::cerr << es.str();
152 OMPL_ERROR(es.str().c_str());
153 }
154
155 timeUsed_ = time::seconds(time::now() - timeStart);
156 memUsed_ = machine::getProcessMemoryUsage();
157 }
158
159 void collectProgressProperties(const base::Planner::PlannerProgressProperties &properties,
160 const time::duration &timePerUpdate)
161 {
162 time::point timeStart = time::now();
163
164 std::unique_lock<std::mutex> ulock(solvedFlag_);
165 while (!solved_)
166 {
167 if (solvedCondition_.wait_for(ulock, timePerUpdate) == std::cv_status::no_timeout)
168 return;
169 else
170 {
171 double timeInSeconds = time::seconds(time::now() - timeStart);
172 std::string timeStamp = ompl::toString(timeInSeconds);
173 std::map<std::string, std::string> data;
174 data["time REAL"] = timeStamp;
175 for (const auto &property : properties)
176 {
177 data[property.first] = property.second();
178 }
179 runProgressData_.push_back(data);
180 }
181 }
182 }
183
184 const Benchmark *benchmark_;
185 double timeUsed_;
186 machine::MemUsage_t memUsed_;
187 base::PlannerStatus status_;
188 Benchmark::RunProgressData runProgressData_;
189
190 // variables needed for progress property collection
191 bool solved_;
192 std::mutex solvedFlag_;
193 std::condition_variable solvedCondition_;
194 };
195 }
196}
198
199bool ompl::tools::Benchmark::saveResultsToFile(const char *filename) const
200{
201 bool result = false;
202
203 std::ofstream fout(filename);
204 if (fout.good())
205 {
206 result = saveResultsToStream(fout);
207 OMPL_INFORM("Results saved to '%s'", filename);
208 }
209 else
210 {
211 // try to save to a different file, if we can
212 if (getResultsFilename(exp_) != std::string(filename))
213 result = saveResultsToFile();
214
215 OMPL_ERROR("Unable to write results to '%s'", filename);
216 }
217 return result;
218}
219
221{
222 std::string filename = getResultsFilename(exp_);
223 return saveResultsToFile(filename.c_str());
224}
225
227{
228 if (exp_.planners.empty())
229 {
230 OMPL_WARN("There is no experimental data to save");
231 return false;
232 }
233
234 if (!out.good())
235 {
236 OMPL_ERROR("Unable to write to stream");
237 return false;
238 }
239
240 out << "OMPL version " << OMPL_VERSION << std::endl;
241 out << "Experiment " << (exp_.name.empty() ? "NO_NAME" : exp_.name) << std::endl;
242
243 out << exp_.parameters.size() << " experiment properties" << std::endl;
244 for (const auto &parameter : exp_.parameters)
245 out << parameter.first << " = " << parameter.second << std::endl;
246
247 out << "Running on " << (exp_.host.empty() ? "UNKNOWN" : exp_.host) << std::endl;
248 out << "Starting at " << time::as_string(exp_.startTime) << std::endl;
249 out << "<<<|" << std::endl << exp_.setupInfo << "|>>>" << std::endl;
250 out << "<<<|" << std::endl << exp_.cpuInfo << "|>>>" << std::endl;
251
252 out << exp_.seed << " is the random seed" << std::endl;
253 out << exp_.maxTime << " seconds per run" << std::endl;
254 out << exp_.maxMem << " MB per run" << std::endl;
255 out << exp_.runCount << " runs per planner" << std::endl;
256 out << exp_.totalDuration << " seconds spent to collect the data" << std::endl;
257
258 // change this if more enum types are added
259 out << "1 enum type" << std::endl;
260 out << "status";
261 for (unsigned int i = 0; i < base::PlannerStatus::TYPE_COUNT; ++i)
262 out << '|' << base::PlannerStatus(static_cast<base::PlannerStatus::StatusType>(i)).asString();
263 out << std::endl;
264
265 out << exp_.planners.size() << " planners" << std::endl;
266
267 for (const auto &planner : exp_.planners)
268 {
269 out << planner.name << std::endl;
270
271 // get names of common properties
272 std::vector<std::string> properties;
273 for (auto &property : planner.common)
274 properties.push_back(property.first);
275 std::sort(properties.begin(), properties.end());
276
277 // print names & values of common properties
278 out << properties.size() << " common properties" << std::endl;
279 for (auto &property : properties)
280 {
281 auto it = planner.common.find(property);
282 out << it->first << " = " << it->second << std::endl;
283 }
284
285 // construct the list of all possible properties for all runs
286 std::map<std::string, bool> propSeen;
287 for (auto &run : planner.runs)
288 for (auto &property : run)
289 propSeen[property.first] = true;
290
291 properties.clear();
292
293 for (auto &it : propSeen)
294 properties.push_back(it.first);
295 std::sort(properties.begin(), properties.end());
296
297 // print the property names
298 out << properties.size() << " properties for each run" << std::endl;
299 for (auto &property : properties)
300 out << property << std::endl;
301
302 // print the data for each run
303 out << planner.runs.size() << " runs" << std::endl;
304 for (auto &run : planner.runs)
305 {
306 for (auto &property : properties)
307 {
308 auto it = run.find(property);
309 if (it != run.end())
310 out << it->second;
311 out << "; ";
312 }
313 out << std::endl;
314 }
315
316 // print the run progress data if it was reported
317 if (planner.runsProgressData.size() > 0)
318 {
319 // Print number of progress properties
320 out << planner.progressPropertyNames.size() << " progress properties for each run" << std::endl;
321 // Print progress property names
322 for (const auto &progPropName : planner.progressPropertyNames)
323 {
324 out << progPropName << std::endl;
325 }
326 // Print progress properties for each run
327 out << planner.runsProgressData.size() << " runs" << std::endl;
328 for (const auto &r : planner.runsProgressData)
329 {
330 // For each time point
331 for (const auto &t : r)
332 {
333 // Print each of the properties at that time point
334 for (const auto &iter : t)
335 {
336 out << iter.second << ",";
337 }
338
339 // Separate time points by semicolons
340 out << ";";
341 }
342
343 // Separate runs by newlines
344 out << std::endl;
345 }
346 }
347
348 out << '.' << std::endl;
349 }
350 return true;
351}
352
354{
355 // sanity checks
356 if (gsetup_)
357 {
358 if (!gsetup_->getSpaceInformation()->isSetup())
359 gsetup_->getSpaceInformation()->setup();
360 }
361 else
362 {
363 if (!csetup_->getSpaceInformation()->isSetup())
364 csetup_->getSpaceInformation()->setup();
365 }
366
367 if (!(gsetup_ ? gsetup_->getGoal() : csetup_->getGoal()))
368 {
369 OMPL_ERROR("No goal defined");
370 return;
371 }
372
373 if (planners_.empty())
374 {
375 OMPL_ERROR("There are no planners to benchmark");
376 return;
377 }
378
379 status_.running = true;
380 exp_.totalDuration = 0.0;
381 exp_.maxTime = req.maxTime;
382 exp_.maxMem = req.maxMem;
383 exp_.runCount = req.runCount;
384 exp_.host = machine::getHostname();
385 exp_.cpuInfo = machine::getCPUInfo();
386 exp_.seed = RNG::getSeed();
387
388 exp_.startTime = time::now();
389
390 OMPL_INFORM("Configuring planners ...");
391
392 // clear previous experimental data
393 exp_.planners.clear();
394 exp_.planners.resize(planners_.size());
395
396 const base::ProblemDefinitionPtr &pdef =
397 gsetup_ ? gsetup_->getProblemDefinition() : csetup_->getProblemDefinition();
398 // set up all the planners
399 for (unsigned int i = 0; i < planners_.size(); ++i)
400 {
401 // configure the planner
402 planners_[i]->setProblemDefinition(pdef);
403 if (!planners_[i]->isSetup())
404 planners_[i]->setup();
405 exp_.planners[i].name = (gsetup_ ? "geometric_" : "control_") + planners_[i]->getName();
406 OMPL_INFORM("Configured %s", exp_.planners[i].name.c_str());
407 }
408
409 OMPL_INFORM("Done configuring planners.");
410 OMPL_INFORM("Saving planner setup information ...");
411
412 std::stringstream setupInfo;
413 if (gsetup_)
414 gsetup_->print(setupInfo);
415 else
416 csetup_->print(setupInfo);
417 setupInfo << std::endl << "Properties of benchmarked planners:" << std::endl;
418 for (auto &planner : planners_)
419 planner->printProperties(setupInfo);
420
421 exp_.setupInfo = setupInfo.str();
422
423 OMPL_INFORM("Done saving information");
424
425 OMPL_INFORM("Beginning benchmark");
427 boost::scoped_ptr<msg::OutputHandlerFile> ohf;
428 if (req.saveConsoleOutput)
429 {
430 ohf.reset(new msg::OutputHandlerFile(getConsoleFilename(exp_).c_str()));
431 msg::useOutputHandler(ohf.get());
432 }
433 else
435 OMPL_INFORM("Beginning benchmark");
436
437 boost::scoped_ptr<ompl::time::ProgressDisplay> progress;
438 if (req.displayProgress)
439 {
440 std::cout << "Running experiment " << exp_.name << "." << std::endl;
441 if (req.runCount)
442 std::cout << "Each planner will be executed " << req.runCount << " times for at most " << req.maxTime << " seconds.";
443 else
444 std::cout << "Each planner will be executed as many times as possible within " << req.maxTime << " seconds.";
445 std::cout << " Memory is limited at " << req.maxMem << "MB." << std::endl;
446 progress.reset(new ompl::time::ProgressDisplay);
447 }
448
450 auto maxMemBytes = (machine::MemUsage_t)(req.maxMem * 1024 * 1024);
451
452 for (unsigned int i = 0; i < planners_.size(); ++i)
453 {
454 status_.activePlanner = exp_.planners[i].name;
455 // execute planner switch event, if set
456 try
457 {
458 if (plannerSwitch_)
459 {
460 OMPL_INFORM("Executing planner-switch event for planner %s ...", status_.activePlanner.c_str());
462 OMPL_INFORM("Completed execution of planner-switch event");
463 }
464 }
465 catch (std::runtime_error &e)
466 {
467 std::stringstream es;
468 es << "There was an error executing the planner-switch event for planner " << status_.activePlanner
469 << std::endl;
470 es << "*** " << e.what() << std::endl;
471 std::cerr << es.str();
472 OMPL_ERROR(es.str().c_str());
473 }
474 if (gsetup_)
475 gsetup_->setup();
476 else
477 csetup_->setup();
478 planners_[i]->params().getParams(exp_.planners[i].common);
479 planners_[i]->getSpaceInformation()->params().getParams(exp_.planners[i].common);
480
481 // Add planner progress property names to struct
482 exp_.planners[i].progressPropertyNames.emplace_back("time REAL");
483 for (const auto &property : planners_[i]->getPlannerProgressProperties())
484 {
485 exp_.planners[i].progressPropertyNames.push_back(property.first);
486 }
487 std::sort(exp_.planners[i].progressPropertyNames.begin(), exp_.planners[i].progressPropertyNames.end());
488
489 // run the planner
490 double maxTime = req.maxTime;
491 unsigned int j = 0;
492 while (true)
493 {
494 status_.activeRun = j;
495 status_.progressPercentage = req.runCount ?
496 (double)(100 * (req.runCount * i + j)) / (double)(planners_.size() * req.runCount) :
497 (double)(100 * i) / (double)(planners_.size());
498
499 if (req.displayProgress)
500 while (status_.progressPercentage > progress->count())
501 ++(*progress);
502
503 OMPL_INFORM("Preparing for run %d of %s", status_.activeRun, status_.activePlanner.c_str());
504
505 // make sure all planning data structures are cleared
506 try
507 {
508 planners_[i]->clear();
509 if (gsetup_)
510 {
511 gsetup_->getProblemDefinition()->clearSolutionPaths();
512 gsetup_->getSpaceInformation()->getMotionValidator()->resetMotionCounter();
513 }
514 else
515 {
516 csetup_->getProblemDefinition()->clearSolutionPaths();
517 csetup_->getSpaceInformation()->getMotionValidator()->resetMotionCounter();
518 }
519 }
520 catch (std::runtime_error &e)
521 {
522 std::stringstream es;
523 es << "There was an error while preparing for run " << status_.activeRun << " of planner "
524 << status_.activePlanner << std::endl;
525 es << "*** " << e.what() << std::endl;
526 std::cerr << es.str();
527 OMPL_ERROR(es.str().c_str());
528 }
529
530 // execute pre-run event, if set
531 try
532 {
533 if (preRun_)
534 {
535 OMPL_INFORM("Executing pre-run event for run %d of planner %s ...", status_.activeRun,
536 status_.activePlanner.c_str());
537 preRun_(planners_[i]);
538 OMPL_INFORM("Completed execution of pre-run event");
539 }
540 }
541 catch (std::runtime_error &e)
542 {
543 std::stringstream es;
544 es << "There was an error executing the pre-run event for run " << status_.activeRun << " of planner "
545 << status_.activePlanner << std::endl;
546 es << "*** " << e.what() << std::endl;
547 std::cerr << es.str();
548 OMPL_ERROR(es.str().c_str());
549 }
550
551 RunPlanner rp(this);
552 rp.run(planners_[i], memStart, maxMemBytes, maxTime, req.timeBetweenUpdates);
553 bool solved = gsetup_ ? gsetup_->haveSolutionPath() : csetup_->haveSolutionPath();
554
555 // store results
556 try
557 {
558 RunProperties run;
559
560 run["time REAL"] = ompl::toString(rp.getTimeUsed());
561 run["memory REAL"] = ompl::toString((double)rp.getMemUsed() / (1024.0 * 1024.0));
562 run["status ENUM"] = std::to_string((int)static_cast<base::PlannerStatus::StatusType>(rp.getStatus()));
563 if (gsetup_)
564 {
565 run["solved BOOLEAN"] = std::to_string(gsetup_->haveExactSolutionPath());
566 run["valid segment fraction REAL"] =
567 ompl::toString(gsetup_->getSpaceInformation()->getMotionValidator()->getValidMotionFraction());
568 }
569 else
570 {
571 run["solved BOOLEAN"] = std::to_string(csetup_->haveExactSolutionPath());
572 run["valid segment fraction REAL"] =
573 ompl::toString(csetup_->getSpaceInformation()->getMotionValidator()->getValidMotionFraction());
574 }
575
576 if (solved)
577 {
578 if (gsetup_)
579 {
580 run["approximate solution BOOLEAN"] =
581 std::to_string(gsetup_->getProblemDefinition()->hasApproximateSolution());
582 run["solution difference REAL"] =
583 ompl::toString(gsetup_->getProblemDefinition()->getSolutionDifference());
584 run["solution length REAL"] = ompl::toString(gsetup_->getSolutionPath().length());
585 run["solution smoothness REAL"] = ompl::toString(gsetup_->getSolutionPath().smoothness());
586 run["solution clearance REAL"] = ompl::toString(gsetup_->getSolutionPath().clearance());
587 run["solution segments INTEGER"] =
588 std::to_string(gsetup_->getSolutionPath().getStateCount() - 1);
589 run["correct solution BOOLEAN"] = std::to_string(gsetup_->getSolutionPath().check());
590
591 unsigned int factor = gsetup_->getStateSpace()->getValidSegmentCountFactor();
592 gsetup_->getStateSpace()->setValidSegmentCountFactor(factor * 4);
593 run["correct solution strict BOOLEAN"] = std::to_string(gsetup_->getSolutionPath().check());
594 gsetup_->getStateSpace()->setValidSegmentCountFactor(factor);
595
596 if (req.simplify)
597 {
598 // simplify solution
599 time::point timeStart = time::now();
600 gsetup_->simplifySolution();
601 double timeUsed = time::seconds(time::now() - timeStart);
602 run["simplification time REAL"] = ompl::toString(timeUsed);
603 run["simplified solution length REAL"] =
604 ompl::toString(gsetup_->getSolutionPath().length());
605 run["simplified solution smoothness REAL"] =
606 ompl::toString(gsetup_->getSolutionPath().smoothness());
607 run["simplified solution clearance REAL"] =
608 ompl::toString(gsetup_->getSolutionPath().clearance());
609 run["simplified solution segments INTEGER"] =
610 std::to_string(gsetup_->getSolutionPath().getStateCount() - 1);
611 run["simplified correct solution BOOLEAN"] =
612 std::to_string(gsetup_->getSolutionPath().check());
613 gsetup_->getStateSpace()->setValidSegmentCountFactor(factor * 4);
614 run["simplified correct solution strict BOOLEAN"] =
615 std::to_string(gsetup_->getSolutionPath().check());
616 gsetup_->getStateSpace()->setValidSegmentCountFactor(factor);
617 }
618 }
619 else
620 {
621 run["approximate solution BOOLEAN"] =
622 std::to_string(csetup_->getProblemDefinition()->hasApproximateSolution());
623 run["solution difference REAL"] =
624 ompl::toString(csetup_->getProblemDefinition()->getSolutionDifference());
625 run["solution length REAL"] = ompl::toString(csetup_->getSolutionPath().length());
626 run["solution clearance REAL"] =
627 ompl::toString(csetup_->getSolutionPath().asGeometric().clearance());
628 run["solution segments INTEGER"] = std::to_string(csetup_->getSolutionPath().getControlCount());
629 run["correct solution BOOLEAN"] = std::to_string(csetup_->getSolutionPath().check());
630 }
631 }
632
633 base::PlannerData pd(gsetup_ ? gsetup_->getSpaceInformation() : csetup_->getSpaceInformation());
634 planners_[i]->getPlannerData(pd);
635 run["graph states INTEGER"] = std::to_string(pd.numVertices());
636 run["graph motions INTEGER"] = std::to_string(pd.numEdges());
637
638 for (const auto &prop : pd.properties)
639 run[prop.first] = prop.second;
640
641 // execute post-run event, if set
642 try
643 {
644 if (postRun_)
645 {
646 OMPL_INFORM("Executing post-run event for run %d of planner %s ...", status_.activeRun,
647 status_.activePlanner.c_str());
648 postRun_(planners_[i], run);
649 OMPL_INFORM("Completed execution of post-run event");
650 }
651 }
652 catch (std::runtime_error &e)
653 {
654 std::stringstream es;
655 es << "There was an error in the execution of the post-run event for run " << status_.activeRun
656 << " of planner " << status_.activePlanner << std::endl;
657 es << "*** " << e.what() << std::endl;
658 std::cerr << es.str();
659 OMPL_ERROR(es.str().c_str());
660 }
661
662 exp_.planners[i].runs.push_back(run);
663
664 // Add planner progress data from the planner progress
665 // collector if there was anything to report
666 if (planners_[i]->getPlannerProgressProperties().size() > 0)
667 {
668 exp_.planners[i].runsProgressData.push_back(rp.getRunProgressData());
669 }
670 }
671 catch (std::runtime_error &e)
672 {
673 std::stringstream es;
674 es << "There was an error in the extraction of planner results: planner = " << status_.activePlanner
675 << ", run = " << status_.activePlanner << std::endl;
676 es << "*** " << e.what() << std::endl;
677 std::cerr << es.str();
678 OMPL_ERROR(es.str().c_str());
679 }
680
681 ++j;
682 if (req.runCount == 0)
683 {
684 maxTime -= rp.getTimeUsed();
685 if (maxTime < 0.)
686 break;
687 }
688 else
689 {
690 if (j >= req.runCount)
691 break;
692 }
693 }
694 planners_[i]->clear();
695 }
696
697 status_.running = false;
698 status_.progressPercentage = 100.0;
699 if (req.displayProgress)
700 {
701 while (status_.progressPercentage > progress->count())
702 ++(*progress);
703 std::cout << std::endl;
704 }
705
706 exp_.totalDuration = time::seconds(time::now() - exp_.startTime);
707
708 OMPL_INFORM("Benchmark complete");
710 OMPL_INFORM("Benchmark complete");
711}
static std::uint_fast32_t getSeed()
Get the seed used to generate the seeds of each RNG instance. Passing the returned value to setSeed()...
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
unsigned int numEdges() const
Retrieve the number of edges in this structure.
unsigned int numVertices() const
Retrieve the number of vertices in this structure.
std::map< std::string, std::string > properties
Any extra properties (key-value pairs) the planner can set.
Implementation of OutputHandler that saves messages in a file.
Definition Console.h:126
Generic class to handle output from a piece of code.
Definition Console.h:103
PreSetupEvent plannerSwitch_
Event to be called when the evaluated planner is switched.
Definition Benchmark.h:344
Status status_
The current status of this benchmarking instance.
Definition Benchmark.h:341
virtual void benchmark(const Request &req)
Benchmark the added planners on the defined problem. Repeated calls clear previously gathered data.
std::vector< base::PlannerPtr > planners_
The set of planners to be tested.
Definition Benchmark.h:335
PostSetupEvent postRun_
Event to be called after the run of a planner.
Definition Benchmark.h:350
CompleteExperiment exp_
The collected experimental data (for all planners)
Definition Benchmark.h:338
control::SimpleSetup * csetup_
The instance of the problem to benchmark (if planning with controls)
Definition Benchmark.h:332
geometric::SimpleSetup * gsetup_
The instance of the problem to benchmark (if geometric planning)
Definition Benchmark.h:329
bool saveResultsToFile() const
Save the results of the benchmark to a file. The name of the file is the current date and time.
virtual bool saveResultsToStream(std::ostream &out=std::cout) const
Save the results of the benchmark to a stream.
PreSetupEvent preRun_
Event to be called before the run of a planner.
Definition Benchmark.h:347
bool saveResultsToFile(const char *filename) const
Save the results of the benchmark to a file.
std::map< std::string, std::string > RunProperties
The data collected from a run of a planner is stored as key-value pairs.
Definition Benchmark.h:79
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition Console.h:68
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition Console.h:64
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition Console.h:66
MemUsage_t getProcessMemoryUsage()
Get the amount of memory the current process is using. This should work on major platforms (Windows,...
std::string getHostname()
Get the hostname of the machine in use.
std::string getCPUInfo()
Get information about the CPU of the machine in use.
unsigned long long MemUsage_t
Amount of memory used, in bytes.
OutputHandler * getOutputHandler()
Get the instance of the OutputHandler currently used. This is nullptr in case there is no output hand...
Definition Console.cpp:115
void useOutputHandler(OutputHandler *oh)
Specify the instance of the OutputHandler to use. By default, this is OutputHandlerSTD.
Definition Console.cpp:108
void noOutputHandler()
This function instructs ompl that no messages should be outputted. Equivalent to useOutputHandler(nul...
Definition Console.cpp:95
std::chrono::system_clock::time_point point
Representation of a point in time.
Definition Time.h:52
point now()
Get the current time point.
Definition Time.h:58
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition Time.h:64
std::string as_string(const point &p)
Return string representation of point in time.
Definition Time.h:78
Includes various tools such as self config, benchmarking, etc.
Main namespace. Contains everything in this library.
std::string toString(float val)
convert float to string using classic "C" locale semantics
Definition String.cpp:82
A class to store the exit status of Planner::solve()
StatusType
The possible values of the status returned by a planner.
@ TYPE_COUNT
The number of possible status values.
std::string asString() const
Return a string representation.
This structure holds experimental data for a set of planners.
Definition Benchmark.h:117
Representation of a benchmark request.
Definition Benchmark.h:157
unsigned int runCount
the number of times to run each planner; 100 by default If set to 0, then run each planner as many ti...
Definition Benchmark.h:180
bool displayProgress
flag indicating whether progress is to be displayed or not; true by default
Definition Benchmark.h:187
double maxMem
the maximum amount of memory a planner is allowed to use (MB); 4096.0 by default
Definition Benchmark.h:176
double timeBetweenUpdates
When collecting time-varying data from a planner during its execution, the planner's progress will be...
Definition Benchmark.h:184
bool saveConsoleOutput
flag indicating whether console output is saved (in an automatically generated filename); true by def...
Definition Benchmark.h:191
double maxTime
the maximum amount of time a planner is allowed to run (seconds); 5.0 by default
Definition Benchmark.h:173
bool simplify
flag indicating whether simplification should be applied to path; true by default
Definition Benchmark.h:194