37 #include "ompl/control/SimpleSetup.h"
38 #include "ompl/control/planners/rrt/RRT.h"
39 #include "ompl/control/planners/kpiece/KPIECE1.h"
45 throw Exception(
"Unable to allocate default planner for unspecified goal definition");
48 if (si->getStateSpace()->hasDefaultProjection())
57 configured_(false), planTime_(0.0), invalid_request_(false)
66 if (!configured_ || !si_->isSetup() || !planner_->isSetup())
76 logInform(
"No planner specified. Using default.");
80 planner_->setProblemDefinition(pdef_);
81 if (!planner_->isSetup())
85 params_.include(si_->params());
86 params_.include(planner_->params());
96 pdef_->clearSolutionPaths();
102 invalid_request_ =
false;
107 logInform(
"Solution found in %f seconds", planTime_);
110 if (planTime_ < time)
111 invalid_request_ =
true;
112 logInform(
"No solution found after %f seconds", planTime_);
120 invalid_request_ =
false;
125 logInform(
"Solution found in %f seconds", planTime_);
129 invalid_request_ =
true;
130 logInform(
"No solution found after %f seconds", planTime_);
148 return haveSolutionPath() && (!pdef_->hasApproximateSolution() || pdef_->getSolutionDifference() < std::numeric_limits<double>::epsilon());
155 planner_->getPlannerData(pd);
162 si_->printProperties(out);
163 si_->printSettings(out);
167 planner_->printProperties(out);
168 planner_->printSettings(out);