All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
MapShape.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2012 Nate Koenig
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 /* Desc: Occupancy grid collision
18  * Author: Nate Koenig
19 */
20 
21 #ifndef _MAPSHAPE_HH_
22 #define _MAPSHAPE_HH_
23 
24 #include <deque>
25 #include <string>
26 
28 
30 #include "gazebo/physics/Shape.hh"
31 
32 namespace gazebo
33 {
34  namespace physics
35  {
36  class SpaceTree;
37  class QuadNode;
38 
41 
45  class MapShape : public Shape
46  {
49  public: explicit MapShape(CollisionPtr _parent);
50 
52  public: virtual ~MapShape();
53 
55  public: void Update();
56 
59  public: virtual void Load(sdf::ElementPtr _sdf);
60 
62  public: virtual void Init();
63 
67  public: void FillMsg(msgs::Geometry &_msg);
68 
71  public: virtual void ProcessMsg(const msgs::Geometry &_msg);
72 
75  public: std::string GetURI() const;
76 
79  public: double GetScale() const;
80 
85  public: int GetThreshold() const;
86 
91  public: double GetHeight() const;
92 
96  public: int GetGranularity() const;
97 
100  private: void BuildTree(QuadNode *_node);
101 
109  private: void GetPixelCount(unsigned int _xStart, unsigned int _yStart,
110  unsigned int _width, unsigned int _height,
111  unsigned int &_freePixels,
112  unsigned int &_occPixels);
113 
116  private: void ReduceTree(QuadNode *_node);
117 
121  private: void Merge(QuadNode *_nodeA, QuadNode *_nodeB);
122 
124  private: void CreateBox();
125 
128  private: void CreateBoxes(QuadNode *_node);
129 
131  private: common::Image *mapImage;
132 
134  private: QuadNode *root;
135 
137  private: bool merged;
138 
141  private: static unsigned int collisionCounter;
142  };
143 
144 
147  class QuadNode
148  {
151  public: QuadNode(QuadNode *_parent)
152  : x(0), y(0), width(0), height(0)
153  {
154  parent = _parent;
155  occupied = false;
156  leaf = true;
157  valid = true;
158  }
159 
161  public: ~QuadNode()
162  {
163  std::deque<QuadNode*>::iterator iter;
164  for (iter = children.begin(); iter != children.end(); ++iter)
165  delete (*iter);
166  }
167 
170  public: void Print(std::string _space)
171  {
172  std::deque<QuadNode*>::iterator iter;
173 
174  printf("%sXY[%d %d] WH[%d %d] O[%d] L[%d] V[%d]\n",
175  _space.c_str(), x, y, width, height, occupied, leaf, valid);
176  _space += " ";
177  for (iter = children.begin(); iter != children.end(); ++iter)
178  if ((*iter)->occupied)
179  (*iter)->Print(_space);
180  }
181 
183  public: uint32_t x, y;
184 
186  public: uint32_t width, height;
187 
189  public: QuadNode *parent;
190 
192  public: std::deque<QuadNode*> children;
193 
195  public: bool occupied;
196 
198  public: bool leaf;
199 
201  public: bool valid;
202  };
204 
206  }
207 }
208 #endif