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 49 namespace ompl 50 { 51 namespace geometric 52 { 53 /** 54 @anchor gpSBL 55 @par Short description 56 SBL is a tree-based motion planner that attempts to grow two 57 trees at once: one grows from the starting state and the other 58 from the goal state. The tree expansion strategy is the same as for \ref gEST "EST". 59 Attempts are made to connect these trees 60 at every step of the expansion. If they are connected, a 61 solution path is obtained. However, this solution path is not 62 certain to be valid (the lazy part of the algorithm) so it is 63 checked for validity. If invalid parts are found, they are 64 removed from the tree and exploration of the state space 65 continues until a solution is found. 66 To guide the exploration, an additional grid data 67 structure is maintained. Grid cells contain states that 68 have been previously visited. When deciding which state to 69 use for further expansion, this grid is used; 70 least-filled grid cells have most chances of being selected. The 71 grid is usually imposed on a projection of the state 72 space. This projection needs to be set before using the 73 planner (setProjectionEvaluator() function). Connection of states in different trees is 74 attempted if they fall in the same grid cell. If no projection is 75 set, the planner will attempt to use the default projection 76 associated to the state space. An exception is thrown if 77 no default projection is available either. 78 @par External documentation 79 G. Sánchez and J.-C. Latombe, A single-query bi-directional probabilistic roadmap planner with lazy collision 80 checking, in <em>The Tenth International Symposium on Robotics Research</em>, pp. 403–417, 2001. 81 DOI: [10.1007/3-540-36460-9_27](http://dx.doi.org/10.1007/3-540-36460-9_27)<br> 82 [[PDF]](http://www.springerlink.com/content/9843341054386hh6/fulltext.pdf)</a> 83 */ 84 85 /** \brief Parallel Single-query Bi-directional Lazy collision checking planner */ 86 class pSBL : public base::Planner 87 { 88 public: 89 pSBL(const base::SpaceInformationPtr &si); 90 91 ~pSBL() override; 92 93 /** \brief Set the projection evaluator. This class is 94 able to compute the projection of a given state. */ setProjectionEvaluator(const base::ProjectionEvaluatorPtr & projectionEvaluator)95 void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator) 96 { 97 projectionEvaluator_ = projectionEvaluator; 98 } 99 100 /** \brief Set the projection evaluator (select one from 101 the ones registered with the state space). */ setProjectionEvaluator(const std::string & name)102 void setProjectionEvaluator(const std::string &name) 103 { 104 projectionEvaluator_ = si_->getStateSpace()->getProjection(name); 105 } 106 107 /** \brief Get the projection evaluator. */ getProjectionEvaluator()108 const base::ProjectionEvaluatorPtr &getProjectionEvaluator() const 109 { 110 return projectionEvaluator_; 111 } 112 113 /** \brief Set the range the planner is supposed to use. 114 115 This parameter greatly influences the runtime of the 116 algorithm. It represents the maximum length of a 117 motion to be added in the tree of motions. */ setRange(double distance)118 void setRange(double distance) 119 { 120 maxDistance_ = distance; 121 } 122 123 /** \brief Get the range the planner is using */ getRange()124 double getRange() const 125 { 126 return maxDistance_; 127 } 128 129 /** \brief Set the number of threads the planner should use. Default is 2. */ 130 void setThreadCount(unsigned int nthreads); 131 132 /** \brief Get the thread count */ getThreadCount()133 unsigned int getThreadCount() const 134 { 135 return threadCount_; 136 } 137 138 void setup() override; 139 140 base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override; 141 142 void clear() override; 143 144 void getPlannerData(base::PlannerData &data) const override; 145 146 protected: 147 class Motion; 148 struct MotionInfo; 149 150 /** \brief A grid cell */ 151 using GridCell = Grid<MotionInfo>::Cell; 152 153 /** \brief A PDF of grid cells */ 154 using CellPDF = PDF<GridCell *>; 155 156 class Motion 157 { 158 public: 159 Motion() = default; 160 Motion(const base::SpaceInformationPtr & si)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 176 /** \brief A struct containing an array of motions and a corresponding PDF element */ 177 struct MotionInfo 178 { 179 Motion *operator[](unsigned int i) 180 { 181 return motions_[i]; 182 } beginMotionInfo183 std::vector<Motion *>::iterator begin() 184 { 185 return motions_.begin(); 186 } eraseMotionInfo187 void erase(std::vector<Motion *>::iterator iter) 188 { 189 motions_.erase(iter); 190 } push_backMotionInfo191 void push_back(Motion *m) 192 { 193 motions_.push_back(m); 194 } sizeMotionInfo195 unsigned int size() const 196 { 197 return motions_.size(); 198 } emptyMotionInfo199 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 217 struct SolutionInfo 218 { 219 std::vector<Motion *> solution; 220 bool found; 221 std::mutex lock; 222 }; 223 224 struct PendingRemoveMotion 225 { 226 TreeData *tree; 227 Motion *motion; 228 }; 229 230 struct MotionsToBeRemoved 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 freeMemory()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 253 base::StateSamplerArray<base::ValidStateSampler> samplerArray_; 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 268 /** \brief The pair of states in each tree connected during planning. Used for PlannerData computation */ 269 std::pair<base::State *, base::State *> connectionPoint_{nullptr, nullptr}; 270 }; 271 } 272 } 273 274 #endif 275