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), last_status_(base::PlannerStatus::UNKNOWN)
66 if (!configured_ || !si_->isSetup() || !planner_->isSetup())
76 OMPL_INFORM(
"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();
105 last_status_ = planner_->solve(time);
108 OMPL_INFORM(
"Solution found in %f seconds", planTime_);
110 OMPL_INFORM(
"No solution found after %f seconds", planTime_);
119 last_status_ = planner_->solve(ptc);
122 OMPL_INFORM(
"Solution found in %f seconds", planTime_);
124 OMPL_INFORM(
"No solution found after %f seconds", planTime_);
141 return haveSolutionPath() && (!pdef_->hasApproximateSolution() || pdef_->getSolutionDifference() < std::numeric_limits<double>::epsilon());
148 planner_->getPlannerData(pd);
155 si_->printProperties(out);
156 si_->printSettings(out);
160 planner_->printProperties(out);
161 planner_->printSettings(out);