37 #include "ompl/geometric/planners/rrt/pRRT.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/tools/config/SelfConfig.h"
40 #include <boost/thread/thread.hpp>
43 ompl::geometric::pRRT::pRRT(
const base::SpaceInformationPtr &si) : base::Planner(si,
"pRRT"),
57 Planner::declareParam<unsigned int>(
"thread_count",
this, &
pRRT::setThreadCount, &pRRT::getThreadCount,
"1:64");
60 ompl::geometric::pRRT::~pRRT(
void)
72 nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(si_->getStateSpace()));
73 nn_->setDistanceFunction(boost::bind(&pRRT::distanceFunction,
this, _1, _2));
79 samplerArray_.clear();
83 lastGoalMotion_ = NULL;
86 void ompl::geometric::pRRT::freeMemory(
void)
90 std::vector<Motion*> motions;
92 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
94 if (motions[i]->state)
95 si_->freeState(motions[i]->state);
101 void ompl::geometric::pRRT::threadSolve(
unsigned int tid,
const base::PlannerTerminationCondition &ptc, SolutionInfo *sol)
104 base::Goal *goal = pdef_->getGoal().get();
105 base::GoalSampleableRegion *goal_s =
dynamic_cast<base::GoalSampleableRegion*
>(goal);
108 Motion *rmotion =
new Motion(si_);
109 base::State *rstate = rmotion->state;
110 base::State *xstate = si_->allocState();
112 while (sol->solution == NULL && ptc ==
false)
115 if (goal_s && rng.uniform01() < goalBias_ && goal_s->canSample())
116 goal_s->sampleGoal(rstate);
118 samplerArray_[tid]->sampleUniform(rstate);
122 Motion *nmotion = nn_->nearest(rmotion);
124 base::State *dstate = rstate;
127 double d = si_->distance(nmotion->state, rstate);
128 if (d > maxDistance_)
130 si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
134 if (si_->checkMotion(nmotion->state, dstate))
137 Motion *motion =
new Motion(si_);
138 si_->copyState(motion->state, dstate);
139 motion->parent = nmotion;
146 bool solved = goal->isSatisfied(motion->state, &dist);
150 sol->approxdif = dist;
151 sol->solution = motion;
155 if (dist < sol->approxdif)
158 if (dist < sol->approxdif)
160 sol->approxdif = dist;
161 sol->approxsol = motion;
168 si_->freeState(xstate);
170 si_->freeState(rmotion->state);
184 samplerArray_.resize(threadCount_);
189 si_->copyState(motion->state, st);
193 if (nn_->size() == 0)
195 OMPL_ERROR(
"There are no valid initial states!");
199 OMPL_INFORM(
"Starting with %u states", nn_->size());
203 sol.approxsol = NULL;
204 sol.approxdif = std::numeric_limits<double>::infinity();
206 std::vector<boost::thread*> th(threadCount_);
207 for (
unsigned int i = 0 ; i < threadCount_ ; ++i)
208 th[i] =
new boost::thread(boost::bind(&pRRT::threadSolve,
this, i, ptc, &sol));
209 for (
unsigned int i = 0 ; i < threadCount_ ; ++i)
216 bool approximate =
false;
217 if (sol.solution == NULL)
219 sol.solution = sol.approxsol;
223 if (sol.solution != NULL)
225 lastGoalMotion_ = sol.solution;
228 std::vector<Motion*> mpath;
229 while (sol.solution != NULL)
231 mpath.push_back(sol.solution);
232 sol.solution = sol.solution->parent;
237 for (
int i = mpath.size() - 1 ; i >= 0 ; --i)
238 path->
append(mpath[i]->state);
240 pdef_->addSolutionPath(
base::PathPtr(path), approximate, sol.approxdif);
251 Planner::getPlannerData(data);
253 std::vector<Motion*> motions;
260 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
262 if (motions[i]->parent == NULL)
272 assert(nthreads > 0);
273 threadCount_ = nthreads;