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/NearestNeighborsGNAT.h"
00039 #include "ompl/base/GoalSampleableRegion.h"
00040 #include "ompl/tools/config/SelfConfig.h"
00041 #include <boost/thread/thread.hpp>
00042 #include <limits>
00043
00044 ompl::geometric::pRRT::pRRT(const base::SpaceInformationPtr &si) : base::Planner(si, "pRRT"),
00045 samplerArray_(si)
00046 {
00047 specs_.approximateSolutions = true;
00048 specs_.multithreaded = true;
00049
00050 setThreadCount(2);
00051 goalBias_ = 0.05;
00052 maxDistance_ = 0.0;
00053
00054 Planner::declareParam<double>("range", this, &pRRT::setRange, &pRRT::getRange);
00055 Planner::declareParam<double>("goal_bias", this, &pRRT::setGoalBias, &pRRT::getGoalBias);
00056 Planner::declareParam<unsigned int>("thread_count", this, &pRRT::setThreadCount, &pRRT::getThreadCount);
00057 }
00058
00059 ompl::geometric::pRRT::~pRRT(void)
00060 {
00061 freeMemory();
00062 }
00063
00064 void ompl::geometric::pRRT::setup(void)
00065 {
00066 Planner::setup();
00067 tools::SelfConfig sc(si_, getName());
00068 sc.configurePlannerRange(maxDistance_);
00069
00070 if (!nn_)
00071 nn_.reset(new NearestNeighborsGNAT<Motion*>());
00072 nn_->setDistanceFunction(boost::bind(&pRRT::distanceFunction, this, _1, _2));
00073 }
00074
00075 void ompl::geometric::pRRT::clear(void)
00076 {
00077 Planner::clear();
00078 samplerArray_.clear();
00079 freeMemory();
00080 if (nn_)
00081 nn_->clear();
00082 }
00083
00084 void ompl::geometric::pRRT::freeMemory(void)
00085 {
00086 if (nn_)
00087 {
00088 std::vector<Motion*> motions;
00089 nn_->list(motions);
00090 for (unsigned int i = 0 ; i < motions.size() ; ++i)
00091 {
00092 if (motions[i]->state)
00093 si_->freeState(motions[i]->state);
00094 delete motions[i];
00095 }
00096 }
00097 }
00098
00099 void ompl::geometric::pRRT::threadSolve(unsigned int tid, const base::PlannerTerminationCondition &ptc, SolutionInfo *sol)
00100 {
00101 checkValidity();
00102 base::Goal *goal = pdef_->getGoal().get();
00103 base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
00104 RNG rng;
00105
00106 Motion *rmotion = new Motion(si_);
00107 base::State *rstate = rmotion->state;
00108 base::State *xstate = si_->allocState();
00109
00110 while (sol->solution == NULL && ptc() == false)
00111 {
00112
00113 if (goal_s && rng.uniform01() < goalBias_ && goal_s->canSample())
00114 goal_s->sampleGoal(rstate);
00115 else
00116 samplerArray_[tid]->sampleUniform(rstate);
00117
00118
00119 nnLock_.lock();
00120 Motion *nmotion = nn_->nearest(rmotion);
00121 nnLock_.unlock();
00122 base::State *dstate = rstate;
00123
00124
00125 double d = si_->distance(nmotion->state, rstate);
00126 if (d > maxDistance_)
00127 {
00128 si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
00129 dstate = xstate;
00130 }
00131
00132 if (si_->checkMotion(nmotion->state, dstate))
00133 {
00134
00135 Motion *motion = new Motion(si_);
00136 si_->copyState(motion->state, dstate);
00137 motion->parent = nmotion;
00138
00139 nnLock_.lock();
00140 nn_->add(motion);
00141 nnLock_.unlock();
00142
00143 double dist = 0.0;
00144 bool solved = goal->isSatisfied(motion->state, &dist);
00145 if (solved)
00146 {
00147 sol->lock.lock();
00148 sol->approxdif = dist;
00149 sol->solution = motion;
00150 sol->lock.unlock();
00151 break;
00152 }
00153 if (dist < sol->approxdif)
00154 {
00155 sol->lock.lock();
00156 if (dist < sol->approxdif)
00157 {
00158 sol->approxdif = dist;
00159 sol->approxsol = motion;
00160 }
00161 sol->lock.unlock();
00162 }
00163 }
00164 }
00165
00166 si_->freeState(xstate);
00167 if (rmotion->state)
00168 si_->freeState(rmotion->state);
00169 delete rmotion;
00170 }
00171
00172 bool ompl::geometric::pRRT::solve(const base::PlannerTerminationCondition &ptc)
00173 {
00174 base::GoalRegion *goal = dynamic_cast<base::GoalRegion*>(pdef_->getGoal().get());
00175
00176 if (!goal)
00177 {
00178 msg_.error("Goal undefined");
00179 return false;
00180 }
00181
00182 samplerArray_.resize(threadCount_);
00183
00184 while (const base::State *st = pis_.nextStart())
00185 {
00186 Motion *motion = new Motion(si_);
00187 si_->copyState(motion->state, st);
00188 nn_->add(motion);
00189 }
00190
00191 if (nn_->size() == 0)
00192 {
00193 msg_.error("There are no valid initial states!");
00194 return false;
00195 }
00196
00197 msg_.inform("Starting with %u states", nn_->size());
00198
00199 SolutionInfo sol;
00200 sol.solution = NULL;
00201 sol.approxsol = NULL;
00202 sol.approxdif = std::numeric_limits<double>::infinity();
00203
00204 std::vector<boost::thread*> th(threadCount_);
00205 for (unsigned int i = 0 ; i < threadCount_ ; ++i)
00206 th[i] = new boost::thread(boost::bind(&pRRT::threadSolve, this, i, ptc, &sol));
00207 for (unsigned int i = 0 ; i < threadCount_ ; ++i)
00208 {
00209 th[i]->join();
00210 delete th[i];
00211 }
00212
00213 bool solved = false;
00214 bool approximate = false;
00215 if (sol.solution == NULL)
00216 {
00217 sol.solution = sol.approxsol;
00218 approximate = true;
00219 }
00220
00221 if (sol.solution != NULL)
00222 {
00223
00224 std::vector<Motion*> mpath;
00225 while (sol.solution != NULL)
00226 {
00227 mpath.push_back(sol.solution);
00228 sol.solution = sol.solution->parent;
00229 }
00230
00231
00232 PathGeometric *path = new PathGeometric(si_);
00233 for (int i = mpath.size() - 1 ; i >= 0 ; --i)
00234 path->append(mpath[i]->state);
00235
00236 goal->addSolutionPath(base::PathPtr(path), approximate, sol.approxdif);
00237 solved = true;
00238 }
00239
00240 msg_.inform("Created %u states", nn_->size());
00241
00242 return solved;
00243 }
00244
00245 void ompl::geometric::pRRT::getPlannerData(base::PlannerData &data) const
00246 {
00247 Planner::getPlannerData(data);
00248
00249 std::vector<Motion*> motions;
00250 if (nn_)
00251 nn_->list(motions);
00252
00253 for (unsigned int i = 0 ; i < motions.size() ; ++i)
00254 data.recordEdge(motions[i]->parent ? motions[i]->parent->state : NULL, motions[i]->state);
00255 }
00256
00257 void ompl::geometric::pRRT::setThreadCount(unsigned int nthreads)
00258 {
00259 assert(nthreads > 0);
00260 threadCount_ = nthreads;
00261 }