37 #include "ompl/geometric/planners/rrt/pRRT.h"
38 #include "ompl/datastructures/NearestNeighborsGNAT.h"
39 #include "ompl/base/goals/GoalSampleableRegion.h"
40 #include "ompl/tools/config/SelfConfig.h"
41 #include <boost/thread/thread.hpp>
44 ompl::geometric::pRRT::pRRT(
const base::SpaceInformationPtr &si) : base::Planner(si,
"pRRT"),
58 Planner::declareParam<unsigned int>(
"thread_count",
this, &
pRRT::setThreadCount, &pRRT::getThreadCount);
61 ompl::geometric::pRRT::~pRRT(
void)
74 nn_->setDistanceFunction(boost::bind(&pRRT::distanceFunction,
this, _1, _2));
80 samplerArray_.clear();
84 lastGoalMotion_ = NULL;
87 void ompl::geometric::pRRT::freeMemory(
void)
91 std::vector<Motion*> motions;
93 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
95 if (motions[i]->state)
96 si_->freeState(motions[i]->state);
102 void ompl::geometric::pRRT::threadSolve(
unsigned int tid,
const base::PlannerTerminationCondition &ptc, SolutionInfo *sol)
105 base::Goal *goal = pdef_->getGoal().get();
106 base::GoalSampleableRegion *goal_s =
dynamic_cast<base::GoalSampleableRegion*
>(goal);
109 Motion *rmotion =
new Motion(si_);
110 base::State *rstate = rmotion->state;
111 base::State *xstate = si_->allocState();
113 while (sol->solution == NULL && ptc() ==
false)
116 if (goal_s && rng.uniform01() < goalBias_ && goal_s->canSample())
117 goal_s->sampleGoal(rstate);
119 samplerArray_[tid]->sampleUniform(rstate);
123 Motion *nmotion = nn_->nearest(rmotion);
125 base::State *dstate = rstate;
128 double d = si_->distance(nmotion->state, rstate);
129 if (d > maxDistance_)
131 si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
135 if (si_->checkMotion(nmotion->state, dstate))
138 Motion *motion =
new Motion(si_);
139 si_->copyState(motion->state, dstate);
140 motion->parent = nmotion;
147 bool solved = goal->isSatisfied(motion->state, &dist);
151 sol->approxdif = dist;
152 sol->solution = motion;
156 if (dist < sol->approxdif)
159 if (dist < sol->approxdif)
161 sol->approxdif = dist;
162 sol->approxsol = motion;
169 si_->freeState(xstate);
171 si_->freeState(rmotion->state);
185 samplerArray_.resize(threadCount_);
190 si_->copyState(motion->state, st);
194 if (nn_->size() == 0)
196 logError(
"There are no valid initial states!");
200 logInform(
"Starting with %u states", nn_->size());
204 sol.approxsol = NULL;
205 sol.approxdif = std::numeric_limits<double>::infinity();
207 std::vector<boost::thread*> th(threadCount_);
208 for (
unsigned int i = 0 ; i < threadCount_ ; ++i)
209 th[i] =
new boost::thread(boost::bind(&pRRT::threadSolve,
this, i, ptc, &sol));
210 for (
unsigned int i = 0 ; i < threadCount_ ; ++i)
217 bool approximate =
false;
218 if (sol.solution == NULL)
220 sol.solution = sol.approxsol;
224 if (sol.solution != NULL)
226 lastGoalMotion_ = sol.solution;
229 std::vector<Motion*> mpath;
230 while (sol.solution != NULL)
232 mpath.push_back(sol.solution);
233 sol.solution = sol.solution->parent;
238 for (
int i = mpath.size() - 1 ; i >= 0 ; --i)
239 path->
append(mpath[i]->state);
241 pdef_->addSolutionPath(
base::PathPtr(path), approximate, sol.approxdif);
245 logInform(
"Created %u states", nn_->size());
252 Planner::getPlannerData(data);
254 std::vector<Motion*> motions;
261 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
263 if (motions[i]->parent == NULL)
273 assert(nthreads > 0);
274 threadCount_ = nthreads;