37 #include "ompl/geometric/planners/rrt/RRT.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/tools/config/SelfConfig.h"
55 ompl::geometric::RRT::~RRT(
void)
67 lastGoalMotion_ = NULL;
77 nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(si_->getStateSpace()));
85 std::vector<Motion*> motions;
87 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
89 if (motions[i]->state)
90 si_->freeState(motions[i]->state);
105 si_->copyState(motion->
state, st);
109 if (nn_->size() == 0)
111 OMPL_ERROR(
"There are no valid initial states!");
116 sampler_ = si_->allocStateSampler();
118 OMPL_INFORM(
"Starting with %u states", nn_->size());
122 double approxdif = std::numeric_limits<double>::infinity();
131 if (goal_s && rng_.uniform01() < goalBias_ && goal_s->
canSample())
134 sampler_->sampleUniform(rstate);
137 Motion *nmotion = nn_->nearest(rmotion);
141 double d = si_->distance(nmotion->
state, rstate);
142 if (d > maxDistance_)
144 si_->getStateSpace()->interpolate(nmotion->
state, rstate, maxDistance_ / d, xstate);
148 if (si_->checkMotion(nmotion->
state, dstate))
152 si_->copyState(motion->
state, dstate);
164 if (dist < approxdif)
173 bool approximate =
false;
174 if (solution == NULL)
176 solution = approxsol;
180 if (solution != NULL)
182 lastGoalMotion_ = solution;
185 std::vector<Motion*> mpath;
186 while (solution != NULL)
188 mpath.push_back(solution);
189 solution = solution->parent;
194 for (
int i = mpath.size() - 1 ; i >= 0 ; --i)
195 path->
append(mpath[i]->state);
196 pdef_->addSolutionPath(
base::PathPtr(path), approximate, approxdif);
200 si_->freeState(xstate);
202 si_->freeState(rmotion->
state);
212 Planner::getPlannerData(data);
214 std::vector<Motion*> motions;
221 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
223 if (motions[i]->parent == NULL)