1 /********************************************************************* 2 * Software License Agreement (BSD License) 3 * 4 * Copyright (c) 2010, Rice University 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 Rice University 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_BASE_PLANNER_ 38 #define OMPL_BASE_PLANNER_ 39 40 #include "ompl/base/SpaceInformation.h" 41 #include "ompl/base/ProblemDefinition.h" 42 #include "ompl/base/PlannerData.h" 43 #include "ompl/base/PlannerStatus.h" 44 #include "ompl/base/PlannerTerminationCondition.h" 45 #include "ompl/base/GenericParam.h" 46 #include "ompl/util/Console.h" 47 #include "ompl/util/Time.h" 48 #include "ompl/util/ClassForward.h" 49 #include <functional> 50 #include <boost/concept_check.hpp> 51 #include <string> 52 #include <map> 53 54 namespace ompl 55 { 56 namespace base 57 { 58 /// @cond IGNORE 59 /** \brief Forward declaration of ompl::base::Planner */ 60 OMPL_CLASS_FORWARD(Planner); 61 /// @endcond 62 63 /** \class ompl::base::PlannerPtr 64 \brief A shared pointer wrapper for ompl::base::Planner */ 65 66 /** \brief Helper class to extract valid start & goal 67 states. Usually used internally by planners. 68 69 This class is meant to behave correctly if the user 70 updates the problem definition between subsequent calls to 71 ompl::base::Planner::solve() \b without calling 72 ompl::base::Planner::clear() in between. Only allowed 73 changes to the problem definition are accounted for: 74 adding of starring states or adding of goal states for 75 instances inherited from 76 ompl::base::GoalSampleableRegion. */ 77 class PlannerInputStates 78 { 79 public: 80 /** \brief Default constructor. No work is performed. */ PlannerInputStates(const PlannerPtr & planner)81 PlannerInputStates(const PlannerPtr &planner) : planner_(planner.get()) 82 { 83 tempState_ = nullptr; 84 update(); 85 } 86 87 /** \brief Default constructor. No work is performed. */ PlannerInputStates(const Planner * planner)88 PlannerInputStates(const Planner *planner) : planner_(planner) 89 { 90 tempState_ = nullptr; 91 update(); 92 } 93 94 /** \brief Default constructor. No work is performed. A 95 call to use() needs to be made, before making any 96 calls to nextStart() or nextGoal(). */ PlannerInputStates()97 PlannerInputStates() 98 { 99 tempState_ = nullptr; 100 clear(); 101 } 102 103 /** \brief Destructor. Clear allocated memory. */ ~PlannerInputStates()104 ~PlannerInputStates() 105 { 106 clear(); 107 } 108 109 /** \brief Clear all stored information. */ 110 void clear(); 111 112 /** \brief Forget how many states were returned by 113 nextStart() and nextGoal() and return all states 114 again */ 115 void restart(); 116 117 /** \brief Set the space information and problem 118 definition this class operates on, based on the 119 available planner instance. Returns true if changes 120 were found (different problem definition) and clear() 121 was called. */ 122 bool update(); 123 124 /** \brief Set the problem definition this class operates on. 125 If a planner is not set in the constructor argument, a call 126 to this function is needed before any calls to nextStart() 127 or nextGoal() are made. Returns true if changes were found 128 (different problem definition) and clear() was called. */ 129 bool use(const ProblemDefinitionPtr &pdef); 130 131 /** \brief Set the problem definition this class operates on. 132 If a planner is not set in the constructor argument, a call 133 to this function is needed before any calls to nextStart() 134 or nextGoal() are made. Returns true if changes were found 135 (different problem definition) and clear() was called. */ 136 bool use(const ProblemDefinition *pdef); 137 138 /** \brief Check if the problem definition was set, start 139 state are available and goal was set */ 140 void checkValidity() const; 141 142 /** \brief Return the next valid start state or nullptr if no 143 more valid start states are available. */ 144 const State *nextStart(); 145 146 /** \brief Return the next valid goal state or nullptr if no 147 more valid goal states are available. Because 148 sampling of goal states may also produce invalid 149 goals, this function takes an argument that specifies 150 whether a termination condition has been reached. If 151 the termination condition evaluates to true the 152 function terminates even if no valid goal has been 153 found. */ 154 const State *nextGoal(const PlannerTerminationCondition &ptc); 155 156 /** \brief Same as above but only one attempt is made to find a valid goal. */ 157 const State *nextGoal(); 158 159 /** \brief Check if there are more potential start states */ 160 bool haveMoreStartStates() const; 161 162 /** \brief Check if there are more potential goal states */ 163 bool haveMoreGoalStates() const; 164 165 /** \brief Get the number of start states from the problem 166 definition that were already seen, including invalid 167 ones. */ getSeenStartStatesCount()168 unsigned int getSeenStartStatesCount() const 169 { 170 return addedStartStates_; 171 } 172 173 /** \brief Get the number of sampled goal states, including invalid ones */ getSampledGoalsCount()174 unsigned int getSampledGoalsCount() const 175 { 176 return sampledGoalsCount_; 177 } 178 179 private: 180 const Planner *planner_{nullptr}; 181 182 unsigned int addedStartStates_; 183 unsigned int sampledGoalsCount_; 184 State *tempState_; 185 186 const ProblemDefinition *pdef_; 187 const SpaceInformation *si_; 188 }; 189 190 /** \brief Properties that planners may have */ 191 struct PlannerSpecs 192 { 193 PlannerSpecs() = default; 194 195 /** \brief The type of goal specification the planner can use */ 196 GoalType recognizedGoal{GOAL_ANY}; 197 198 /** \brief Flag indicating whether multiple threads are used in the computation of the planner */ 199 bool multithreaded{false}; 200 201 /** \brief Flag indicating whether the planner is able to compute approximate solutions */ 202 bool approximateSolutions{false}; 203 204 /** \brief Flag indicating whether the planner attempts to optimize the path and reduce its length until the 205 maximum path length specified by the goal representation is satisfied */ 206 bool optimizingPaths{false}; 207 208 /** \brief Flag indicating whether the planner is able to account for the fact that the validity of a motion 209 from A to B may not be the same as the validity of a motion from B to A. 210 If this flag is true, the planner will return solutions that do not make this assumption. Usually 211 roadmap-based planners make this assumption and tree-based planners do not. */ 212 bool directed{false}; 213 214 /** \brief Flag indicating whether the planner is able to prove that no solution path exists. */ 215 bool provingSolutionNonExistence{false}; 216 217 /** \brief Flag indicating whether the planner is able to report the computation of intermediate paths. */ 218 bool canReportIntermediateSolutions{false}; 219 }; 220 221 /** \brief Base class for a planner */ 222 class Planner 223 { 224 public: 225 // non-copyable 226 Planner(const Planner &) = delete; 227 Planner &operator=(const Planner &) = delete; 228 229 /** \brief Constructor */ 230 Planner(SpaceInformationPtr si, std::string name); 231 232 /** \brief Destructor */ 233 virtual ~Planner() = default; 234 235 /** \brief Cast this instance to a desired type. */ 236 template <class T> as()237 T *as() 238 { 239 /** \brief Make sure the type we are casting to is indeed a planner */ 240 BOOST_CONCEPT_ASSERT((boost::Convertible<T *, Planner *>)); 241 242 return static_cast<T *>(this); 243 } 244 245 /** \brief Cast this instance to a desired type. */ 246 template <class T> as()247 const T *as() const 248 { 249 /** \brief Make sure the type we are casting to is indeed a Planner */ 250 BOOST_CONCEPT_ASSERT((boost::Convertible<T *, Planner *>)); 251 252 return static_cast<const T *>(this); 253 } 254 255 /** \brief Get the space information this planner is using */ 256 const SpaceInformationPtr &getSpaceInformation() const; 257 258 /** \brief Get the problem definition the planner is trying to solve */ 259 const ProblemDefinitionPtr &getProblemDefinition() const; 260 261 /** \brief Get the problem definition the planner is trying to solve */ 262 ProblemDefinitionPtr &getProblemDefinition(); 263 264 /** \brief Get the planner input states */ 265 const PlannerInputStates &getPlannerInputStates() const; 266 267 /** \brief Set the problem definition for the planner. The 268 problem needs to be set before calling solve(). Note: 269 If this problem definition replaces a previous one, it 270 may also be necessary to call clear() or clearQuery(). */ 271 virtual void setProblemDefinition(const ProblemDefinitionPtr &pdef); 272 273 /** \brief Function that can solve the motion planning problem. This 274 function can be called multiple times on the same problem, 275 without calling clear() in between. This allows the planner to 276 continue work for more time on an unsolved problem, for example. 277 If this option is used, it is assumed the problem definition is 278 not changed (unpredictable results otherwise). The only change 279 in the problem definition that is accounted for is the addition 280 of starting or goal states (but not changing previously added 281 start/goal states). If clearQuery() is called, the planner may 282 retain prior datastructures generated from a previous query on a 283 new problem definition. The function terminates if the call to 284 \e ptc returns true. */ 285 virtual PlannerStatus solve(const PlannerTerminationCondition &ptc) = 0; 286 287 /** \brief Same as above except the termination condition 288 is only evaluated at a specified interval. */ 289 PlannerStatus solve(const PlannerTerminationConditionFn &ptc, double checkInterval); 290 291 /** \brief Same as above except the termination condition 292 is solely a time limit: the number of seconds the 293 algorithm is allowed to spend planning. */ 294 PlannerStatus solve(double solveTime); 295 296 /** \brief Clear all internal datastructures. Planner 297 settings are not affected. Subsequent calls to solve() 298 will ignore all previous work. */ 299 virtual void clear(); 300 301 /** \brief Clears internal datastructures of any query-specific 302 information from the previous query. Planner settings are not 303 affected. The planner, if able, should retain all datastructures 304 generated from previous queries that can be used to help solve 305 the next query. Note that clear() should also clear all 306 query-specific information along with all other datastructures 307 in the planner. By default clearQuery() calls clear(). */ 308 virtual void clearQuery(); 309 310 /** \brief Get information about the current run of the 311 motion planner. Repeated calls to this function will 312 update \e data (only additions are made). This is 313 useful to see what changed in the exploration 314 datastructure, between calls to solve(), for example 315 (without calling clear() in between). */ 316 virtual void getPlannerData(PlannerData &data) const; 317 318 /** \brief Get the name of the planner */ 319 const std::string &getName() const; 320 321 /** \brief Set the name of the planner */ 322 void setName(const std::string &name); 323 324 /** \brief Return the specifications (capabilities of this planner) */ 325 const PlannerSpecs &getSpecs() const; 326 327 /** \brief Perform extra configuration steps, if 328 needed. This call will also issue a call to 329 ompl::base::SpaceInformation::setup() if needed. This 330 must be called before solving */ 331 virtual void setup(); 332 333 /** \brief Check to see if the planner is in a working 334 state (setup has been called, a goal was set, the 335 input states seem to be in order). In case of error, 336 this function throws an exception.*/ 337 virtual void checkValidity(); 338 339 /** \brief Check if setup() was called for this planner */ 340 bool isSetup() const; 341 342 /** \brief Get the parameters for this planner */ params()343 ParamSet ¶ms() 344 { 345 return params_; 346 } 347 348 /** \brief Get the parameters for this planner */ params()349 const ParamSet ¶ms() const 350 { 351 return params_; 352 } 353 354 /** \brief Definition of a function which returns a property about the planner's progress that can be 355 * queried by a benchmarking routine */ 356 using PlannerProgressProperty = std::function<std::string()>; 357 358 /** \brief A dictionary which maps the name of a progress property to the function to be used for querying 359 * that property */ 360 using PlannerProgressProperties = std::map<std::string, PlannerProgressProperty>; 361 362 /** \brief Retrieve a planner's planner progress property map */ getPlannerProgressProperties()363 const PlannerProgressProperties &getPlannerProgressProperties() const 364 { 365 return plannerProgressProperties_; 366 } 367 368 /** \brief Print properties of the motion planner */ 369 virtual void printProperties(std::ostream &out) const; 370 371 /** \brief Print information about the motion planner's settings */ 372 virtual void printSettings(std::ostream &out) const; 373 374 protected: 375 /** \brief This function declares a parameter for this planner instance, and specifies the setter and getter 376 * functions. */ 377 template <typename T, typename PlannerType, typename SetterType, typename GetterType> 378 void declareParam(const std::string &name, const PlannerType &planner, const SetterType &setter, 379 const GetterType &getter, const std::string &rangeSuggestion = "") 380 { 381 params_.declareParam<T>(name, 382 [planner, setter](T param) 383 { 384 (*planner.*setter)(param); 385 }, 386 [planner, getter] 387 { 388 return (*planner.*getter)(); 389 }); 390 if (!rangeSuggestion.empty()) 391 params_[name].setRangeSuggestion(rangeSuggestion); 392 } 393 394 /** \brief This function declares a parameter for this planner instance, and specifies the setter function. 395 */ 396 template <typename T, typename PlannerType, typename SetterType> 397 void declareParam(const std::string &name, const PlannerType &planner, const SetterType &setter, 398 const std::string &rangeSuggestion = "") 399 { 400 params_.declareParam<T>(name, [planner, setter](T param) 401 { 402 (*planner.*setter)(param); 403 }); 404 if (!rangeSuggestion.empty()) 405 params_[name].setRangeSuggestion(rangeSuggestion); 406 } 407 408 /** \brief Add a planner progress property called \e progressPropertyName with a property querying function 409 * \e prop to this planner's progress property map */ addPlannerProgressProperty(const std::string & progressPropertyName,const PlannerProgressProperty & prop)410 void addPlannerProgressProperty(const std::string &progressPropertyName, 411 const PlannerProgressProperty &prop) 412 { 413 plannerProgressProperties_[progressPropertyName] = prop; 414 } 415 416 /** \brief The space information for which planning is done */ 417 SpaceInformationPtr si_; 418 419 /** \brief The user set problem definition */ 420 ProblemDefinitionPtr pdef_; 421 422 /** \brief Utility class to extract valid input states */ 423 PlannerInputStates pis_; 424 425 /** \brief The name of this planner */ 426 std::string name_; 427 428 /** \brief The specifications of the planner (its capabilities) */ 429 PlannerSpecs specs_; 430 431 /** \brief A map from parameter names to parameter instances for this planner. This field is populated by 432 * the declareParam() function */ 433 ParamSet params_; 434 435 /** \brief A mapping between this planner's progress property names and the functions used for querying 436 * those progress properties */ 437 PlannerProgressProperties plannerProgressProperties_; 438 439 /** \brief Flag indicating whether setup() has been called */ 440 bool setup_; 441 }; 442 443 /** \brief Definition of a function that can allocate a planner */ 444 using PlannerAllocator = std::function<PlannerPtr(const SpaceInformationPtr &)>; 445 } 446 } 447 448 #endif 449