Loading...
Searching...
No Matches
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
46namespace 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>
71 {
73 static_assert(std::is_base_of<BundleSpace, T>::value, "Template must inherit from BundleSpace");
74
75 public:
79 BundleSpaceSequence(ompl::base::SpaceInformationPtr si,
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
125
128
132 unsigned int stopAtLevel_;
133
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 };
144
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
Definition of a goal state.
Definition GoalState.h:49
Definition of a set of goal states.
Definition GoalStates.h:50
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...
Definition of an abstract state.
Definition State.h:50
void setFindSectionStrategy(FindSectionType type)
Set strategy to use to solve the find section problem.
virtual void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
BundleSpaceSequence(std::vector< ompl::base::SpaceInformationPtr > &siVec, std::vector< ompl::multilevel::ProjectionPtr > &projVec, std::string type="BundleSpacePlannerCustomProjection")
Advanced Mode: Specify not only the vector of ompl::base::SpaceInformationPtr, but also how we should...
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::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(std::vector< ompl::base::SpaceInformationPtr > &siVec, std::string type="BundleSpacePlanner")
Basic Mode: Specify vector of ompl::base::SpaceInformationPtr and let the algorithm figure out the pr...
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...
virtual void clear() override
Clear multilevel planner by clearing all levels.
virtual void getPlannerData(ompl::base::PlannerData &data) const override
Return annotated vertices (with information about BundleSpace level)
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...
BundleSpaceSequence(ompl::base::SpaceInformationPtr si, std::string type="BundleSpacePlannerNonMultilevel")
Non-multilevel Mode: Calling with a single ompl::base::SpaceInformationPtr will revert to standard pl...
A single Bundle-space.
Definition BundleSpace.h:64
virtual double getImportance() const =0
Compute importance of bundle space (to decide where to.
MultiLevel Planner Interface. Extends base::Planner by allowing sequences of base::SpaceInformationPt...
PlannerMultiLevel(std::vector< ompl::base::SpaceInformationPtr > &siVec, std::string type="PlannerMultiLevel")
Constructor for a set of ompl::base::SpaceInformationPtr which represent different abstraction levels...
This namespace contains sampling based planning routines shared by both planning under geometric cons...
This namespace contains datastructures and planners to exploit multilevel abstractions,...
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()