37 #include "ompl/geometric/planners/est/EST.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/tools/config/SelfConfig.h"
55 ompl::geometric::EST::~EST(
void)
67 tree_.grid.setDimension(projectionEvaluator_->getDimension());
78 lastGoalMotion_ = NULL;
85 for (
unsigned int i = 0 ; i < it->second->data.size() ; ++i)
87 if (it->second->data[i]->state)
88 si_->freeState(it->second->data[i]->state);
89 delete it->second->data[i];
103 si_->copyState(motion->
state, st);
107 if (tree_.grid.size() == 0)
109 logError(
"There are no valid initial states!");
114 sampler_ = si_->allocValidStateSampler();
116 logInform(
"Starting with %u states", tree_.size);
120 double approxdif = std::numeric_limits<double>::infinity();
123 while (ptc() ==
false)
126 Motion *existing = selectMotion();
130 if (goal_s && rng_.uniform01() < goalBias_ && goal_s->
canSample())
133 if (!sampler_->sampleNear(xstate, existing->
state, maxDistance_))
136 if (si_->checkMotion(existing->
state, xstate))
140 si_->copyState(motion->
state, xstate);
141 motion->
parent = existing;
152 if (dist < approxdif)
161 bool approximate =
false;
162 if (solution == NULL)
164 solution = approxsol;
168 if (solution != NULL)
170 lastGoalMotion_ = solution;
173 std::vector<Motion*> mpath;
174 while (solution != NULL)
176 mpath.push_back(solution);
177 solution = solution->
parent;
182 for (
int i = mpath.size() - 1 ; i >= 0 ; --i)
183 path->
append(mpath[i]->state);
184 pdef_->addSolutionPath(
base::PathPtr(path), approximate, approxdif);
188 si_->freeState(xstate);
190 logInform(
"Created %u states in %u cells", tree_.size, tree_.grid.size());
197 GridCell* cell = pdf_.sample(rng_.uniform01());
198 return cell && !cell->data.empty() ? cell->data[rng_.uniformInt(0, cell->data.size() - 1)] : NULL;
204 projectionEvaluator_->computeCoordinates(motion->
state, coord);
208 cell->
data.push_back(motion);
209 pdf_.update(cell->data.elem_, 1.0/cell->data.size());
213 cell = tree_.grid.createCell(coord);
214 cell->data.push_back(motion);
215 tree_.grid.add(cell);
216 cell->data.elem_ = pdf_.add(cell, 1.0);
223 Planner::getPlannerData(data);
225 std::vector<MotionInfo> motions;
226 tree_.grid.getContent(motions);
231 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
232 for (
unsigned int j = 0 ; j < motions[i].size() ; ++j)
234 if (motions[i][j]->parent == NULL)