Loading...
Searching...
No Matches
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
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...
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: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()
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