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 (C) 2012-2014 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 #include "gazebo/util/system.hh"
32 
33 namespace gazebo
34 {
35  namespace physics
36  {
37  class SpaceTree;
38  class QuadNode;
39 
42 
46  class GAZEBO_VISIBLE MapShape : public Shape
47  {
50  public: explicit MapShape(CollisionPtr _parent);
51 
53  public: virtual ~MapShape();
54 
56  public: void Update();
57 
60  public: virtual void Load(sdf::ElementPtr _sdf);
61 
63  public: virtual void Init();
64 
68  public: void FillMsg(msgs::Geometry &_msg);
69 
72  public: virtual void ProcessMsg(const msgs::Geometry &_msg);
73 
76  public: std::string GetURI() const;
77 
80  public: void SetScale(const math::Vector3 &_scale);
81 
84  public: virtual math::Vector3 GetScale() const;
85 
90  public: int GetThreshold() const;
91 
96  public: double GetHeight() const;
97 
101  public: int GetGranularity() const;
102 
105  private: void BuildTree(QuadNode *_node);
106 
114  private: void GetPixelCount(unsigned int _xStart, unsigned int _yStart,
115  unsigned int _width, unsigned int _height,
116  unsigned int &_freePixels,
117  unsigned int &_occPixels);
118 
121  private: void ReduceTree(QuadNode *_node);
122 
126  private: void Merge(QuadNode *_nodeA, QuadNode *_nodeB);
127 
129  private: void CreateBox();
130 
133  private: void CreateBoxes(QuadNode *_node);
134 
136  private: common::Image *mapImage;
137 
139  private: QuadNode *root;
140 
142  private: bool merged;
143 
146  private: static unsigned int collisionCounter;
147  };
148 
149 
153  {
156  public: QuadNode(QuadNode *_parent)
157  : x(0), y(0), width(0), height(0)
158  {
159  parent = _parent;
160  occupied = false;
161  leaf = true;
162  valid = true;
163  }
164 
166  public: ~QuadNode()
167  {
168  std::deque<QuadNode*>::iterator iter;
169  for (iter = children.begin(); iter != children.end(); ++iter)
170  delete (*iter);
171  }
172 
175  public: void Print(std::string _space)
176  {
177  std::deque<QuadNode*>::iterator iter;
178 
179  printf("%sXY[%d %d] WH[%d %d] O[%d] L[%d] V[%d]\n",
180  _space.c_str(), x, y, width, height, occupied, leaf, valid);
181  _space += " ";
182  for (iter = children.begin(); iter != children.end(); ++iter)
183  if ((*iter)->occupied)
184  (*iter)->Print(_space);
185  }
186 
188  public: uint32_t x, y;
189 
191  public: uint32_t width, height;
192 
194  public: QuadNode *parent;
195 
197  public: std::deque<QuadNode*> children;
198 
200  public: bool occupied;
201 
203  public: bool leaf;
204 
206  public: bool valid;
207  };
209 
211  }
212 }
213 #endif