37 #include "ompl/geometric/planners/rrt/LazyRRT.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/tools/config/SelfConfig.h"
53 ompl::geometric::LazyRRT::~LazyRRT(
void)
65 nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(si_->getStateSpace()));
76 lastGoalMotion_ = NULL;
83 std::vector<Motion*> motions;
85 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
87 if (motions[i]->state)
88 si_->freeState(motions[i]->state);
103 si_->copyState(motion->
state, st);
104 motion->
valid =
true;
108 if (nn_->size() == 0)
110 OMPL_ERROR(
"There are no valid initial states!");
115 sampler_ = si_->allocStateSampler();
117 OMPL_INFORM(
"Starting with %u states", nn_->size());
120 double distsol = -1.0;
125 bool solutionFound =
false;
127 while (ptc ==
false && !solutionFound)
130 if (goal_s && rng_.uniform01() < goalBias_ && goal_s->
canSample())
133 sampler_->sampleUniform(rstate);
136 Motion *nmotion = nn_->nearest(rmotion);
137 assert(nmotion != rmotion);
141 double d = si_->distance(nmotion->
state, rstate);
142 if (d > maxDistance_)
144 si_->getStateSpace()->interpolate(nmotion->
state, rstate, maxDistance_ / d, xstate);
150 si_->copyState(motion->
state, dstate);
152 nmotion->
children.push_back(motion);
160 solutionFound =
true;
161 lastGoalMotion_ = solution;
165 std::vector<Motion*> mpath;
166 while (solution != NULL)
168 mpath.push_back(solution);
169 solution = solution->parent;
173 for (
int i = mpath.size() - 1 ; i >= 0 && solutionFound; --i)
174 if (!mpath[i]->valid)
176 if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state))
177 mpath[i]->valid =
true;
180 removeMotion(mpath[i]);
181 solutionFound =
false;
182 lastGoalMotion_ = NULL;
190 for (
int i = mpath.size() - 1 ; i >= 0 ; --i)
191 path->
append(mpath[i]->state);
198 si_->freeState(xstate);
199 si_->freeState(rstate);
215 for (
unsigned int i = 0 ; i < motion->
parent->
children.size() ; ++i)
224 for (
unsigned int i = 0 ; i < motion->
children.size() ; ++i)
231 si_->freeState(motion->
state);
237 Planner::getPlannerData(data);
239 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);