QuotientSpacePlanningCommon.h
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2019, University of Stuttgart
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 University of Stuttgart nor the names
18 * of its contributors may be used to endorse or promote products
19 * derived from this software without specific prior written
20 * permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *********************************************************************/
35
36/* Author: Andreas Orthey */
37
38#include <ompl/base/SpaceInformation.h>
39#include <ompl/tools/benchmark/Benchmark.h>
40
41namespace ot = ompl::tools;
42namespace ob = ompl::base;
43namespace og = ompl::geometric;
44
45void printBenchmarkResults(const ot::Benchmark &b)
46{
47 ot::Benchmark::CompleteExperiment experiment = b.getRecordedExperimentData();
48
49 std::vector<double> meanTime;
50 std::vector<std::string> plannerName;
51 std::map<double, std::pair<std::string, int>> plannerTimes;
52
53 for (unsigned int k = 0; k < experiment.planners.size(); k++)
54 {
55 ot::Benchmark::PlannerExperiment pk = experiment.planners.at(k);
56 std::vector<ot::Benchmark::RunProperties> runs = pk.runs;
57
58 unsigned int N = runs.size();
59 double time = 0;
60 double percentSuccess = 0.0;
61 for (unsigned int j = 0; j < N; j++)
62 {
63 ot::Benchmark::RunProperties run = runs.at(j);
64 double timeJrun = std::atof(run["time REAL"].c_str());
65 time += timeJrun;
66 if (timeJrun < experiment.maxTime)
67 percentSuccess++;
68 }
69
70 time = time / (double)N;
71 percentSuccess = 100.0 * (percentSuccess / (double)N);
72 pk.name.erase(0, 10);
73
74 plannerTimes[time] = std::make_pair(pk.name, percentSuccess);
75 }
76
77 std::cout << "Finished Benchmark (Runtime:" << experiment.maxTime << ", RunCount:" << experiment.runCount << ")"
78 << std::endl;
79 std::cout << "Placement <Rank> <Time (in Seconds)> <Success (in Percentage)>" << std::endl;
80 unsigned int ctr = 1;
81 std::cout << std::string(80, '-') << std::endl;
82 for (auto const &p : plannerTimes)
83 {
84 std::cout << "Place <" << ctr << "> Time: <" << p.first << "> \%Success: <" << p.second.second << "> ("
85 << p.second.first << ")" << (ctr < 2 ? " <-- Winner" : "") << std::endl;
86 ctr++;
87 }
88 std::cout << std::string(80, '-') << std::endl;
89}
90
91void printEstimatedTimeToCompletion(unsigned int numberPlanners, unsigned int run_count, unsigned int runtime_limit)
92{
93 std::cout << std::string(80, '-') << std::endl;
94 double worst_case_time_estimate_in_seconds = numberPlanners * run_count * runtime_limit;
95 double worst_case_time_estimate_in_minutes = worst_case_time_estimate_in_seconds / 60.0;
96 double worst_case_time_estimate_in_hours = worst_case_time_estimate_in_minutes / 60.0;
97 std::cout << "Number of Planners : " << numberPlanners << std::endl;
98 std::cout << "Number of Runs Per Planner : " << run_count << std::endl;
99 std::cout << "Time Per Run (s) : " << runtime_limit << std::endl;
100 std::cout << "Worst-case time requirement : ";
101
102 if (worst_case_time_estimate_in_hours < 1)
103 {
104 if (worst_case_time_estimate_in_minutes < 1)
105 {
106 std::cout << worst_case_time_estimate_in_seconds << "s" << std::endl;
107 }
108 else
109 {
110 std::cout << worst_case_time_estimate_in_minutes << "m" << std::endl;
111 }
112 }
113 else
114 {
115 std::cout << worst_case_time_estimate_in_hours << "h" << std::endl;
116 }
117 std::cout << std::string(80, '-') << std::endl;
118}
Benchmark a set of planners on a problem instance.
Definition: Benchmark.h:49
const CompleteExperiment & getRecordedExperimentData() const
Return all the experiment data that would be written to the results file. The data should not be chan...
Definition: Benchmark.h:312
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
This namespace contains sampling based planning routines shared by both planning under geometric cons...
This namespace contains code that is specific to planning under geometric constraints.
Definition: GeneticSearch.h:48
Includes various tools such as self config, benchmarking, etc.
std::vector< PlannerExperiment > planners
The collected experimental data; each element of the array (an experiment) corresponds to a planner.
Definition: Benchmark.h:122