XXLPlanarDecomposition.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2015, 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: Ryan Luna */
36 
37 #ifndef OMPL_GEOMETRIC_PLANNERS_XXL_XXLPLANARDECOMPOSITION_
38 #define OMPL_GEOMETRIC_PLANNERS_XXL_XXLPLANARDECOMPOSITION_
39 
40 #include <boost/math/constants/constants.hpp>
41 #include "ompl/geometric/planners/xxl/XXLDecomposition.h"
42 #include "ompl/util/RandomNumbers.h"
43 
44 namespace ompl
45 {
46  namespace geometric
47  {
48  // A decomposition of a 2D planar space (position and orientation in 2D)
49  // The orientation part of the decomposition is circular (2pi = 0)
50  class XXLPlanarDecomposition : public XXLDecomposition
51  {
52  public:
53  XXLPlanarDecomposition(const base::RealVectorBounds& xyBounds, const std::vector<int>& xySlices,
54  const int thetaSlices, bool diagonalEdges = false);
55 
56  XXLPlanarDecomposition(const base::RealVectorBounds& xyBounds, const std::vector<int>& xySlices,
57  const int thetaSlices, double thetaLowerBound, double thetaUpperBound, bool diagonalEdges = false);
58 
59  virtual ~XXLPlanarDecomposition();
60 
62  virtual int getNumRegions() const;
63 
65  virtual int getDimension() const;
66 
68  virtual int numLayers() const = 0;
69 
71  virtual int locateRegion(const base::State *s) const;
72 
74  virtual int locateRegion(const std::vector<double>& coord) const;
75 
77  virtual void getNeighbors(int rid, std::vector<int>& neighbors) const;
78 
81  virtual void getNeighborhood(int rid, std::vector<int>& neighborhood) const;
82 
84  virtual double distanceHeuristic(int r1, int r2) const;
85 
87  virtual bool sampleFromRegion(int r, base::State* s, const base::State* seed = nullptr) const = 0;
88 
90  virtual bool sampleFromRegion(int r, base::State* s, const base::State* seed, int layer) const = 0;
91 
93  virtual void project(const base::State *s, std::vector<double>& coord, int layer = 0) const = 0;
94 
96  virtual void project(const base::State *s, std::vector<int>& layers) const = 0;
97 
98 
99 
100  // \brief Return the (discrete) grid cell coordinates of the given region id
101  void ridToGridCell(int rid, std::vector<int>& cell) const;
102 
104  int gridCellToRid(const std::vector<int>& cell) const;
105 
107  int coordToRegion(const std::vector<double>& coord) const;
108  int coordToRegion(const double* coord) const;
109 
111  bool hasDiagonalEdges() const;
112 
113  protected:
114  // Create the new graph structure, mostly edge weights
115  void constructGraph();
116 
117  // Compute the neighbors of rid (no diagonal edges)
118  void getNonDiagonalNeighbors(int rid, std::vector<int>& neighbors) const;
119 
120  // Compute the neighbors of rid (including diagonal edges)
121  void getDiagonalNeighbors(int rid, std::vector<int>& neighbors) const;
122 
123  // Sample a point in the SE(2) decomposition (position and orientation)
124  void sampleCoordinateFromRegion(int r, std::vector<double>& coord) const;
125  void sampleCoordinateFromRegion(int r, double* coord) const;
126 
127  // If true, the decomposition has diagonal edges
128  bool diagonalEdges_;
129 
130  // The bounds on the Euclidean part of the decomposition
131  base::RealVectorBounds xyBounds_;
132  // The bounds on the orientation part of the decomposition
133  double thetaLow_{-boost::math::constants::pi<double>()};
134  double thetaHigh_{boost::math::constants::pi<double>()};
135 
136  // The number of splits in the X and Y dimensions
137  std::vector<int> xySlices_;
138  // The number of splits in orientation
139  int thetaSlices_;
140 
141  // The total number of regions
142  int numRegions_;
143 
144  // The total range of x and y
145  double dx_, dy_;
146  // The size of an individual cell in x and y
147  double xSize_, ySize_;
148 
149  // The total range of theta
150  double dTheta_;
151  // The size of an individual cell in theta
152  double thetaSize_;
153 
154  // The realized dimension of this decomposition (from 1-3)
155  int dimension_;
156 
157  // Random number generator.
158  mutable ompl::RNG rng_;
159  };
160  }
161 }
162 
163 
164 #endif
virtual int locateRegion(const base::State *s) const
Return the id of the region that this state falls in.
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:90
int gridCellToRid(const std::vector< int > &cell) const
Return the region id corresponding to the (discrete) grid cell coordinates.
virtual int numLayers() const =0
Return the number of layers possible in this decomposition. Must be at least 1.
virtual bool sampleFromRegion(int r, base::State *s, const base::State *seed=nullptr) const =0
Sample a state s from region r in layer 0.
virtual void project(const base::State *s, std::vector< double > &coord, int layer=0) const =0
Project the given State into the XXLDecomposition.
virtual int getNumRegions() const
Return the total number of regions in this decomposition.
virtual void getNeighbors(int rid, std::vector< int > &neighbors) const
Stores the given region's neighbors into a given vector.
virtual double distanceHeuristic(int r1, int r2) const
An admissible and consistent distance heuristic between two regions. Manhattan distance on grid.
int coordToRegion(const std::vector< double > &coord) const
Return the region id of the given coordinate in the decomposition.
virtual int getDimension() const
Return the dimension of this HiLoDecomposition.
virtual void getNeighborhood(int rid, std::vector< int > &neighborhood) const
Stores the given region's neighbors into the vector. This returns the 8-connected grid neighbors of t...
bool hasDiagonalEdges() const
Return true if the decomposition has diagonal edges.
Main namespace. Contains everything in this library.
Definition: AppBase.h:22
The lower and upper bounds for an Rn space.