Robowflex  v0.1
Making MoveIt Easy
fetch_ompl_benchmark.cpp File Reference

Go to the source code of this file.

Functions

Profiler::ComputeMetricCallback getNumVerticesCallback ()
 
Profiler::ComputeMetricCallback getGoalDistanceCallback ()
 
int main (int argc, char **argv)
 

Variables

static const std::string GROUP = "arm_with_torso"
 

Function Documentation

◆ getGoalDistanceCallback()

Profiler::ComputeMetricCallback getGoalDistanceCallback ( )

Definition at line 34 of file fetch_ompl_benchmark.cpp.

35 {
36  return [](const PlannerPtr &planner, //
37  const SceneConstPtr &scene, //
38  const planning_interface::MotionPlanRequest &request, //
39  const PlanData &run) -> PlannerMetric {
40  const auto &ompl_planner = std::dynamic_pointer_cast<const OMPL::OMPLInterfacePlanner>(planner);
41 
42  const auto &pdef = ompl_planner->getLastSimpleSetup()->getProblemDefinition();
43  double distance = pdef->getSolutionDifference();
44 
45  if (distance == -1)
46  {
47  const auto &start = pdef->getStartState(0);
48  const auto &goal = std::dynamic_pointer_cast<ompl::base::GoalRegion>(pdef->getGoal());
49  distance = goal->distanceGoal(start);
50  }
51 
52  return distance;
53  };
54 }
Detailed statistics and metrics computed from profiling a planner's motion planning.
Definition: benchmarking.h:79
A shared pointer wrapper for robowflex::Planner.
A const shared pointer wrapper for robowflex::Scene.
T distance(T... args)
moveit_msgs::MotionPlanRequest MotionPlanRequest
boost::variant< bool, double, int, std::size_t, std::string > PlannerMetric
Variant type of possible values a run metric could be.
Definition: benchmarking.h:33

◆ getNumVerticesCallback()

Profiler::ComputeMetricCallback getNumVerticesCallback ( )

Definition at line 18 of file fetch_ompl_benchmark.cpp.

19 {
20  return [](const PlannerPtr &planner, //
21  const SceneConstPtr &scene, //
22  const planning_interface::MotionPlanRequest &request, //
23  const PlanData &run) -> PlannerMetric {
24  const auto &ompl_planner = std::dynamic_pointer_cast<const OMPL::OMPLInterfacePlanner>(planner);
25  const auto &op = ompl_planner->getLastSimpleSetup()->getPlanner();
26 
27  ompl::base::PlannerData pd(op->getSpaceInformation());
28  op->getPlannerData(pd);
29 
30  return (int)pd.numVertices();
31  };
32 }

◆ main()

int main ( int  argc,
char **  argv 
)

Definition at line 56 of file fetch_ompl_benchmark.cpp.

57 {
58  // Startup ROS
59  ROS ros(argc, argv);
60 
61  // Create the default Fetch robot.
62  auto fetch = std::make_shared<FetchRobot>();
63  fetch->initialize();
64 
65  // Create an empty scene.
66  auto scene = std::make_shared<Scene>(fetch);
67 
68  // Create the default planner for the Fetch.
69  auto planner = std::make_shared<OMPL::OMPLInterfacePlanner>(fetch, "default");
70  planner->initialize("package://robowflex_resources/fetch/config/ompl_planning.yaml");
71 
72  // Create a motion planning request with a pose goal.
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}); // Stow
75  request->setStartConfiguration(fetch->getScratchState());
76 
77  fetch->setGroupState(GROUP, {0.265, 0.501, 1.281, -2.272, 2.243, -2.774, 0.976, -2.007}); // Unfurl
78  request->setGoalConfiguration(fetch->getScratchState());
79 
80  request->setConfig("RRTstar");
81 
82  Profiler::Options options;
83  options.metrics = Profiler::WAYPOINTS | Profiler::CORRECT | Profiler::LENGTH | Profiler::SMOOTHNESS;
84  Experiment experiment("unfurl", // Name of experiment
85  options, // Options for internal profiler
86  10.0, // Timeout allowed for ALL queries
87  5); // Number of trials
88 
89  auto &profiler = experiment.getProfiler();
90  profiler.addMetricCallback("goal_distance", getGoalDistanceCallback());
91  profiler.addMetricCallback("num_vertices", getNumVerticesCallback());
92 
93  experiment.addQuery("rrtstar", scene, planner, request);
94 
95  // Note: Only 1 thread can be used when profiling the OMPL planners, as planning contexts under the hood
96  // are reused between queries.
97  auto dataset = experiment.benchmark(1);
98 
99  OMPLPlanDataSetOutputter output("robowflex_fetch_ompl");
100  output.dump(*dataset);
101 
102  return 0;
103 }
A helper class for benchmarking that controls running multiple queries.
Definition: benchmarking.h:349
Benchmark outputter that saves results into OMPL benchmarking log files. If ompl_benchmark_statistics...
Definition: benchmarking.h:572
RAII-pattern for starting up ROS.
Definition: util.h:52
Profiler::ComputeMetricCallback getNumVerticesCallback()
Profiler::ComputeMetricCallback getGoalDistanceCallback()
static const std::string GROUP
Functions for loading and animating scenes in Blender.
Options for profiling.
Definition: benchmarking.h:220
uint32_t metrics
Bitmask of which metrics to compute after planning.
Definition: benchmarking.h:221

Variable Documentation

◆ GROUP

const std::string GROUP = "arm_with_torso"
static

Definition at line 16 of file fetch_ompl_benchmark.cpp.