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