Loading...
Searching...
No Matches
pRRT.cpp
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2008, Willow Garage, Inc.
5* All rights reserved.
6*
7* Redistribution and use in source and binary forms, with or without
8* modification, are permitted provided that the following conditions
9* are met:
10*
11* * Redistributions of source code must retain the above copyright
12* notice, this list of conditions and the following disclaimer.
13* * Redistributions in binary form must reproduce the above
14* copyright notice, this list of conditions and the following
15* disclaimer in the documentation and/or other materials provided
16* with the distribution.
17* * Neither the name of the Willow Garage nor the names of its
18* contributors may be used to endorse or promote products derived
19* from this software without specific prior written permission.
20*
21* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32* POSSIBILITY OF SUCH DAMAGE.
33*********************************************************************/
34
35/* Author: Ioan Sucan */
36
37#include "ompl/geometric/planners/rrt/pRRT.h"
38#include "ompl/base/goals/GoalSampleableRegion.h"
39#include "ompl/tools/config/SelfConfig.h"
40#include <limits>
41
42ompl::geometric::pRRT::pRRT(const base::SpaceInformationPtr &si) : base::Planner(si, "pRRT"), samplerArray_(si)
43{
44 specs_.approximateSolutions = true;
45 specs_.multithreaded = true;
46 specs_.directed = true;
47
48 setThreadCount(2);
49
50 Planner::declareParam<double>("range", this, &pRRT::setRange, &pRRT::getRange, "0.:1.:10000.");
51 Planner::declareParam<double>("goal_bias", this, &pRRT::setGoalBias, &pRRT::getGoalBias, "0.:.05:1.");
52 Planner::declareParam<unsigned int>("thread_count", this, &pRRT::setThreadCount, &pRRT::getThreadCount, "1:64");
53}
54
55ompl::geometric::pRRT::~pRRT()
56{
57 freeMemory();
58}
59
61{
62 Planner::setup();
63 tools::SelfConfig sc(si_, getName());
64 sc.configurePlannerRange(maxDistance_);
65
66 if (!nn_)
67 nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
68 nn_->setDistanceFunction([this](const Motion *a, const Motion *b)
69 {
70 return distanceFunction(a, b);
71 });
72}
73
75{
76 Planner::clear();
77 samplerArray_.clear();
78 freeMemory();
79 if (nn_)
80 nn_->clear();
81 lastGoalMotion_ = nullptr;
82}
83
84void ompl::geometric::pRRT::freeMemory()
85{
86 if (nn_)
87 {
88 std::vector<Motion *> motions;
89 nn_->list(motions);
90 for (auto &motion : motions)
91 {
92 if (motion->state)
93 si_->freeState(motion->state);
94 delete motion;
95 }
96 }
97}
98
99void ompl::geometric::pRRT::threadSolve(unsigned int tid, const base::PlannerTerminationCondition &ptc,
100 SolutionInfo *sol)
101{
102 base::Goal *goal = pdef_->getGoal().get();
103 auto *goal_s = dynamic_cast<base::GoalSampleableRegion *>(goal);
104 RNG rng;
105
106 auto *rmotion = new Motion(si_);
107 base::State *rstate = rmotion->state;
108 base::State *xstate = si_->allocState();
109
110 while (sol->solution == nullptr && ptc == false)
111 {
112 /* sample random state (with goal biasing) */
113 if (goal_s && rng.uniform01() < goalBias_ && goal_s->canSample())
114 goal_s->sampleGoal(rstate);
115 else
116 samplerArray_[tid]->sampleUniform(rstate);
117
118 /* find closest state in the tree */
119 nnLock_.lock();
120 Motion *nmotion = nn_->nearest(rmotion);
121 nnLock_.unlock();
122 base::State *dstate = rstate;
123
124 /* find state to add */
125 double d = si_->distance(nmotion->state, rstate);
126 if (d > maxDistance_)
127 {
128 si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
129 dstate = xstate;
130 }
131
132 if (si_->checkMotion(nmotion->state, dstate))
133 {
134 /* create a motion */
135 auto *motion = new Motion(si_);
136 si_->copyState(motion->state, dstate);
137 motion->parent = nmotion;
138
139 nnLock_.lock();
140 nn_->add(motion);
141 nnLock_.unlock();
142
143 double dist = 0.0;
144 bool solved = goal->isSatisfied(motion->state, &dist);
145 if (solved)
146 {
147 sol->lock.lock();
148 sol->approxdif = dist;
149 sol->solution = motion;
150 sol->lock.unlock();
151 break;
152 }
153 if (dist < sol->approxdif)
154 {
155 sol->lock.lock();
156 if (dist < sol->approxdif)
157 {
158 sol->approxdif = dist;
159 sol->approxsol = motion;
160 }
161 sol->lock.unlock();
162 }
163 }
164 }
165
166 si_->freeState(xstate);
167 if (rmotion->state)
168 si_->freeState(rmotion->state);
169 delete rmotion;
170}
171
173{
174 checkValidity();
175
176 auto *goal = dynamic_cast<base::GoalRegion *>(pdef_->getGoal().get());
177
178 if (!goal)
179 {
180 OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
182 }
183
184 samplerArray_.resize(threadCount_);
185
186 while (const base::State *st = pis_.nextStart())
187 {
188 auto *motion = new Motion(si_);
189 si_->copyState(motion->state, st);
190 nn_->add(motion);
191 }
192
193 if (nn_->size() == 0)
194 {
195 OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
197 }
198
199 OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
200
201 SolutionInfo sol;
202 sol.solution = nullptr;
203 sol.approxsol = nullptr;
204 sol.approxdif = std::numeric_limits<double>::infinity();
205
206 std::vector<std::thread *> th(threadCount_);
207 for (unsigned int i = 0; i < threadCount_; ++i)
208 th[i] = new std::thread([this, i, &ptc, &sol]
209 {
210 return threadSolve(i, ptc, &sol);
211 });
212 for (unsigned int i = 0; i < threadCount_; ++i)
213 {
214 th[i]->join();
215 delete th[i];
216 }
217
218 bool solved = false;
219 bool approximate = false;
220 if (sol.solution == nullptr)
221 {
222 sol.solution = sol.approxsol;
223 approximate = true;
224 }
225
226 if (sol.solution != nullptr)
227 {
228 lastGoalMotion_ = sol.solution;
229
230 /* construct the solution path */
231 std::vector<Motion *> mpath;
232 while (sol.solution != nullptr)
233 {
234 mpath.push_back(sol.solution);
235 sol.solution = sol.solution->parent;
236 }
237
238 /* set the solution path */
239 auto path(std::make_shared<PathGeometric>(si_));
240 for (int i = mpath.size() - 1; i >= 0; --i)
241 path->append(mpath[i]->state);
242
243 pdef_->addSolutionPath(path, approximate, sol.approxdif, getName());
244 solved = true;
245 }
246
247 OMPL_INFORM("%s: Created %u states", getName().c_str(), nn_->size());
248
249 return {solved, approximate};
250}
251
253{
254 Planner::getPlannerData(data);
255
256 std::vector<Motion *> motions;
257 if (nn_)
258 nn_->list(motions);
259
260 if (lastGoalMotion_)
261 data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));
262
263 for (auto &motion : motions)
264 {
265 if (motion->parent == nullptr)
266 data.addStartVertex(base::PlannerDataVertex(motion->state));
267 else
268 data.addEdge(base::PlannerDataVertex(motion->parent->state), base::PlannerDataVertex(motion->state));
269 }
270}
271
272void ompl::geometric::pRRT::setThreadCount(unsigned int nthreads)
273{
274 assert(nthreads > 0);
275 threadCount_ = nthreads;
276}
Definition of a goal region.
Definition GoalRegion.h:48
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition PlannerData.h:59
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
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...
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...
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...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition of an abstract state.
Definition State.h:50
void getPlannerData(base::PlannerData &data) const override
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition pRRT.cpp:252
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition pRRT.cpp:74
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition pRRT.cpp:60
void setThreadCount(unsigned int nthreads)
Set the number of threads the planner should use. Default is 2.
Definition pRRT.cpp:272
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Function that can solve the motion planning problem. This function can be called multiple times on th...
Definition pRRT.cpp:172
This class contains methods that automatically configure various parameters for motion planning....
Definition SelfConfig.h:60
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition Console.h:68
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition Console.h:64
A class to store the exit status of Planner::solve()
@ INVALID_START
Invalid start state or no start state specified.
@ UNRECOGNIZED_GOAL_TYPE
The goal is of a type that a planner does not recognize.