BundleSpaceSequence.h
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2019, University of Stuttgart
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 University of Stuttgart nor the names
18  * of its contributors may be used to endorse or promote products
19  * derived from this software without specific prior written
20  * permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 /* Author: Andreas Orthey */
37 
38 #ifndef OMPL_MULTILEVEL_PLANNERS_BUNDLESPACE_BUNDLESPACESEQUENCE_
39 #define OMPL_MULTILEVEL_PLANNERS_BUNDLESPACE_BUNDLESPACESEQUENCE_
40 #include <ompl/multilevel/datastructures/BundleSpace.h>
41 #include <ompl/multilevel/datastructures/PlannerMultiLevel.h>
42 #include <ompl/multilevel/datastructures/pathrestriction/FindSectionTypes.h>
43 #include <type_traits>
44 #include <queue>
45 
46 namespace ompl
47 {
48  namespace base
49  {
51 
52  OMPL_CLASS_FORWARD(GoalState);
54  OMPL_CLASS_FORWARD(GoalStates);
56  }
57  namespace multilevel
58  {
69  template <class T>
70  class BundleSpaceSequence : public PlannerMultiLevel
71  {
73  static_assert(std::is_base_of<BundleSpace, T>::value, "Template must inherit from BundleSpace");
74 
75  public:
80  std::string type = "BundleSpacePlannerNonMultilevel");
81 
84  BundleSpaceSequence(std::vector<ompl::base::SpaceInformationPtr> &siVec,
85  std::string type = "BundleSpacePlanner");
86 
90  BundleSpaceSequence(std::vector<ompl::base::SpaceInformationPtr> &siVec,
91  std::vector<ompl::multilevel::ProjectionPtr> &projVec,
92  std::string type = "BundleSpacePlannerCustomProjection");
93 
94  virtual ~BundleSpaceSequence();
95 
96  void declareBundleSpaces(bool guessProjection = true);
97 
99  virtual void getPlannerData(ompl::base::PlannerData &data) const override;
100 
102 
103  virtual void setup() override;
104  virtual void clear() override;
105  virtual void setProblemDefinition(const ompl::base::ProblemDefinitionPtr &pdef) override;
106 
107  void setStopLevel(unsigned int level_);
108 
111  void setFindSectionStrategy(FindSectionType type);
112 
113  protected:
118  ompl::base::State *getTotalState(int baseLevel, const base::State *baseState) const;
119 
121  std::vector<T *> bundleSpaces_;
122 
124  bool foundKLevelSolution_{false};
125 
127  unsigned int currentBundleSpaceLevel_{0};
128 
132  unsigned int stopAtLevel_;
133 
135  struct CmpBundleSpacePtrs
136  {
137  // ">" operator: smallest value is top in queue
138  // "<" operator: largest value is top in queue (default)
139  bool operator()(const BundleSpace *lhs, const BundleSpace *rhs) const
140  {
141  return lhs->getImportance() < rhs->getImportance();
142  }
143  };
146  typedef std::priority_queue<BundleSpace *, std::vector<BundleSpace *>, CmpBundleSpacePtrs>
148  BundleSpacePriorityQueue priorityQueue_;
149  };
150  } // namespace multilevel
151 } // namespace ompl
152 #include "BundleSpaceSequenceImpl.h"
153 #endif
ompl::base::State * getTotalState(int baseLevel, const base::State *baseState) const
Starting from a baseState on baseLevel, we lift it iteratively upwards into the total space of the se...
A shared pointer wrapper for ompl::base::SpaceInformation.
ompl::base::PlannerStatus solve(const ompl::base::PlannerTerminationCondition &ptc) override
Function that can solve the motion planning problem. This function can be called multiple times on th...
Definition of an abstract state.
Definition: State.h:113
unsigned int currentBundleSpaceLevel_
Current level on which we have not yet found a path.
virtual void setProblemDefinition(const ompl::base::ProblemDefinitionPtr &pdef) override
Set the problem definition for the planner. The problem needs to be set before calling solve()....
std::vector< T * > bundleSpaces_
Sequence of BundleSpaces.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
std::priority_queue< BundleSpace *, std::vector< BundleSpace * >, CmpBundleSpacePtrs > BundleSpacePriorityQueue
Priority queue of BundleSpaces which keeps track of how often every graph on each space has been expa...
BundleSpaceSequence(ompl::base::SpaceInformationPtr si, std::string type="BundleSpacePlannerNonMultilevel")
Non-multilevel Mode: Calling with a single ompl::base::SpaceInformationPtr will revert to standard pl...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
virtual void getPlannerData(ompl::base::PlannerData &data) const override
Return annotated vertices (with information about BundleSpace level)
virtual void clear() override
Clear multilevel planner by clearing all levels.
A class to store the exit status of Planner::solve()
A shared pointer wrapper for ompl::base::ProblemDefinition.
unsigned int stopAtLevel_
Sometimes we only want to plan until a certain BundleSpace level (for debugging for example)....
virtual void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
MultiLevel Planner Interface. Extends base::Planner by allowing sequences of base::SpaceInformationPt...
bool foundKLevelSolution_
Indicator if a solution has been found on the current BundleSpaces.
void setFindSectionStrategy(FindSectionType type)
Set strategy to use to solve the find section problem.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21