00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "ompl/geometric/planners/rrt/pRRT.h"
00038 #include "ompl/datastructures/NearestNeighborsSqrtApprox.h"
00039 #include "ompl/base/GoalSampleableRegion.h"
00040 #include <boost/thread/thread.hpp>
00041 #include <limits>
00042
00043 void ompl::geometric::pRRT::setup(void)
00044 {
00045 Planner::setup();
00046 checkMotionLength(this, maxDistance_);
00047
00048 if (!nn_)
00049 nn_.reset(new NearestNeighborsSqrtApprox<Motion*>());
00050 nn_->setDistanceFunction(boost::bind(&pRRT::distanceFunction, this, _1, _2));
00051 }
00052
00053 void ompl::geometric::pRRT::clear(void)
00054 {
00055 Planner::clear();
00056 samplerArray_.clear();
00057 freeMemory();
00058 if (nn_)
00059 nn_->clear();
00060 }
00061
00062 void ompl::geometric::pRRT::freeMemory(void)
00063 {
00064 if (nn_)
00065 {
00066 std::vector<Motion*> motions;
00067 nn_->list(motions);
00068 for (unsigned int i = 0 ; i < motions.size() ; ++i)
00069 {
00070 if (motions[i]->state)
00071 si_->freeState(motions[i]->state);
00072 delete motions[i];
00073 }
00074 }
00075 }
00076
00077 void ompl::geometric::pRRT::threadSolve(unsigned int tid, const base::PlannerTerminationCondition &ptc, SolutionInfo *sol)
00078 {
00079 checkValidity();
00080 base::Goal *goal = pdef_->getGoal().get();
00081 base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
00082 RNG rng;
00083
00084 Motion *rmotion = new Motion(si_);
00085 base::State *rstate = rmotion->state;
00086 base::State *xstate = si_->allocState();
00087
00088 while (sol->solution == NULL && ptc() == false)
00089 {
00090
00091 if (goal_s && rng.uniform01() < goalBias_ && goal_s->canSample())
00092 goal_s->sampleGoal(rstate);
00093 else
00094 samplerArray_[tid]->sampleUniform(rstate);
00095
00096
00097 nnLock_.lock();
00098 Motion *nmotion = nn_->nearest(rmotion);
00099 nnLock_.unlock();
00100 base::State *dstate = rstate;
00101
00102
00103 double d = si_->distance(nmotion->state, rstate);
00104 if (d > maxDistance_)
00105 {
00106 si_->getStateManifold()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
00107 dstate = xstate;
00108 }
00109
00110 if (si_->checkMotion(nmotion->state, dstate))
00111 {
00112
00113 Motion *motion = new Motion(si_);
00114 si_->copyState(motion->state, dstate);
00115 motion->parent = nmotion;
00116
00117 nnLock_.lock();
00118 nn_->add(motion);
00119 nnLock_.unlock();
00120
00121 double dist = 0.0;
00122 bool solved = goal->isSatisfied(motion->state, &dist);
00123 if (solved)
00124 {
00125 sol->lock.lock();
00126 sol->approxdif = dist;
00127 sol->solution = motion;
00128 sol->lock.unlock();
00129 break;
00130 }
00131 if (dist < sol->approxdif)
00132 {
00133 sol->lock.lock();
00134 if (dist < sol->approxdif)
00135 {
00136 sol->approxdif = dist;
00137 sol->approxsol = motion;
00138 }
00139 sol->lock.unlock();
00140 }
00141 }
00142 }
00143
00144 si_->freeState(xstate);
00145 if (rmotion->state)
00146 si_->freeState(rmotion->state);
00147 delete rmotion;
00148 }
00149
00150 bool ompl::geometric::pRRT::solve(const base::PlannerTerminationCondition &ptc)
00151 {
00152 base::GoalRegion *goal = dynamic_cast<base::GoalRegion*>(pdef_->getGoal().get());
00153
00154 if (!goal)
00155 {
00156 msg_.error("Goal undefined");
00157 return false;
00158 }
00159
00160 samplerArray_.resize(threadCount_);
00161
00162 while (const base::State *st = pis_.nextStart())
00163 {
00164 Motion *motion = new Motion(si_);
00165 si_->copyState(motion->state, st);
00166 nn_->add(motion);
00167 }
00168
00169 if (nn_->size() == 0)
00170 {
00171 msg_.error("There are no valid initial states!");
00172 return false;
00173 }
00174
00175 msg_.inform("Starting with %u states", nn_->size());
00176
00177 SolutionInfo sol;
00178 sol.solution = NULL;
00179 sol.approxsol = NULL;
00180 sol.approxdif = std::numeric_limits<double>::infinity();
00181
00182 std::vector<boost::thread*> th(threadCount_);
00183 for (unsigned int i = 0 ; i < threadCount_ ; ++i)
00184 th[i] = new boost::thread(boost::bind(&pRRT::threadSolve, this, i, ptc, &sol));
00185 for (unsigned int i = 0 ; i < threadCount_ ; ++i)
00186 {
00187 th[i]->join();
00188 delete th[i];
00189 }
00190
00191 bool approximate = false;
00192 if (sol.solution == NULL)
00193 {
00194 sol.solution = sol.approxsol;
00195 approximate = true;
00196 }
00197
00198 if (sol.solution != NULL)
00199 {
00200
00201 std::vector<Motion*> mpath;
00202 while (sol.solution != NULL)
00203 {
00204 mpath.push_back(sol.solution);
00205 sol.solution = sol.solution->parent;
00206 }
00207
00208
00209 PathGeometric *path = new PathGeometric(si_);
00210 for (int i = mpath.size() - 1 ; i >= 0 ; --i)
00211 path->states.push_back(si_->cloneState(mpath[i]->state));
00212
00213 goal->setDifference(sol.approxdif);
00214 goal->setSolutionPath(base::PathPtr(path), approximate);
00215
00216 if (approximate)
00217 msg_.warn("Found approximate solution");
00218 }
00219
00220 msg_.inform("Created %u states", nn_->size());
00221
00222 return goal->isAchieved();
00223 }
00224
00225 void ompl::geometric::pRRT::getPlannerData(base::PlannerData &data) const
00226 {
00227 Planner::getPlannerData(data);
00228
00229 std::vector<Motion*> motions;
00230 if (nn_)
00231 nn_->list(motions);
00232
00233 for (unsigned int i = 0 ; i < motions.size() ; ++i)
00234 data.recordEdge(motions[i]->parent ? motions[i]->parent->state : NULL, motions[i]->state);
00235 }
00236
00237 void ompl::geometric::pRRT::setThreadCount(unsigned int nthreads)
00238 {
00239 assert(nthreads > 0);
00240 threadCount_ = nthreads;
00241 }