MapShape.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 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 #ifndef GAZEBO_PHYSISCS_MAPSHAPE_HH_
18 #define GAZEBO_PHYSISCS_MAPSHAPE_HH_
19 
20 #include <deque>
21 #include <string>
22 
24 #include "gazebo/physics/Shape.hh"
25 #include "gazebo/util/system.hh"
26 
27 namespace gazebo
28 {
29  namespace common
30  {
31  class Image;
32  }
33 
34  namespace physics
35  {
36  class SpaceTree;
37  class QuadNode;
38 
41 
45  class GZ_PHYSICS_VISIBLE 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 ignition::math::Vector3d &_scale);
80 
83  public: virtual ignition::math::Vector3d Scale() 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 GZ_PHYSICS_VISIBLE QuadNode
152  {
155  public: explicit 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
Base class for all shapes.
Definition: Shape.hh:45
Creates box extrusions based on an image.
Definition: MapShape.hh:45
GAZEBO_VISIBLE void Init(google::protobuf::Message &_message, const std::string &_id="")
Initialize a message.
Encapsulates an image.
Definition: Image.hh:75
boost::shared_ptr< Collision > CollisionPtr
Definition: PhysicsTypes.hh:113