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 Open Source Robotics Foundation
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: void SetScale(const math::Vector3 &_scale);
80 
83  public: virtual math::Vector3 GetScale() const;
84 
89  public: int GetThreshold() const;
90 
95  public: double GetHeight() const;
96 
100  public: int GetGranularity() const;
101 
104  private: void BuildTree(QuadNode *_node);
105 
113  private: void GetPixelCount(unsigned int _xStart, unsigned int _yStart,
114  unsigned int _width, unsigned int _height,
115  unsigned int &_freePixels,
116  unsigned int &_occPixels);
117 
120  private: void ReduceTree(QuadNode *_node);
121 
125  private: void Merge(QuadNode *_nodeA, QuadNode *_nodeB);
126 
128  private: void CreateBox();
129 
132  private: void CreateBoxes(QuadNode *_node);
133 
135  private: common::Image *mapImage;
136 
138  private: QuadNode *root;
139 
141  private: bool merged;
142 
145  private: static unsigned int collisionCounter;
146  };
147 
148 
151  class QuadNode
152  {
155  public: QuadNode(QuadNode *_parent)
156  : x(0), y(0), width(0), height(0)
157  {
158  parent = _parent;
159  occupied = false;
160  leaf = true;
161  valid = true;
162  }
163 
165  public: ~QuadNode()
166  {
167  std::deque<QuadNode*>::iterator iter;
168  for (iter = children.begin(); iter != children.end(); ++iter)
169  delete (*iter);
170  }
171 
174  public: void Print(std::string _space)
175  {
176  std::deque<QuadNode*>::iterator iter;
177 
178  printf("%sXY[%d %d] WH[%d %d] O[%d] L[%d] V[%d]\n",
179  _space.c_str(), x, y, width, height, occupied, leaf, valid);
180  _space += " ";
181  for (iter = children.begin(); iter != children.end(); ++iter)
182  if ((*iter)->occupied)
183  (*iter)->Print(_space);
184  }
185 
187  public: uint32_t x, y;
188 
190  public: uint32_t width, height;
191 
193  public: QuadNode *parent;
194 
196  public: std::deque<QuadNode*> children;
197 
199  public: bool occupied;
200 
202  public: bool leaf;
203 
205  public: bool valid;
206  };
208 
210  }
211 }
212 #endif