37 #include "ompl/geometric/SimpleSetup.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/geometric/planners/rrt/RRTConnect.h"
40 #include "ompl/geometric/planners/rrt/RRT.h"
41 #include "ompl/geometric/planners/kpiece/LBKPIECE1.h"
42 #include "ompl/geometric/planners/kpiece/KPIECE1.h"
48 throw Exception(
"Unable to allocate default planner for unspecified goal definition");
51 if (goal->hasType(base::GOAL_SAMPLEABLE_REGION))
54 if (goal->getSpaceInformation()->getStateSpace()->hasDefaultProjection())
63 if (goal->getSpaceInformation()->getStateSpace()->hasDefaultProjection())
70 throw Exception(
"Unable to allocate default planner");
76 configured_(false), planTime_(0.0), simplifyTime_(0.0), invalid_request_(false)
86 if (!configured_ || !si_->isSetup() || !planner_->isSetup())
96 logInform(
"No planner specified. Using default.");
100 planner_->setProblemDefinition(pdef_);
101 if (!planner_->isSetup())
105 params_.include(si_->params());
106 params_.include(planner_->params());
116 pdef_->clearSolutionPaths();
122 invalid_request_ =
false;
127 logInform(
"Solution found in %f seconds", planTime_);
130 if (planTime_ < time)
131 invalid_request_ =
true;
132 logInform(
"No solution found after %f seconds", planTime_);
140 invalid_request_ =
false;
145 logInform(
"Solution found in %f seconds", planTime_);
149 invalid_request_ =
true;
150 logInform(
"No solution found after %f seconds", planTime_);
163 psk_->simplify(static_cast<PathGeometric&>(*p), ptc);
165 logInform(
"Path simplification took %f seconds", simplifyTime_);
169 logWarn(
"No solution to simplify");
180 if (duration < std::numeric_limits<double>::epsilon())
181 psk_->simplifyMax(static_cast<PathGeometric&>(*p));
183 psk_->simplify(static_cast<PathGeometric&>(*p), duration);
185 logInform(
"Path simplification took %f seconds", simplifyTime_);
189 logWarn(
"No solution to simplify");
205 return haveSolutionPath() && (!pdef_->hasApproximateSolution() || pdef_->getSolutionDifference() < std::numeric_limits<double>::epsilon());
212 planner_->getPlannerData(pd);
219 si_->printProperties(out);
220 si_->printSettings(out);
224 planner_->printProperties(out);
225 planner_->printSettings(out);