38#include <ompl/base/SpaceInformation.h>
39#include <ompl/tools/benchmark/Benchmark.h>
49 std::vector<double> meanTime;
50 std::vector<std::string> plannerName;
51 std::map<double, std::pair<std::string, int>> plannerTimes;
53 for (
unsigned int k = 0; k < experiment.planners.size(); k++)
55 ot::Benchmark::PlannerExperiment pk = experiment.
planners.at(k);
56 std::vector<ot::Benchmark::RunProperties> runs = pk.runs;
58 unsigned int N = runs.size();
60 double percentSuccess = 0.0;
61 for (
unsigned int j = 0; j < N; j++)
63 ot::Benchmark::RunProperties run = runs.at(j);
64 double timeJrun = std::atof(run[
"time REAL"].c_str());
66 if (timeJrun < experiment.maxTime)
70 time = time / (double)N;
71 percentSuccess = 100.0 * (percentSuccess / (double)N);
74 plannerTimes[time] = std::make_pair(pk.name, percentSuccess);
77 std::cout <<
"Finished Benchmark (Runtime:" << experiment.maxTime <<
", RunCount:" << experiment.runCount <<
")"
79 std::cout <<
"Placement <Rank> <Time (in Seconds)> <Success (in Percentage)>" << std::endl;
81 std::cout << std::string(80,
'-') << std::endl;
82 for (
auto const &p : plannerTimes)
84 std::cout <<
"Place <" << ctr <<
"> Time: <" << p.first <<
"> \%Success: <" << p.second.second <<
"> ("
85 << p.second.first <<
")" << (ctr < 2 ?
" <-- Winner" :
"") << std::endl;
88 std::cout << std::string(80,
'-') << std::endl;
91void printEstimatedTimeToCompletion(
unsigned int numberPlanners,
unsigned int run_count,
unsigned int runtime_limit)
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 : ";
102 if (worst_case_time_estimate_in_hours < 1)
104 if (worst_case_time_estimate_in_minutes < 1)
106 std::cout << worst_case_time_estimate_in_seconds <<
"s" << std::endl;
110 std::cout << worst_case_time_estimate_in_minutes <<
"m" << std::endl;
115 std::cout << worst_case_time_estimate_in_hours <<
"h" << std::endl;
117 std::cout << std::string(80,
'-') << std::endl;
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.