37 #include "ompl/geometric/planners/rrt/LazyRRT.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/datastructures/NearestNeighborsSqrtApprox.h"
40 #include "ompl/tools/config/SelfConfig.h"
54 ompl::geometric::LazyRRT::~LazyRRT(
void)
77 lastGoalMotion_ = NULL;
84 std::vector<Motion*> motions;
86 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
88 if (motions[i]->state)
89 si_->freeState(motions[i]->state);
104 si_->copyState(motion->
state, st);
105 motion->
valid =
true;
109 if (nn_->size() == 0)
111 logError(
"There are no valid initial states!");
116 sampler_ = si_->allocStateSampler();
118 logInform(
"Starting with %u states", nn_->size());
121 double distsol = -1.0;
126 bool solutionFound =
false;
128 while (ptc() ==
false && !solutionFound)
131 if (goal_s && rng_.uniform01() < goalBias_ && goal_s->
canSample())
134 sampler_->sampleUniform(rstate);
137 Motion *nmotion = nn_->nearest(rmotion);
138 assert(nmotion != rmotion);
142 double d = si_->distance(nmotion->
state, rstate);
143 if (d > maxDistance_)
145 si_->getStateSpace()->interpolate(nmotion->
state, rstate, maxDistance_ / d, xstate);
151 si_->copyState(motion->
state, dstate);
153 nmotion->
children.push_back(motion);
161 solutionFound =
true;
163 lastGoalMotion_ = solution;
167 std::vector<Motion*> mpath;
168 while (solution != NULL)
170 mpath.push_back(solution);
171 solution = solution->parent;
175 for (
int i = mpath.size() - 1 ; i >= 0 && solutionFound; --i)
176 if (!mpath[i]->valid)
178 if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state))
179 mpath[i]->valid =
true;
182 removeMotion(mpath[i]);
183 solutionFound =
false;
191 for (
int i = mpath.size() - 1 ; i >= 0 ; --i)
192 path->
append(mpath[i]->state);
199 si_->freeState(xstate);
200 si_->freeState(rstate);
203 logInform(
"Created %u states", nn_->size());
216 for (
unsigned int i = 0 ; i < motion->
parent->
children.size() ; ++i)
225 for (
unsigned int i = 0 ; i < motion->
children.size() ; ++i)
232 si_->freeState(motion->
state);
238 Planner::getPlannerData(data);
240 std::vector<Motion*> motions;
246 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
248 if (motions[i]->parent == NULL)
254 data.
tagState(motions[i]->state, motions[i]->valid ? 1 : 0);