Loading...
Searching...
No Matches
pSBL.h
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#ifndef OMPL_GEOMETRIC_PLANNERS_SBL_pSBL_
38#define OMPL_GEOMETRIC_PLANNERS_SBL_pSBL_
39
40#include "ompl/geometric/planners/PlannerIncludes.h"
41#include "ompl/base/ProjectionEvaluator.h"
42#include "ompl/base/StateSamplerArray.h"
43#include "ompl/datastructures/Grid.h"
44#include "ompl/datastructures/PDF.h"
45#include <thread>
46#include <mutex>
47#include <vector>
48
49namespace ompl
50{
51 namespace geometric
52 {
86 class pSBL : public base::Planner
87 {
88 public:
89 pSBL(const base::SpaceInformationPtr &si);
90
91 ~pSBL() override;
92
95 void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
96 {
97 projectionEvaluator_ = projectionEvaluator;
98 }
99
102 void setProjectionEvaluator(const std::string &name)
103 {
104 projectionEvaluator_ = si_->getStateSpace()->getProjection(name);
105 }
106
108 const base::ProjectionEvaluatorPtr &getProjectionEvaluator() const
109 {
110 return projectionEvaluator_;
111 }
112
118 void setRange(double distance)
119 {
120 maxDistance_ = distance;
121 }
122
124 double getRange() const
125 {
126 return maxDistance_;
127 }
128
130 void setThreadCount(unsigned int nthreads);
131
133 unsigned int getThreadCount() const
134 {
135 return threadCount_;
136 }
137
138 void setup() override;
139
141
142 void clear() override;
143
144 void getPlannerData(base::PlannerData &data) const override;
145
146 protected:
147 class Motion;
148 struct MotionInfo;
149
152
155
156 class Motion
157 {
158 public:
159 Motion() = default;
160
161 Motion(const base::SpaceInformationPtr &si)
162 : state(si->allocState())
163 {
164 }
165
166 ~Motion() = default;
167
168 const base::State *root{nullptr};
169 base::State *state{nullptr};
170 Motion *parent{nullptr};
171 bool valid{false};
172 std::vector<Motion *> children;
173 std::mutex lock;
174 };
175
178 {
179 Motion *operator[](unsigned int i)
180 {
181 return motions_[i];
182 }
183 std::vector<Motion *>::iterator begin()
184 {
185 return motions_.begin();
186 }
187 void erase(std::vector<Motion *>::iterator iter)
188 {
189 motions_.erase(iter);
190 }
191 void push_back(Motion *m)
192 {
193 motions_.push_back(m);
194 }
195 unsigned int size() const
196 {
197 return motions_.size();
198 }
199 bool empty() const
200 {
201 return motions_.empty();
202 }
203 std::vector<Motion *> motions_;
204 CellPDF::Element *elem_;
205 };
206
207 struct TreeData
208 {
209 TreeData() = default;
210
211 Grid<MotionInfo> grid{0};
212 unsigned int size{0};
213 CellPDF pdf;
214 std::mutex lock;
215 };
216
218 {
219 std::vector<Motion *> solution;
220 bool found;
221 std::mutex lock;
222 };
223
225 {
226 TreeData *tree;
227 Motion *motion;
228 };
229
231 {
232 std::vector<PendingRemoveMotion> motions;
233 std::mutex lock;
234 };
235
236 void threadSolve(unsigned int tid, const base::PlannerTerminationCondition &ptc, SolutionInfo *sol);
237
238 void freeMemory()
239 {
240 freeGridMotions(tStart_.grid);
241 freeGridMotions(tGoal_.grid);
242 }
243
244 void freeGridMotions(Grid<MotionInfo> &grid);
245
246 void addMotion(TreeData &tree, Motion *motion);
247 Motion *selectMotion(RNG &rng, TreeData &tree);
248 void removeMotion(TreeData &tree, Motion *motion, std::map<Motion *, bool> &seen);
249 bool isPathValid(TreeData &tree, Motion *motion);
250 bool checkSolution(RNG &rng, bool start, TreeData &tree, TreeData &otherTree, Motion *motion,
251 std::vector<Motion *> &solution);
252
254 base::ProjectionEvaluatorPtr projectionEvaluator_;
255
256 TreeData tStart_;
257 TreeData tGoal_;
258
259 MotionsToBeRemoved removeList_;
260 std::mutex loopLock_;
261 std::mutex loopLockCounter_;
262 unsigned int loopCounter_;
263
264 double maxDistance_{0.};
265
266 unsigned int threadCount_;
267
269 std::pair<base::State *, base::State *> connectionPoint_{nullptr, nullptr};
270 };
271 }
272}
273
274#endif
Representation of a simple grid.
Definition Grid.h:52
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Base class for a planner.
Definition Planner.h:216
SpaceInformationPtr si_
The space information for which planning is done.
Definition Planner.h:410
Class to ease the creation of a set of samplers. This is especially useful for multi-threaded planner...
Definition of an abstract state.
Definition State.h:50
Parallel Single-query Bi-directional Lazy collision checking planner.
Definition pSBL.h:87
double getRange() const
Get the range the planner is using.
Definition pSBL.h:124
std::pair< base::State *, base::State * > connectionPoint_
The pair of states in each tree connected during planning. Used for PlannerData computation.
Definition pSBL.h:269
const base::ProjectionEvaluatorPtr & getProjectionEvaluator() const
Get the projection evaluator.
Definition pSBL.h:108
void setProjectionEvaluator(const std::string &name)
Set the projection evaluator (select one from the ones registered with the state space).
Definition pSBL.h:102
unsigned int getThreadCount() const
Get the thread count.
Definition pSBL.h:133
void setRange(double distance)
Set the range the planner is supposed to use.
Definition pSBL.h:118
void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
Set the projection evaluator. This class is able to compute the projection of a given state.
Definition pSBL.h:95
void setThreadCount(unsigned int nthreads)
Set the number of threads the planner should use. Default is 2.
Definition pSBL.cpp:480
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 pSBL.cpp:451
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition pSBL.cpp:58
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition pSBL.cpp:69
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 pSBL.cpp:188
Main namespace. Contains everything in this library.
Definition of a cell in this grid.
Definition Grid.h:59
A class to store the exit status of Planner::solve()
A struct containing an array of motions and a corresponding PDF element.
Definition pSBL.h:178