62 auto fetch = std::make_shared<FetchRobot>();
69 auto planner = std::make_shared<OMPL::OMPLInterfacePlanner>(
fetch,
"default");
70 planner->initialize(
"package://robowflex_resources/fetch/config/ompl_planning.yaml");
73 auto request = std::make_shared<MotionRequestBuilder>(planner,
GROUP);
74 fetch->setGroupState(
GROUP, {0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0});
75 request->setStartConfiguration(
fetch->getScratchState());
77 fetch->setGroupState(
GROUP, {0.265, 0.501, 1.281, -2.272, 2.243, -2.774, 0.976, -2.007});
78 request->setGoalConfiguration(
fetch->getScratchState());
80 request->setConfig(
"RRTstar");
83 options.
metrics = Profiler::WAYPOINTS | Profiler::CORRECT | Profiler::LENGTH | Profiler::SMOOTHNESS;
89 auto &profiler = experiment.getProfiler();
93 experiment.addQuery(
"rrtstar",
scene, planner, request);
97 auto dataset = experiment.benchmark(1);
100 output.dump(*dataset);
A helper class for benchmarking that controls running multiple queries.
Benchmark outputter that saves results into OMPL benchmarking log files. If ompl_benchmark_statistics...
RAII-pattern for starting up ROS.
Profiler::ComputeMetricCallback getNumVerticesCallback()
Profiler::ComputeMetricCallback getGoalDistanceCallback()
static const std::string GROUP
Functions for loading and animating scenes in Blender.
uint32_t metrics
Bitmask of which metrics to compute after planning.