dynamicEDT3D.h
Go to the documentation of this file.
1 
9 /*
10  * Copyright (c) 2011-2012, C. Sprunk, B. Lau, W. Burgard, University of Freiburg
11  * All rights reserved.
12  *
13  * Redistribution and use in source and binary forms, with or without
14  * modification, are permitted provided that the following conditions are met:
15  *
16  * * Redistributions of source code must retain the above copyright
17  * notice, this list of conditions and the following disclaimer.
18  * * Redistributions in binary form must reproduce the above copyright
19  * notice, this list of conditions and the following disclaimer in the
20  * documentation and/or other materials provided with the distribution.
21  * * Neither the name of the University of Freiburg nor the names of its
22  * contributors may be used to endorse or promote products derived from
23  * this software without specific prior written permission.
24  *
25  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
26  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
27  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
28  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
29  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
30  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
31  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
32  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
33  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
34  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  */
37 
38 #ifndef _DYNAMICEDT3D_H_
39 #define _DYNAMICEDT3D_H_
40 
41 #include <limits.h>
42 #include <queue>
43 
44 #include "bucketedqueue.h"
45 
47 class DynamicEDT3D {
48 
49 public:
50 
51  DynamicEDT3D(int _maxdist_squared);
52  ~DynamicEDT3D();
53 
55  void initializeEmpty(int _sizeX, int _sizeY, int sizeZ, bool initGridMap=true);
57  void initializeMap(int _sizeX, int _sizeY, int sizeZ, bool*** _gridMap);
58 
60  void occupyCell(int x, int y, int z);
62  void clearCell(int x, int y, int z);
64  void exchangeObstacles(std::vector<INTPOINT3D> newObstacles);
65 
67  virtual void update(bool updateRealDist=true);
68 
70  float getDistance( int x, int y, int z ) const;
72  INTPOINT3D getClosestObstacle( int x, int y, int z ) const;
73 
75  int getSQCellDistance( int x, int y, int z ) const;
77  bool isOccupied(int x, int y, int z) const;
78 
80  unsigned int getSizeX() const {return sizeX;}
82  unsigned int getSizeY() const {return sizeY;}
84  unsigned int getSizeZ() const {return sizeZ;}
85 
86  typedef enum {invalidObstData = INT_MAX} ObstDataState;
87 
89  static float distanceValue_Error;
92 
93 protected:
94  struct dataCell {
95  float dist;
96  int obstX;
97  int obstY;
98  int obstZ;
99  int sqdist;
100  char queueing;
102  };
103 
104  typedef enum {free=0, occupied=1} State;
106 
107  // methods
108  inline void raiseCell(INTPOINT3D &p, dataCell &c, bool updateRealDist);
109  inline void propagateCell(INTPOINT3D &p, dataCell &c, bool updateRealDist);
110  inline void inspectCellRaise(int &nx, int &ny, int &nz, bool updateRealDist);
111  inline void inspectCellPropagate(int &nx, int &ny, int &nz, dataCell &c, bool updateRealDist);
112 
113  void setObstacle(int x, int y, int z);
114  void removeObstacle(int x, int y, int z);
115 
116 private:
117  void commitAndColorize(bool updateRealDist=true);
118 
119  inline bool isOccupied(int &x, int &y, int &z, dataCell &c);
120 
121  // queues
123 
124  std::vector<INTPOINT3D> removeList;
125  std::vector<INTPOINT3D> addList;
126  std::vector<INTPOINT3D> lastObstacles;
127 
128  // maps
129 protected:
130  int sizeX;
131  int sizeY;
132  int sizeZ;
133  int sizeXm1;
134  int sizeYm1;
135  int sizeZm1;
136 
138  bool*** gridMap;
139 
140  // parameters
141  int padding;
143 
144  double sqrt2;
145  double maxDist;
147 };
148 
149 
150 #endif
151 
void initializeMap(int _sizeX, int _sizeY, int sizeZ, bool ***_gridMap)
Initialization with a given binary map (false==free, true==occupied)
void clearCell(int x, int y, int z)
remove an obstacle at the specified cell coordinate
void setObstacle(int x, int y, int z)
static int distanceInCellsValue_Error
distance value returned when requesting distance in cell units for a cell outside the map ...
Definition: dynamicEDT3D.h:91
std::vector< INTPOINT3D > removeList
Definition: dynamicEDT3D.h:124
void raiseCell(INTPOINT3D &p, dataCell &c, bool updateRealDist)
float getDistance(int x, int y, int z) const
returns the obstacle distance at the specified location
void occupyCell(int x, int y, int z)
add an obstacle at the specified cell coordinate
std::vector< INTPOINT3D > lastObstacles
Definition: dynamicEDT3D.h:126
dataCell *** data
Definition: dynamicEDT3D.h:137
void propagateCell(INTPOINT3D &p, dataCell &c, bool updateRealDist)
BucketPrioQueue< INTPOINT3D > open
Definition: dynamicEDT3D.h:122
unsigned int getSizeZ() const
returns the z size of the workspace/map
Definition: dynamicEDT3D.h:84
void exchangeObstacles(std::vector< INTPOINT3D > newObstacles)
remove old dynamic obstacles and add the new ones
void initializeEmpty(int _sizeX, int _sizeY, int sizeZ, bool initGridMap=true)
Initialization with an empty map.
void commitAndColorize(bool updateRealDist=true)
#define INTPOINT3D
Definition: point.h:42
std::vector< INTPOINT3D > addList
Definition: dynamicEDT3D.h:125
double maxDist
Definition: dynamicEDT3D.h:145
static float distanceValue_Error
distance value returned when requesting distance for a cell outside the map
Definition: dynamicEDT3D.h:89
void inspectCellRaise(int &nx, int &ny, int &nz, bool updateRealDist)
bool *** gridMap
Definition: dynamicEDT3D.h:138
A DynamicEDT3D object computes and updates a 3D distance map.
Definition: dynamicEDT3D.h:47
int getSQCellDistance(int x, int y, int z) const
returns the squared obstacle distance in cell units at the specified location
bool isOccupied(int x, int y, int z) const
checks whether the specficied location is occupied
virtual void update(bool updateRealDist=true)
update distance map to reflect the changes
void removeObstacle(int x, int y, int z)
INTPOINT3D getClosestObstacle(int x, int y, int z) const
gets the closest occupied cell for that location
unsigned int getSizeY() const
returns the y size of the workspace/map
Definition: dynamicEDT3D.h:82
unsigned int getSizeX() const
returns the x size of the workspace/map
Definition: dynamicEDT3D.h:80
DynamicEDT3D(int _maxdist_squared)
double doubleThreshold
Definition: dynamicEDT3D.h:142
void inspectCellPropagate(int &nx, int &ny, int &nz, dataCell &c, bool updateRealDist)


dynamicEDT3D
Author(s): Christoph Sprunk
autogenerated on Wed Jun 5 2019 19:26:37