SBL.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_SBL_
38#define OMPL_GEOMETRIC_PLANNERS_SBL_SBL_
39
40#include "ompl/geometric/planners/PlannerIncludes.h"
41#include "ompl/base/ProjectionEvaluator.h"
42#include "ompl/datastructures/Grid.h"
43#include "ompl/datastructures/PDF.h"
44#include <vector>
45
46namespace ompl
47{
48 namespace geometric
49 {
84 class SBL : public base::Planner
85 {
86 public:
88 SBL(const base::SpaceInformationPtr &si);
89
90 ~SBL() override;
91
94 void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
95 {
96 projectionEvaluator_ = projectionEvaluator;
97 }
98
101 void setProjectionEvaluator(const std::string &name)
102 {
103 projectionEvaluator_ = si_->getStateSpace()->getProjection(name);
104 }
105
107 const base::ProjectionEvaluatorPtr &getProjectionEvaluator() const
108 {
110 }
111
117 void setRange(double distance)
118 {
119 maxDistance_ = distance;
120 }
121
123 double getRange() const
124 {
125 return maxDistance_;
126 }
127
128 void setup() override;
129
131
132 void clear() override;
133
134 void getPlannerData(base::PlannerData &data) const override;
135
136 protected:
137 struct MotionInfo;
138
141
144
146 class Motion
147 {
148 public:
150 Motion() = default;
151
153 Motion(const base::SpaceInformationPtr &si)
154 : state(si->allocState())
155 {
156 }
157
159 const base::State *root{nullptr};
160
163
165 Motion *parent{nullptr};
166
168 bool valid{false};
169
171 std::vector<Motion *> children;
172 };
173
176 {
177 Motion *operator[](unsigned int i)
178 {
179 return motions_[i];
180 }
181 std::vector<Motion *>::iterator begin()
182 {
183 return motions_.begin();
184 }
185 void erase(std::vector<Motion *>::iterator iter)
186 {
187 motions_.erase(iter);
188 }
189 void push_back(Motion *m)
190 {
191 motions_.push_back(m);
192 }
193 unsigned int size() const
194 {
195 return motions_.size();
196 }
197 bool empty() const
198 {
199 return motions_.empty();
200 }
201
202 std::vector<Motion *> motions_;
203 CellPDF::Element *elem_;
204 };
205
207 struct TreeData
208 {
209 TreeData() = default;
210
213
215 unsigned int size{0};
216
219 };
220
223 {
226 }
227
230
232 void addMotion(TreeData &tree, Motion *motion);
233
235 Motion *selectMotion(TreeData &tree);
236
238 void removeMotion(TreeData &tree, Motion *motion);
239
245 bool isPathValid(TreeData &tree, Motion *motion);
246
248 bool checkSolution(bool start, TreeData &tree, TreeData &otherTree, Motion *motion,
249 std::vector<Motion *> &solution);
250
252 base::ValidStateSamplerPtr sampler_;
253
255 base::ProjectionEvaluatorPtr projectionEvaluator_;
256
259
262
264 double maxDistance_{0.};
265
268
270 std::pair<base::State *, base::State *> connectionPoint_{nullptr, nullptr};
271 };
272 }
273}
274
275#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...
Definition: RandomNumbers.h:58
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Base class for a planner.
Definition: Planner.h:223
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:417
Definition of an abstract state.
Definition: State.h:50
Representation of a motion.
Definition: SBL.h:147
base::State * state
The state this motion leads to.
Definition: SBL.h:162
std::vector< Motion * > children
The set of motions descending from the current motion.
Definition: SBL.h:171
Motion(const base::SpaceInformationPtr &si)
Constructor that allocates storage for a state.
Definition: SBL.h:153
Motion * parent
The parent motion – it contains the state this motion originates at.
Definition: SBL.h:165
const base::State * root
The root of the tree this motion would get to, if we were to follow parent pointers.
Definition: SBL.h:159
Motion()=default
Default constructor. Allocates no memory.
bool valid
Flag indicating whether this motion has been checked for validity.
Definition: SBL.h:168
Single-Query Bi-Directional Probabilistic Roadmap Planner with Lazy Collision Checking.
Definition: SBL.h:85
void freeGridMotions(Grid< MotionInfo > &grid)
Free the memory used by the motions contained in a grid.
Definition: SBL.cpp:66
void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
Set the projection evaluator. This class is able to compute the projection of a given state.
Definition: SBL.h:94
TreeData tStart_
The start tree.
Definition: SBL.h:258
void freeMemory()
Free the memory allocated by the planner.
Definition: SBL.h:222
bool isPathValid(TreeData &tree, Motion *motion)
Since solutions are computed in a lazy fashion, once trees are connected, the solution found needs to...
Definition: SBL.cpp:242
void removeMotion(TreeData &tree, Motion *motion)
Remove a motion from a tree.
Definition: SBL.cpp:274
base::ProjectionEvaluatorPtr projectionEvaluator_
The employed projection evaluator.
Definition: SBL.h:255
base::ValidStateSamplerPtr sampler_
The employed state sampler.
Definition: SBL.h:252
std::pair< base::State *, base::State * > connectionPoint_
The pair of states in each tree connected during planning. Used for PlannerData computation.
Definition: SBL.h:270
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: SBL.cpp:350
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: SBL.cpp:79
void setProjectionEvaluator(const std::string &name)
Set the projection evaluator (select one from the ones registered with the state space).
Definition: SBL.h:101
void addMotion(TreeData &tree, Motion *motion)
Add a motion to a tree.
Definition: SBL.cpp:330
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: SBL.cpp:55
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: SBL.cpp:368
double getRange() const
Get the range the planner is using.
Definition: SBL.h:123
TreeData tGoal_
The goal tree.
Definition: SBL.h:261
const base::ProjectionEvaluatorPtr & getProjectionEvaluator() const
Get the projection evaluator.
Definition: SBL.h:107
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: SBL.h:117
bool checkSolution(bool start, TreeData &tree, TreeData &otherTree, Motion *motion, std::vector< Motion * > &solution)
Check if a solution can be obtained by connecting two trees using a specified motion.
Definition: SBL.cpp:183
Motion * selectMotion(TreeData &tree)
Select a motion from a tree.
Definition: SBL.cpp:268
SBL(const base::SpaceInformationPtr &si)
The constructor needs the instance of the space information.
Definition: SBL.cpp:43
double maxDistance_
The maximum length of a motion to be added in the tree.
Definition: SBL.h:264
RNG rng_
The random number generator to be used.
Definition: SBL.h:267
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()
Definition: PlannerStatus.h:49
A struct containing an array of motions and a corresponding PDF element.
Definition: SBL.h:176
Representation of a search tree. Two instances will be used. One for start and one for goal.
Definition: SBL.h:208
CellPDF pdf
The PDF used for selecting a cell from which to sample a motion.
Definition: SBL.h:218
unsigned int size
The number of motions (in total) from the tree.
Definition: SBL.h:215
Grid< MotionInfo > grid
The grid of motions corresponding to this tree.
Definition: SBL.h:212