37 #include "ompl/geometric/planners/rrt/RRT.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/datastructures/NearestNeighborsGNAT.h"
40 #include "ompl/tools/config/SelfConfig.h"
56 ompl::geometric::RRT::~RRT(
void)
68 lastGoalMotion_ = NULL;
86 std::vector<Motion*> motions;
88 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
90 if (motions[i]->state)
91 si_->freeState(motions[i]->state);
106 si_->copyState(motion->
state, st);
110 if (nn_->size() == 0)
112 logError(
"There are no valid initial states!");
117 sampler_ = si_->allocStateSampler();
119 logInform(
"Starting with %u states", nn_->size());
123 double approxdif = std::numeric_limits<double>::infinity();
128 while (ptc() ==
false)
132 if (goal_s && rng_.uniform01() < goalBias_ && goal_s->
canSample())
135 sampler_->sampleUniform(rstate);
138 Motion *nmotion = nn_->nearest(rmotion);
142 double d = si_->distance(nmotion->
state, rstate);
143 if (d > maxDistance_)
145 si_->getStateSpace()->interpolate(nmotion->
state, rstate, maxDistance_ / d, xstate);
149 if (si_->checkMotion(nmotion->
state, dstate))
153 si_->copyState(motion->
state, dstate);
165 if (dist < approxdif)
174 bool approximate =
false;
175 if (solution == NULL)
177 solution = approxsol;
181 if (solution != NULL)
183 lastGoalMotion_ = solution;
186 std::vector<Motion*> mpath;
187 while (solution != NULL)
189 mpath.push_back(solution);
190 solution = solution->parent;
195 for (
int i = mpath.size() - 1 ; i >= 0 ; --i)
196 path->
append(mpath[i]->state);
197 pdef_->addSolutionPath(
base::PathPtr(path), approximate, approxdif);
201 si_->freeState(xstate);
203 si_->freeState(rmotion->
state);
206 logInform(
"Created %u states", nn_->size());
213 Planner::getPlannerData(data);
215 std::vector<Motion*> motions;
222 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
224 if (motions[i]->parent == NULL)