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)
103 base::Goal *goal = pdef_->getGoal().get();
104 base::GoalSampleableRegion *goal_s =
dynamic_cast<base::GoalSampleableRegion*
>(goal);
107 Motion *rmotion =
new Motion(si_);
108 base::State *rstate = rmotion->state;
109 base::State *xstate = si_->allocState();
111 while (sol->solution == NULL && ptc ==
false)
114 if (goal_s && rng.uniform01() < goalBias_ && goal_s->canSample())
115 goal_s->sampleGoal(rstate);
117 samplerArray_[tid]->sampleUniform(rstate);
121 Motion *nmotion = nn_->nearest(rmotion);
123 base::State *dstate = rstate;
126 double d = si_->distance(nmotion->state, rstate);
127 if (d > maxDistance_)
129 si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
133 if (si_->checkMotion(nmotion->state, dstate))
136 Motion *motion =
new Motion(si_);
137 si_->copyState(motion->state, dstate);
138 motion->parent = nmotion;
145 bool solved = goal->isSatisfied(motion->state, &dist);
149 sol->approxdif = dist;
150 sol->solution = motion;
154 if (dist < sol->approxdif)
157 if (dist < sol->approxdif)
159 sol->approxdif = dist;
160 sol->approxsol = motion;
167 si_->freeState(xstate);
169 si_->freeState(rmotion->state);
181 OMPL_ERROR(
"%s: Unknow type of goal", getName().c_str());
185 samplerArray_.resize(threadCount_);
190 si_->copyState(motion->state, st);
194 if (nn_->size() == 0)
196 OMPL_ERROR(
"%s: There are no valid initial states!", getName().c_str());
200 OMPL_INFORM(
"%s: Starting with %u states", getName().c_str(), 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 OMPL_INFORM(
"%s: Created %u states", getName().c_str(), 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;
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
void setGoalBias(double goalBias)
Set the goal bias.
unsigned int addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
double maxDistance_
The maximum length of a motion to be added to a tree.
virtual void clear(void)
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
void append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
bool multithreaded
Flag indicating whether multiple threads are used in the computation of the planner.
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
double goalBias_
The fraction of time the goal is picked as the state to expand towards (if such a state is available)...
void setThreadCount(unsigned int nthreads)
Set the number of threads the planner should use. Default is 2.
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Invalid start state or no start state specified.
The goal is of a type that a planner does not recognize.
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
void setRange(double distance)
Set the range the planner is supposed to use.
A class to store the exit status of Planner::solve()
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Definition of an abstract state.
PlannerSpecs specs_
The specifications of the planner (its capabilities)
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition of a goal region.
Definition of a geometric path.
double getRange(void) const
Get the range the planner is using.
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
Function that can solve the motion planning problem. This function can be called multiple times on th...
double getGoalBias(void) const
Get the goal bias the planner is using.
A boost shared pointer wrapper for ompl::base::Path.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.