Revision: 7734
http://playerstage.svn.sourceforge.net/playerstage/?rev=7734&view=rev
Author: natepak
Date: 2009-05-28 23:17:04 +0000 (Thu, 28 May 2009)
Log Message:
-----------
Made a generic mapgeom
Added Paths:
-----------
code/gazebo/branches/bullet/server/physics/MapGeom.cc
code/gazebo/branches/bullet/server/physics/MapGeom.hh
Removed Paths:
-------------
code/gazebo/branches/bullet/server/physics/BulletMapGeom.cc
code/gazebo/branches/bullet/server/physics/BulletMapGeom.hh
code/gazebo/branches/bullet/server/physics/ode/ODEMapGeom.cc
code/gazebo/branches/bullet/server/physics/ode/ODEMapGeom.hh
Deleted: code/gazebo/branches/bullet/server/physics/BulletMapGeom.cc
===================================================================
--- code/gazebo/branches/bullet/server/physics/BulletMapGeom.cc 2009-05-28
23:16:05 UTC (rev 7733)
+++ code/gazebo/branches/bullet/server/physics/BulletMapGeom.cc 2009-05-28
23:17:04 UTC (rev 7734)
@@ -1,437 +0,0 @@
-/*
- * Gazebo - Outdoor Multi-Robot Simulator
- * Copyright (C) 2003
- * Nate Koenig & Andrew Howard
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- */
-/* Desc: Map geometry
- * Author: Nate Koenig
- * Date: 14 July 2008
- * CVS: $Id: MapGeom.cc 7661 2009-05-14 15:54:32Z natepak $
- */
-
-
-#include <iostream>
-#include <string.h>
-#include <math.h>
-
-#include "GazeboConfig.hh"
-#include "OgreVisual.hh"
-#include "Image.hh"
-#include "GazeboError.hh"
-#include "Simulator.hh"
-#include "Global.hh"
-#include "Body.hh"
-#include "MapGeom.hh"
-
-using namespace gazebo;
-
-unsigned int MapGeom::geomCounter = 0;
-
-//////////////////////////////////////////////////////////////////////////////
-// Constructor
-MapGeom::MapGeom(Body *body)
- : Geom(body)
-{
- this->root = new QuadNode(NULL);
-
- Param::Begin(&this->parameters);
- this->negativeP = new ParamT<int>("negative", 0, 0);
- this->thresholdP = new ParamT<double>( "threshold", 200.0, 0);
- this->wallHeightP = new ParamT<double>( "height", 1.0, 0 );
- this->scaleP = new ParamT<double>("scale",1.0,0);
- this->materialP = new ParamT<std::string>("material", "", 0);
- this->granularityP = new ParamT<int>("granularity", 5, 0);
- Param::End();
-
-}
-
-
-//////////////////////////////////////////////////////////////////////////////
-// Destructor
-MapGeom::~MapGeom()
-{
- if (this->root)
- delete this->root;
-
- if (this->mapImage)
- delete this->mapImage;
- this->mapImage = NULL;
-
- delete this->negativeP;
- delete this->thresholdP;
- delete this->wallHeightP;
- delete this->scaleP;
- delete this->materialP;
- delete this->granularityP;
-}
-
-//////////////////////////////////////////////////////////////////////////////
-/// Update function.
-void MapGeom::Update()
-{
- Geom::Update();
-}
-
-////////////////////////////////////////////////////////////////////////////////
-/// Load the heightmap
-void MapGeom::Load(XMLConfigNode *node)
-{
- Geom::Load(node);
-
- std::string imageFilename = node->GetString("image","",1);
-
- this->negativeP->Load(node);
- this->thresholdP->Load(node);
- this->wallHeightP->Load(node);
- this->scaleP->Load(node);
- this->materialP->Load(node);
- this->granularityP->Load(node);
-
- // Make sure they are ok
- if (this->scaleP->GetValue() <= 0) this->scaleP->SetValue( 0.1 );
- if (this->thresholdP->GetValue() <=0) this->thresholdP->SetValue(200);
- if (this->wallHeightP->GetValue() <= 0) this->wallHeightP->SetValue( 1.0 );
-
- // Load the image
- this->mapImage = new Image();
-
- std::list<std::string>::iterator iter;
- GazeboConfig *gzcfg = Simulator::Instance()->GetGazeboConfig();
- std::string filename;
-
- for (iter = gzcfg->GetGazeboPaths().begin();
- iter != gzcfg->GetGazeboPaths().end(); iter++)
- {
- filename = (*iter) + "/Media/materials/textures/" + imageFilename;
- if (this->mapImage->Load(filename) >= 0)
- {
- break;
- }
- }
-
- if (!this->mapImage->Valid())
- gzthrow(std::string("Unable to open image file[") + imageFilename + "]" );
-
- this->root->x = 0;
- this->root->y = 0;
-
- this->root->width = this->mapImage->GetWidth();
- this->root->height = this->mapImage->GetHeight();
-
- this->BuildTree(this->root);
-
- this->merged = true;
- while (this->merged)
- {
- this->merged =false;
- this->ReduceTree(this->root);
- }
-
- this->CreateBoxes(this->root);
-
- if (this->visualNode)
- this->visualNode->MakeStatic();
-}
-
-
-//////////////////////////////////////////////////////////////////////////////
-// Create the boxes
-void MapGeom::CreateBoxes(QuadNode *node)
-{
- if (node->leaf)
- {
- if (!node->valid || !node->occupied)
- return;
-
- std::ostringstream stream;
-
- // Create the box geometry
- Geom *newBox = this->physicsEngine->CreateGeom( "box", this->body );
-
- if (!newBox)
- gzthrow("Unable to make an ode box geom");
-
- XMLConfig *boxConfig = new XMLConfig();
-
- stream << "<gazebo:world
xmlns:gazebo=\"http://playerstage.sourceforge.net/gazebo/xmlschema/#gz\";
xmlns:geom=\"http://playerstage.sourceforge.net/gazebo/xmlschema/#geom\";>";
-
- float x = (node->x + node->width / 2.0) * this->scaleP->GetValue();
- float y = (node->y + node->height / 2.0) * this->scaleP->GetValue();
- float z = this->wallHeightP->GetValue() / 2.0;
- float xSize = (node->width) * this->scaleP->GetValue();
- float ySize = (node->height) * this->scaleP->GetValue();
- float zSize = this->wallHeightP->GetValue();
-
- char geomName[256];
- sprintf(geomName,"map_geom_%d",geomCounter++);
-
- stream << "<geom:box name='" << geomName << "'>";
- stream << " <mass>0.0</mass>";
- stream << " <xyz>" << x << " " << y << " " << z << "</xyz>";
- stream << " <rpy>0 0 0</rpy>";
- stream << " <size>" << xSize << " " << ySize << " " << zSize << "</size>";
- stream << " <visual>";
- stream << " <mesh>unit_box</mesh>";
- stream << " <material>" << this->materialP->GetValue() << "</material>";
- stream << " <size>" << xSize << " " << ySize << " " << zSize << "</size>";
- stream << " </visual>";
- stream << "</geom:box>";
- stream << "</gazebo:world>";
-
- boxConfig->LoadString( stream.str() );
-
- newBox->Load( boxConfig->GetRootNode()->GetChild() );
- delete boxConfig;
- }
- else
- {
- std::deque<QuadNode*>::iterator iter;
- for (iter = node->children.begin(); iter != node->children.end(); iter++)
- {
- this->CreateBoxes(*iter);
- }
- }
-}
-
-//////////////////////////////////////////////////////////////////////////////
-// Reduce the size of the quad tree
-void MapGeom::ReduceTree(QuadNode *node)
-{
- std::deque<QuadNode*>::iterator iter;
-
- if (!node->valid)
- return;
-
- if (!node->leaf)
- {
- unsigned int count = 0;
- int size = node->children.size();
-
- for (int i = 0; i < size; i++)
- {
- if (node->children[i]->valid)
- {
- this->ReduceTree(node->children[i]);
- }
- if (node->children[i]->leaf)
- count++;
- }
-
- if (node->parent && count == node->children.size())
- {
- for (iter = node->children.begin(); iter != node->children.end(); iter++)
- {
- node->parent->children.push_back( *iter );
- (*iter)->parent = node->parent;
- }
- node->valid = false;
- }
- else
- {
- bool done = false;
- while (!done)
- {
- done = true;
- for (iter = node->children.begin();
- iter != node->children.end();iter++ )
- {
- if (!(*iter)->valid)
- {
- node->children.erase(iter, iter+1);
- done = false;
- break;
- }
- }
- }
- }
-
- }
- else
- {
- this->Merge(node, node->parent);
- }
-}
-
-//////////////////////////////////////////////////////////////////////////////
-// Merege quad tree cells
-void MapGeom::Merge(QuadNode *nodeA, QuadNode *nodeB)
-{
- std::deque<QuadNode*>::iterator iter;
-
- if (!nodeB)
- return;
-
- if (nodeB->leaf)
- {
- if (nodeB->occupied != nodeA->occupied)
- return;
-
- if ( nodeB->x == nodeA->x + nodeA->width &&
- nodeB->y == nodeA->y &&
- nodeB->height == nodeA->height )
- {
- nodeA->width += nodeB->width;
- nodeB->valid = false;
- nodeA->valid = true;
-
- this->merged = true;
- }
-
- if (nodeB->x == nodeA->x &&
- nodeB->width == nodeA->width &&
- nodeB->y == nodeA->y + nodeA->height )
- {
- nodeA->height += nodeB->height;
- nodeB->valid = false;
- nodeA->valid = true;
-
- this->merged = true;
- }
- }
- else
- {
-
- for (iter = nodeB->children.begin(); iter != nodeB->children.end(); iter++)
- {
- if ((*iter)->valid)
- {
- this->Merge(nodeA, (*iter));
- }
- }
- }
-}
-
-
-//////////////////////////////////////////////////////////////////////////////
-// Construct the quad tree
-void MapGeom::BuildTree(QuadNode *node)
-{
- QuadNode *newNode = NULL;
- unsigned int freePixels, occPixels;
-
- this->GetPixelCount(node->x, node->y, node->width, node->height,
- freePixels, occPixels);
-
- //int diff = labs(freePixels - occPixels);
-
- if ((int)(node->width*node->height) > (**this->granularityP))
- {
- float newX, newY;
- float newW, newH;
-
- newY = node->y;
- newW = node->width / 2.0;
- newH = node->height / 2.0;
-
- // Create the children for the node
- for (int i=0; i<2; i++)
- {
- newX = node->x;
-
- for (int j=0; j<2; j++)
- {
- newNode = new QuadNode(node);
- newNode->x = (unsigned int)newX;
- newNode->y = (unsigned int)newY;
-
- if (j == 0)
- newNode->width = (unsigned int)floor(newW);
- else
- newNode->width = (unsigned int)ceil(newW);
-
- if (i==0)
- newNode->height = (unsigned int)floor(newH);
- else
- newNode->height = (unsigned int)ceil(newH);
-
- node->children.push_back(newNode);
-
- this->BuildTree(newNode);
-
- newX += newNode->width;
-
- if (newNode->width == 0 || newNode->height ==0)
- newNode->valid = false;
- }
-
- if (i==0)
- newY += floor(newH);
- else
- newY += ceil(newH);
- }
-
- //node->occupied = true;
- node->occupied = false;
- node->leaf = false;
- }
- else if (occPixels == 0)
- {
- node->occupied = false;
- node->leaf = true;
- }
- else
- {
- node->occupied = true;
- node->leaf = true;
- }
-
-}
-
-//////////////////////////////////////////////////////////////////////////////
-// Get a count of pixels within a given area
-void MapGeom::GetPixelCount(unsigned int xStart, unsigned int yStart,
- unsigned int width, unsigned int height,
- unsigned int &freePixels,
- unsigned int &occPixels )
-{
- Color pixColor;
- unsigned char v;
- unsigned int x,y;
-
- freePixels = occPixels = 0;
-
- for (y = yStart; y < yStart + height; y++)
- {
- for (x = xStart; x < xStart + width; x++)
- {
- pixColor = this->mapImage->GetPixel(x, y);
-
- v = (unsigned char)(255 * ((pixColor.R() + pixColor.G() + pixColor.B())
/ 3.0));
- if (this->negativeP->GetValue())
- v = 255 - v;
-
- if (v > this->thresholdP->GetValue())
- freePixels++;
- else
- occPixels++;
- }
- }
-}
-
-//////////////////////////////////////////////////////////////////////////////
-/// Save child parameters
-void MapGeom::Save(std::string &prefix, std::ostream &stream)
-{
- Geom::Save(prefix, stream);
- stream << prefix << *(this->negativeP) << "\n";
- stream << prefix << *(this->thresholdP) << "\n";
- stream << prefix << *(this->wallHeightP) << "\n";
- stream << prefix << *(this->scaleP) << "\n";
- stream << prefix << *(this->materialP) << "\n";
- stream << prefix << *(this->granularityP) << "\n";
-}
Deleted: code/gazebo/branches/bullet/server/physics/BulletMapGeom.hh
===================================================================
--- code/gazebo/branches/bullet/server/physics/BulletMapGeom.hh 2009-05-28
23:16:05 UTC (rev 7733)
+++ code/gazebo/branches/bullet/server/physics/BulletMapGeom.hh 2009-05-28
23:17:04 UTC (rev 7734)
@@ -1,197 +0,0 @@
-/*
- * Gazebo - Outdoor Multi-Robot Simulator
- * Copyright (C) 2003
- * Nate Koenig & Andrew Howard
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- */
-/* Desc: Occupancy grid geom
- * Author: Nate Koenig
- * Date: 14 Jly 2008
- * CVS: $Id: BulletMapGeom.hh 7634 2009-05-11 01:43:31Z natepak $
- */
-
-#ifndef BulletMAPGEOM_HH
-#define BulletMAPGEOM_HH
-
-#include <deque>
-
-#include "Vector2.hh"
-#include "Param.hh"
-#include "BulletGeom.hh"
-
-namespace gazebo
-{
- class Image;
-
- /// \addtogroup gazebo_physics_geom
- /// \{
- /** \defgroup gazebo_map_geom Map geom
- \brief Map geom
-
- \par Attributes
- The following attributes are supported.
-
- - image (string)
- - Binary image that defines an occupancy grid
- - Default: (empty)
-
- - scale (float)
- - Scaling factor
- - Default: 1
-
- - granularity (int)
- - Degree of coarseness when determing if an image area is occupied.
Units are pixels
- - Default: 5
-
- - threshold (unsigned char)
- - Grayscale threshold. A pixel value greater than this amount is
considered free space
- - Default: 200
-
- - negative (bool)
- - True if the image pixel values should be inverted.
- - Default: false
-
- - material (string)
- - Material to apply to the map
- - Default: (empty)
-
-
-
- \par Example
- \verbatim
- <geom:map name="map_geom">
- <image>map.png</image>
- <scale>0.1</scale>
- </geom:map>
- \endverbatim
- */
- /// \}
- /// \addtogroup gazebo_map_geom
- /// \{
-
-
- class SpaceTree;
- class QuadNode;
-
- /// \brief Map geom
- class BulletMapGeom : public BulletGeom
- {
- /// \brief Constructor
- public: BulletMapGeom(Body *body);
-
- /// \brief Destructor
- public: virtual ~BulletMapGeom();
-
- /// \brief Update function
- public: void Update();
-
- /// \brief Load the heightmap
- public: virtual void Load(XMLConfigNode *node);
-
- /// \brief Save child parameters
- public: void Save(std::string &prefix, std::ostream &stream);
-
- /// \brief Build the quadtree
- private: void BuildTree(QuadNode *node);
-
- /// \brief Get the number of free and occupied pixels in a given area
- private: void GetPixelCount(unsigned int xStart, unsigned int yStart,
- unsigned int width, unsigned int height,
- unsigned int &freePixels,
- unsigned int &occPixels );
-
- /// \brief Reduce the number of nodes in the tree.
- private: void ReduceTree(QuadNode *node);
-
- /// \brief Try to merge to nodes
- private: void Merge(QuadNode *nodeA, QuadNode *nodeB);
-
- private: void CreateBox();
-
- /// \brief Create the boxes for the map
- private: void CreateBoxes(QuadNode *node);
-
- // The scale factor to apply to the geoms
- private: ParamT<double> *scaleP;
-
- // Negative image?
- private: ParamT<int> *negativeP;
-
- // Image color threshold used for extrusion
- private: ParamT<double> *thresholdP;
-
- // The color of the walls
- private: ParamT<std::string> *materialP;
-
- // The amount of acceptable error in the model
- private: ParamT<int> *granularityP;
-
- private: ParamT<double> *wallHeightP;
-
- private: Image *mapImage;
-
- private: QuadNode *root;
-
- private: bool merged;
- private: static unsigned int geomCounter;
- };
-
-
- class QuadNode
- {
- public: QuadNode( QuadNode *_parent )
- {
- parent = _parent;
- occupied = false;
- leaf = true;
- valid = true;
- }
-
- public: ~QuadNode()
- {
- std::deque<QuadNode*>::iterator iter;
- for (iter = children.begin(); iter != children.end(); iter++)
- delete (*iter);
- }
-
- public: void Print(std::string space)
- {
- std::deque<QuadNode*>::iterator iter;
-
- printf("%sXY[%d %d] WH[%d %d] O[%d] L[%d]
V[%d]\n",space.c_str(),x,y,width, height, occupied, leaf, valid);
- space += " ";
- for (iter = children.begin(); iter != children.end(); iter++)
- if ((*iter)->occupied)
- (*iter)->Print(space);
- }
-
- public: unsigned int x, y;
- public: unsigned int width, height;
-
- public: QuadNode *parent;
- public: std::deque<QuadNode*> children;
- public: bool occupied;
- public: bool leaf;
-
- public: bool valid;
-
- };
-
- /// \}
-}
-
-#endif
Copied: code/gazebo/branches/bullet/server/physics/MapGeom.cc (from rev 7733,
code/gazebo/branches/bullet/server/physics/BulletMapGeom.cc)
===================================================================
--- code/gazebo/branches/bullet/server/physics/MapGeom.cc
(rev 0)
+++ code/gazebo/branches/bullet/server/physics/MapGeom.cc 2009-05-28
23:17:04 UTC (rev 7734)
@@ -0,0 +1,437 @@
+/*
+ * Gazebo - Outdoor Multi-Robot Simulator
+ * Copyright (C) 2003
+ * Nate Koenig & Andrew Howard
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ */
+/* Desc: Map geometry
+ * Author: Nate Koenig
+ * Date: 14 July 2008
+ * CVS: $Id: MapGeom.cc 7661 2009-05-14 15:54:32Z natepak $
+ */
+
+
+#include <iostream>
+#include <string.h>
+#include <math.h>
+
+#include "GazeboConfig.hh"
+#include "OgreVisual.hh"
+#include "Image.hh"
+#include "GazeboError.hh"
+#include "Simulator.hh"
+#include "Global.hh"
+#include "Body.hh"
+#include "MapGeom.hh"
+
+using namespace gazebo;
+
+unsigned int MapGeom::geomCounter = 0;
+
+//////////////////////////////////////////////////////////////////////////////
+// Constructor
+MapGeom::MapGeom(Body *body)
+ : Geom(body)
+{
+ this->root = new QuadNode(NULL);
+
+ Param::Begin(&this->parameters);
+ this->negativeP = new ParamT<int>("negative", 0, 0);
+ this->thresholdP = new ParamT<double>( "threshold", 200.0, 0);
+ this->wallHeightP = new ParamT<double>( "height", 1.0, 0 );
+ this->scaleP = new ParamT<double>("scale",1.0,0);
+ this->materialP = new ParamT<std::string>("material", "", 0);
+ this->granularityP = new ParamT<int>("granularity", 5, 0);
+ Param::End();
+
+}
+
+
+//////////////////////////////////////////////////////////////////////////////
+// Destructor
+MapGeom::~MapGeom()
+{
+ if (this->root)
+ delete this->root;
+
+ if (this->mapImage)
+ delete this->mapImage;
+ this->mapImage = NULL;
+
+ delete this->negativeP;
+ delete this->thresholdP;
+ delete this->wallHeightP;
+ delete this->scaleP;
+ delete this->materialP;
+ delete this->granularityP;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+/// Update function.
+void MapGeom::Update()
+{
+ Geom::Update();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Load the heightmap
+void MapGeom::Load(XMLConfigNode *node)
+{
+ Geom::Load(node);
+
+ std::string imageFilename = node->GetString("image","",1);
+
+ this->negativeP->Load(node);
+ this->thresholdP->Load(node);
+ this->wallHeightP->Load(node);
+ this->scaleP->Load(node);
+ this->materialP->Load(node);
+ this->granularityP->Load(node);
+
+ // Make sure they are ok
+ if (this->scaleP->GetValue() <= 0) this->scaleP->SetValue( 0.1 );
+ if (this->thresholdP->GetValue() <=0) this->thresholdP->SetValue(200);
+ if (this->wallHeightP->GetValue() <= 0) this->wallHeightP->SetValue( 1.0 );
+
+ // Load the image
+ this->mapImage = new Image();
+
+ std::list<std::string>::iterator iter;
+ GazeboConfig *gzcfg = Simulator::Instance()->GetGazeboConfig();
+ std::string filename;
+
+ for (iter = gzcfg->GetGazeboPaths().begin();
+ iter != gzcfg->GetGazeboPaths().end(); iter++)
+ {
+ filename = (*iter) + "/Media/materials/textures/" + imageFilename;
+ if (this->mapImage->Load(filename) >= 0)
+ {
+ break;
+ }
+ }
+
+ if (!this->mapImage->Valid())
+ gzthrow(std::string("Unable to open image file[") + imageFilename + "]" );
+
+ this->root->x = 0;
+ this->root->y = 0;
+
+ this->root->width = this->mapImage->GetWidth();
+ this->root->height = this->mapImage->GetHeight();
+
+ this->BuildTree(this->root);
+
+ this->merged = true;
+ while (this->merged)
+ {
+ this->merged =false;
+ this->ReduceTree(this->root);
+ }
+
+ this->CreateBoxes(this->root);
+
+ if (this->visualNode)
+ this->visualNode->MakeStatic();
+}
+
+
+//////////////////////////////////////////////////////////////////////////////
+// Create the boxes
+void MapGeom::CreateBoxes(QuadNode *node)
+{
+ if (node->leaf)
+ {
+ if (!node->valid || !node->occupied)
+ return;
+
+ std::ostringstream stream;
+
+ // Create the box geometry
+ Geom *newBox = this->physicsEngine->CreateGeom( "box", this->body );
+
+ if (!newBox)
+ gzthrow("Unable to make an ode box geom");
+
+ XMLConfig *boxConfig = new XMLConfig();
+
+ stream << "<gazebo:world
xmlns:gazebo=\"http://playerstage.sourceforge.net/gazebo/xmlschema/#gz\";
xmlns:geom=\"http://playerstage.sourceforge.net/gazebo/xmlschema/#geom\";>";
+
+ float x = (node->x + node->width / 2.0) * this->scaleP->GetValue();
+ float y = (node->y + node->height / 2.0) * this->scaleP->GetValue();
+ float z = this->wallHeightP->GetValue() / 2.0;
+ float xSize = (node->width) * this->scaleP->GetValue();
+ float ySize = (node->height) * this->scaleP->GetValue();
+ float zSize = this->wallHeightP->GetValue();
+
+ char geomName[256];
+ sprintf(geomName,"map_geom_%d",geomCounter++);
+
+ stream << "<geom:box name='" << geomName << "'>";
+ stream << " <mass>0.0</mass>";
+ stream << " <xyz>" << x << " " << y << " " << z << "</xyz>";
+ stream << " <rpy>0 0 0</rpy>";
+ stream << " <size>" << xSize << " " << ySize << " " << zSize << "</size>";
+ stream << " <visual>";
+ stream << " <mesh>unit_box</mesh>";
+ stream << " <material>" << this->materialP->GetValue() << "</material>";
+ stream << " <size>" << xSize << " " << ySize << " " << zSize << "</size>";
+ stream << " </visual>";
+ stream << "</geom:box>";
+ stream << "</gazebo:world>";
+
+ boxConfig->LoadString( stream.str() );
+
+ newBox->Load( boxConfig->GetRootNode()->GetChild() );
+ delete boxConfig;
+ }
+ else
+ {
+ std::deque<QuadNode*>::iterator iter;
+ for (iter = node->children.begin(); iter != node->children.end(); iter++)
+ {
+ this->CreateBoxes(*iter);
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Reduce the size of the quad tree
+void MapGeom::ReduceTree(QuadNode *node)
+{
+ std::deque<QuadNode*>::iterator iter;
+
+ if (!node->valid)
+ return;
+
+ if (!node->leaf)
+ {
+ unsigned int count = 0;
+ int size = node->children.size();
+
+ for (int i = 0; i < size; i++)
+ {
+ if (node->children[i]->valid)
+ {
+ this->ReduceTree(node->children[i]);
+ }
+ if (node->children[i]->leaf)
+ count++;
+ }
+
+ if (node->parent && count == node->children.size())
+ {
+ for (iter = node->children.begin(); iter != node->children.end(); iter++)
+ {
+ node->parent->children.push_back( *iter );
+ (*iter)->parent = node->parent;
+ }
+ node->valid = false;
+ }
+ else
+ {
+ bool done = false;
+ while (!done)
+ {
+ done = true;
+ for (iter = node->children.begin();
+ iter != node->children.end();iter++ )
+ {
+ if (!(*iter)->valid)
+ {
+ node->children.erase(iter, iter+1);
+ done = false;
+ break;
+ }
+ }
+ }
+ }
+
+ }
+ else
+ {
+ this->Merge(node, node->parent);
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Merege quad tree cells
+void MapGeom::Merge(QuadNode *nodeA, QuadNode *nodeB)
+{
+ std::deque<QuadNode*>::iterator iter;
+
+ if (!nodeB)
+ return;
+
+ if (nodeB->leaf)
+ {
+ if (nodeB->occupied != nodeA->occupied)
+ return;
+
+ if ( nodeB->x == nodeA->x + nodeA->width &&
+ nodeB->y == nodeA->y &&
+ nodeB->height == nodeA->height )
+ {
+ nodeA->width += nodeB->width;
+ nodeB->valid = false;
+ nodeA->valid = true;
+
+ this->merged = true;
+ }
+
+ if (nodeB->x == nodeA->x &&
+ nodeB->width == nodeA->width &&
+ nodeB->y == nodeA->y + nodeA->height )
+ {
+ nodeA->height += nodeB->height;
+ nodeB->valid = false;
+ nodeA->valid = true;
+
+ this->merged = true;
+ }
+ }
+ else
+ {
+
+ for (iter = nodeB->children.begin(); iter != nodeB->children.end(); iter++)
+ {
+ if ((*iter)->valid)
+ {
+ this->Merge(nodeA, (*iter));
+ }
+ }
+ }
+}
+
+
+//////////////////////////////////////////////////////////////////////////////
+// Construct the quad tree
+void MapGeom::BuildTree(QuadNode *node)
+{
+ QuadNode *newNode = NULL;
+ unsigned int freePixels, occPixels;
+
+ this->GetPixelCount(node->x, node->y, node->width, node->height,
+ freePixels, occPixels);
+
+ //int diff = labs(freePixels - occPixels);
+
+ if ((int)(node->width*node->height) > (**this->granularityP))
+ {
+ float newX, newY;
+ float newW, newH;
+
+ newY = node->y;
+ newW = node->width / 2.0;
+ newH = node->height / 2.0;
+
+ // Create the children for the node
+ for (int i=0; i<2; i++)
+ {
+ newX = node->x;
+
+ for (int j=0; j<2; j++)
+ {
+ newNode = new QuadNode(node);
+ newNode->x = (unsigned int)newX;
+ newNode->y = (unsigned int)newY;
+
+ if (j == 0)
+ newNode->width = (unsigned int)floor(newW);
+ else
+ newNode->width = (unsigned int)ceil(newW);
+
+ if (i==0)
+ newNode->height = (unsigned int)floor(newH);
+ else
+ newNode->height = (unsigned int)ceil(newH);
+
+ node->children.push_back(newNode);
+
+ this->BuildTree(newNode);
+
+ newX += newNode->width;
+
+ if (newNode->width == 0 || newNode->height ==0)
+ newNode->valid = false;
+ }
+
+ if (i==0)
+ newY += floor(newH);
+ else
+ newY += ceil(newH);
+ }
+
+ //node->occupied = true;
+ node->occupied = false;
+ node->leaf = false;
+ }
+ else if (occPixels == 0)
+ {
+ node->occupied = false;
+ node->leaf = true;
+ }
+ else
+ {
+ node->occupied = true;
+ node->leaf = true;
+ }
+
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Get a count of pixels within a given area
+void MapGeom::GetPixelCount(unsigned int xStart, unsigned int yStart,
+ unsigned int width, unsigned int height,
+ unsigned int &freePixels,
+ unsigned int &occPixels )
+{
+ Color pixColor;
+ unsigned char v;
+ unsigned int x,y;
+
+ freePixels = occPixels = 0;
+
+ for (y = yStart; y < yStart + height; y++)
+ {
+ for (x = xStart; x < xStart + width; x++)
+ {
+ pixColor = this->mapImage->GetPixel(x, y);
+
+ v = (unsigned char)(255 * ((pixColor.R() + pixColor.G() + pixColor.B())
/ 3.0));
+ if (this->negativeP->GetValue())
+ v = 255 - v;
+
+ if (v > this->thresholdP->GetValue())
+ freePixels++;
+ else
+ occPixels++;
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+/// Save child parameters
+void MapGeom::Save(std::string &prefix, std::ostream &stream)
+{
+ Geom::Save(prefix, stream);
+ stream << prefix << *(this->negativeP) << "\n";
+ stream << prefix << *(this->thresholdP) << "\n";
+ stream << prefix << *(this->wallHeightP) << "\n";
+ stream << prefix << *(this->scaleP) << "\n";
+ stream << prefix << *(this->materialP) << "\n";
+ stream << prefix << *(this->granularityP) << "\n";
+}
Copied: code/gazebo/branches/bullet/server/physics/MapGeom.hh (from rev 7733,
code/gazebo/branches/bullet/server/physics/BulletMapGeom.hh)
===================================================================
--- code/gazebo/branches/bullet/server/physics/MapGeom.hh
(rev 0)
+++ code/gazebo/branches/bullet/server/physics/MapGeom.hh 2009-05-28
23:17:04 UTC (rev 7734)
@@ -0,0 +1,197 @@
+/*
+ * Gazebo - Outdoor Multi-Robot Simulator
+ * Copyright (C) 2003
+ * Nate Koenig & Andrew Howard
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ */
+/* Desc: Occupancy grid geom
+ * Author: Nate Koenig
+ * Date: 14 Jly 2008
+ * CVS: $Id: BulletMapGeom.hh 7634 2009-05-11 01:43:31Z natepak $
+ */
+
+#ifndef BulletMAPGEOM_HH
+#define BulletMAPGEOM_HH
+
+#include <deque>
+
+#include "Vector2.hh"
+#include "Param.hh"
+#include "BulletGeom.hh"
+
+namespace gazebo
+{
+ class Image;
+
+ /// \addtogroup gazebo_physics_geom
+ /// \{
+ /** \defgroup gazebo_map_geom Map geom
+ \brief Map geom
+
+ \par Attributes
+ The following attributes are supported.
+
+ - image (string)
+ - Binary image that defines an occupancy grid
+ - Default: (empty)
+
+ - scale (float)
+ - Scaling factor
+ - Default: 1
+
+ - granularity (int)
+ - Degree of coarseness when determing if an image area is occupied.
Units are pixels
+ - Default: 5
+
+ - threshold (unsigned char)
+ - Grayscale threshold. A pixel value greater than this amount is
considered free space
+ - Default: 200
+
+ - negative (bool)
+ - True if the image pixel values should be inverted.
+ - Default: false
+
+ - material (string)
+ - Material to apply to the map
+ - Default: (empty)
+
+
+
+ \par Example
+ \verbatim
+ <geom:map name="map_geom">
+ <image>map.png</image>
+ <scale>0.1</scale>
+ </geom:map>
+ \endverbatim
+ */
+ /// \}
+ /// \addtogroup gazebo_map_geom
+ /// \{
+
+
+ class SpaceTree;
+ class QuadNode;
+
+ /// \brief Map geom
+ class BulletMapGeom : public BulletGeom
+ {
+ /// \brief Constructor
+ public: BulletMapGeom(Body *body);
+
+ /// \brief Destructor
+ public: virtual ~BulletMapGeom();
+
+ /// \brief Update function
+ public: void Update();
+
+ /// \brief Load the heightmap
+ public: virtual void Load(XMLConfigNode *node);
+
+ /// \brief Save child parameters
+ public: void Save(std::string &prefix, std::ostream &stream);
+
+ /// \brief Build the quadtree
+ private: void BuildTree(QuadNode *node);
+
+ /// \brief Get the number of free and occupied pixels in a given area
+ private: void GetPixelCount(unsigned int xStart, unsigned int yStart,
+ unsigned int width, unsigned int height,
+ unsigned int &freePixels,
+ unsigned int &occPixels );
+
+ /// \brief Reduce the number of nodes in the tree.
+ private: void ReduceTree(QuadNode *node);
+
+ /// \brief Try to merge to nodes
+ private: void Merge(QuadNode *nodeA, QuadNode *nodeB);
+
+ private: void CreateBox();
+
+ /// \brief Create the boxes for the map
+ private: void CreateBoxes(QuadNode *node);
+
+ // The scale factor to apply to the geoms
+ private: ParamT<double> *scaleP;
+
+ // Negative image?
+ private: ParamT<int> *negativeP;
+
+ // Image color threshold used for extrusion
+ private: ParamT<double> *thresholdP;
+
+ // The color of the walls
+ private: ParamT<std::string> *materialP;
+
+ // The amount of acceptable error in the model
+ private: ParamT<int> *granularityP;
+
+ private: ParamT<double> *wallHeightP;
+
+ private: Image *mapImage;
+
+ private: QuadNode *root;
+
+ private: bool merged;
+ private: static unsigned int geomCounter;
+ };
+
+
+ class QuadNode
+ {
+ public: QuadNode( QuadNode *_parent )
+ {
+ parent = _parent;
+ occupied = false;
+ leaf = true;
+ valid = true;
+ }
+
+ public: ~QuadNode()
+ {
+ std::deque<QuadNode*>::iterator iter;
+ for (iter = children.begin(); iter != children.end(); iter++)
+ delete (*iter);
+ }
+
+ public: void Print(std::string space)
+ {
+ std::deque<QuadNode*>::iterator iter;
+
+ printf("%sXY[%d %d] WH[%d %d] O[%d] L[%d]
V[%d]\n",space.c_str(),x,y,width, height, occupied, leaf, valid);
+ space += " ";
+ for (iter = children.begin(); iter != children.end(); iter++)
+ if ((*iter)->occupied)
+ (*iter)->Print(space);
+ }
+
+ public: unsigned int x, y;
+ public: unsigned int width, height;
+
+ public: QuadNode *parent;
+ public: std::deque<QuadNode*> children;
+ public: bool occupied;
+ public: bool leaf;
+
+ public: bool valid;
+
+ };
+
+ /// \}
+}
+
+#endif
Deleted: code/gazebo/branches/bullet/server/physics/ode/ODEMapGeom.cc
===================================================================
--- code/gazebo/branches/bullet/server/physics/ode/ODEMapGeom.cc
2009-05-28 23:16:05 UTC (rev 7733)
+++ code/gazebo/branches/bullet/server/physics/ode/ODEMapGeom.cc
2009-05-28 23:17:04 UTC (rev 7734)
@@ -1,440 +0,0 @@
-/*
- * Gazebo - Outdoor Multi-Robot Simulator
- * Copyright (C) 2003
- * Nate Koenig & Andrew Howard
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- */
-/* Desc: Map geometry
- * Author: Nate Koenig
- * Date: 14 July 2008
- * CVS: $Id: ODEMapGeom.cc 7661 2009-05-14 15:54:32Z natepak $
- */
-
-
-#include <ode/ode.h>
-#include <iostream>
-#include <string.h>
-#include <math.h>
-
-#include "ODEPhysics.hh"
-#include "GazeboConfig.hh"
-#include "OgreVisual.hh"
-#include "Image.hh"
-#include "ODEBoxGeom.hh"
-#include "GazeboError.hh"
-#include "Simulator.hh"
-#include "Global.hh"
-#include "Body.hh"
-#include "ODEMapGeom.hh"
-
-using namespace gazebo;
-
-unsigned int ODEMapGeom::geomCounter = 0;
-
-//////////////////////////////////////////////////////////////////////////////
-// Constructor
-ODEMapGeom::ODEMapGeom(Body *body)
- : ODEGeom(body)
-{
- this->root = new QuadNode(NULL);
-
- Param::Begin(&this->parameters);
- this->negativeP = new ParamT<int>("negative", 0, 0);
- this->thresholdP = new ParamT<double>( "threshold", 200.0, 0);
- this->wallHeightP = new ParamT<double>( "height", 1.0, 0 );
- this->scaleP = new ParamT<double>("scale",1.0,0);
- this->materialP = new ParamT<std::string>("material", "", 0);
- this->granularityP = new ParamT<int>("granularity", 5, 0);
- Param::End();
-
-}
-
-
-//////////////////////////////////////////////////////////////////////////////
-// Destructor
-ODEMapGeom::~ODEMapGeom()
-{
- if (this->root)
- delete this->root;
-
- if (this->mapImage)
- delete this->mapImage;
- this->mapImage = NULL;
-
- delete this->negativeP;
- delete this->thresholdP;
- delete this->wallHeightP;
- delete this->scaleP;
- delete this->materialP;
- delete this->granularityP;
-}
-
-//////////////////////////////////////////////////////////////////////////////
-/// Update function.
-void ODEMapGeom::Update()
-{
- ODEGeom::Update();
-}
-
-////////////////////////////////////////////////////////////////////////////////
-/// Load the heightmap
-void ODEMapGeom::Load(XMLConfigNode *node)
-{
- ODEGeom::Load(node);
-
- std::string imageFilename = node->GetString("image","",1);
-
- this->negativeP->Load(node);
- this->thresholdP->Load(node);
- this->wallHeightP->Load(node);
- this->scaleP->Load(node);
- this->materialP->Load(node);
- this->granularityP->Load(node);
-
- // Make sure they are ok
- if (this->scaleP->GetValue() <= 0) this->scaleP->SetValue( 0.1 );
- if (this->thresholdP->GetValue() <=0) this->thresholdP->SetValue(200);
- if (this->wallHeightP->GetValue() <= 0) this->wallHeightP->SetValue( 1.0 );
-
- // Load the image
- this->mapImage = new Image();
-
- std::list<std::string>::iterator iter;
- GazeboConfig *gzcfg = Simulator::Instance()->GetGazeboConfig();
- std::string filename;
-
- for (iter = gzcfg->GetGazeboPaths().begin();
- iter != gzcfg->GetGazeboPaths().end(); iter++)
- {
- filename = (*iter) + "/Media/materials/textures/" + imageFilename;
- if (this->mapImage->Load(filename) >= 0)
- {
- break;
- }
- }
-
- if (!this->mapImage->Valid())
- gzthrow(std::string("Unable to open image file[") + imageFilename + "]" );
-
- this->root->x = 0;
- this->root->y = 0;
-
- this->root->width = this->mapImage->GetWidth();
- this->root->height = this->mapImage->GetHeight();
-
- this->BuildTree(this->root);
-
- this->merged = true;
- while (this->merged)
- {
- this->merged =false;
- this->ReduceTree(this->root);
- }
-
- this->CreateBoxes(this->root);
-
- if (this->visualNode)
- this->visualNode->MakeStatic();
-}
-
-
-//////////////////////////////////////////////////////////////////////////////
-// Create the ODE boxes
-void ODEMapGeom::CreateBoxes(QuadNode *node)
-{
- if (node->leaf)
- {
- if (!node->valid || !node->occupied)
- return;
-
- std::ostringstream stream;
-
- // Create the box geometry
- ODEBoxGeom *newBox =
dynamic_cast<ODEBoxGeom*>(this->odePhysics->CreateGeom( "box", this->body ));
-
- if (!newBox)
- gzthrow("Unable to make an ode box geom");
-
- XMLConfig *boxConfig = new XMLConfig();
-
- stream << "<gazebo:world
xmlns:gazebo=\"http://playerstage.sourceforge.net/gazebo/xmlschema/#gz\";
xmlns:geom=\"http://playerstage.sourceforge.net/gazebo/xmlschema/#geom\";>";
-
- float x = (node->x + node->width / 2.0) * this->scaleP->GetValue();
- float y = (node->y + node->height / 2.0) * this->scaleP->GetValue();
- float z = this->wallHeightP->GetValue() / 2.0;
- float xSize = (node->width) * this->scaleP->GetValue();
- float ySize = (node->height) * this->scaleP->GetValue();
- float zSize = this->wallHeightP->GetValue();
-
- char geomName[256];
- sprintf(geomName,"map_geom_%d",geomCounter++);
-
- stream << "<geom:box name='" << geomName << "'>";
- stream << " <mass>0.0</mass>";
- stream << " <xyz>" << x << " " << y << " " << z << "</xyz>";
- stream << " <rpy>0 0 0</rpy>";
- stream << " <size>" << xSize << " " << ySize << " " << zSize << "</size>";
- stream << " <visual>";
- stream << " <mesh>unit_box</mesh>";
- stream << " <material>" << this->materialP->GetValue() << "</material>";
- stream << " <size>" << xSize << " " << ySize << " " << zSize << "</size>";
- stream << " </visual>";
- stream << "</geom:box>";
- stream << "</gazebo:world>";
-
- boxConfig->LoadString( stream.str() );
-
- newBox->Load( boxConfig->GetRootNode()->GetChild() );
- delete boxConfig;
- }
- else
- {
- std::deque<QuadNode*>::iterator iter;
- for (iter = node->children.begin(); iter != node->children.end(); iter++)
- {
- this->CreateBoxes(*iter);
- }
- }
-}
-
-//////////////////////////////////////////////////////////////////////////////
-// Reduce the size of the quad tree
-void ODEMapGeom::ReduceTree(QuadNode *node)
-{
- std::deque<QuadNode*>::iterator iter;
-
- if (!node->valid)
- return;
-
- if (!node->leaf)
- {
- unsigned int count = 0;
- int size = node->children.size();
-
- for (int i = 0; i < size; i++)
- {
- if (node->children[i]->valid)
- {
- this->ReduceTree(node->children[i]);
- }
- if (node->children[i]->leaf)
- count++;
- }
-
- if (node->parent && count == node->children.size())
- {
- for (iter = node->children.begin(); iter != node->children.end(); iter++)
- {
- node->parent->children.push_back( *iter );
- (*iter)->parent = node->parent;
- }
- node->valid = false;
- }
- else
- {
- bool done = false;
- while (!done)
- {
- done = true;
- for (iter = node->children.begin();
- iter != node->children.end();iter++ )
- {
- if (!(*iter)->valid)
- {
- node->children.erase(iter, iter+1);
- done = false;
- break;
- }
- }
- }
- }
-
- }
- else
- {
- this->Merge(node, node->parent);
- }
-}
-
-//////////////////////////////////////////////////////////////////////////////
-// Merege quad tree cells
-void ODEMapGeom::Merge(QuadNode *nodeA, QuadNode *nodeB)
-{
- std::deque<QuadNode*>::iterator iter;
-
- if (!nodeB)
- return;
-
- if (nodeB->leaf)
- {
- if (nodeB->occupied != nodeA->occupied)
- return;
-
- if ( nodeB->x == nodeA->x + nodeA->width &&
- nodeB->y == nodeA->y &&
- nodeB->height == nodeA->height )
- {
- nodeA->width += nodeB->width;
- nodeB->valid = false;
- nodeA->valid = true;
-
- this->merged = true;
- }
-
- if (nodeB->x == nodeA->x &&
- nodeB->width == nodeA->width &&
- nodeB->y == nodeA->y + nodeA->height )
- {
- nodeA->height += nodeB->height;
- nodeB->valid = false;
- nodeA->valid = true;
-
- this->merged = true;
- }
- }
- else
- {
-
- for (iter = nodeB->children.begin(); iter != nodeB->children.end(); iter++)
- {
- if ((*iter)->valid)
- {
- this->Merge(nodeA, (*iter));
- }
- }
- }
-}
-
-
-//////////////////////////////////////////////////////////////////////////////
-// Construct the quad tree
-void ODEMapGeom::BuildTree(QuadNode *node)
-{
- QuadNode *newNode = NULL;
- unsigned int freePixels, occPixels;
-
- this->GetPixelCount(node->x, node->y, node->width, node->height,
- freePixels, occPixels);
-
- //int diff = labs(freePixels - occPixels);
-
- if ((int)(node->width*node->height) > (**this->granularityP))
- {
- float newX, newY;
- float newW, newH;
-
- newY = node->y;
- newW = node->width / 2.0;
- newH = node->height / 2.0;
-
- // Create the children for the node
- for (int i=0; i<2; i++)
- {
- newX = node->x;
-
- for (int j=0; j<2; j++)
- {
- newNode = new QuadNode(node);
- newNode->x = (unsigned int)newX;
- newNode->y = (unsigned int)newY;
-
- if (j == 0)
- newNode->width = (unsigned int)floor(newW);
- else
- newNode->width = (unsigned int)ceil(newW);
-
- if (i==0)
- newNode->height = (unsigned int)floor(newH);
- else
- newNode->height = (unsigned int)ceil(newH);
-
- node->children.push_back(newNode);
-
- this->BuildTree(newNode);
-
- newX += newNode->width;
-
- if (newNode->width == 0 || newNode->height ==0)
- newNode->valid = false;
- }
-
- if (i==0)
- newY += floor(newH);
- else
- newY += ceil(newH);
- }
-
- //node->occupied = true;
- node->occupied = false;
- node->leaf = false;
- }
- else if (occPixels == 0)
- {
- node->occupied = false;
- node->leaf = true;
- }
- else
- {
- node->occupied = true;
- node->leaf = true;
- }
-
-}
-
-//////////////////////////////////////////////////////////////////////////////
-// Get a count of pixels within a given area
-void ODEMapGeom::GetPixelCount(unsigned int xStart, unsigned int yStart,
- unsigned int width, unsigned int height,
- unsigned int &freePixels,
- unsigned int &occPixels )
-{
- Color pixColor;
- unsigned char v;
- unsigned int x,y;
-
- freePixels = occPixels = 0;
-
- for (y = yStart; y < yStart + height; y++)
- {
- for (x = xStart; x < xStart + width; x++)
- {
- pixColor = this->mapImage->GetPixel(x, y);
-
- v = (unsigned char)(255 * ((pixColor.R() + pixColor.G() + pixColor.B())
/ 3.0));
- if (this->negativeP->GetValue())
- v = 255 - v;
-
- if (v > this->thresholdP->GetValue())
- freePixels++;
- else
- occPixels++;
- }
- }
-}
-
-//////////////////////////////////////////////////////////////////////////////
-/// Save child parameters
-void ODEMapGeom::Save(std::string &prefix, std::ostream &stream)
-{
- ODEGeom::Save(prefix, stream);
- stream << prefix << *(this->negativeP) << "\n";
- stream << prefix << *(this->thresholdP) << "\n";
- stream << prefix << *(this->wallHeightP) << "\n";
- stream << prefix << *(this->scaleP) << "\n";
- stream << prefix << *(this->materialP) << "\n";
- stream << prefix << *(this->granularityP) << "\n";
-}
Deleted: code/gazebo/branches/bullet/server/physics/ode/ODEMapGeom.hh
===================================================================
--- code/gazebo/branches/bullet/server/physics/ode/ODEMapGeom.hh
2009-05-28 23:16:05 UTC (rev 7733)
+++ code/gazebo/branches/bullet/server/physics/ode/ODEMapGeom.hh
2009-05-28 23:17:04 UTC (rev 7734)
@@ -1,197 +0,0 @@
-/*
- * Gazebo - Outdoor Multi-Robot Simulator
- * Copyright (C) 2003
- * Nate Koenig & Andrew Howard
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- */
-/* Desc: Occupancy grid geom
- * Author: Nate Koenig
- * Date: 14 Jly 2008
- * CVS: $Id: ODEMapGeom.hh 7634 2009-05-11 01:43:31Z natepak $
- */
-
-#ifndef ODEMAPGEOM_HH
-#define ODEMAPGEOM_HH
-
-#include <deque>
-
-#include "Vector2.hh"
-#include "Param.hh"
-#include "ODEGeom.hh"
-
-namespace gazebo
-{
- class Image;
-
- /// \addtogroup gazebo_physics_geom
- /// \{
- /** \defgroup gazebo_map_geom Map geom
- \brief Map geom
-
- \par Attributes
- The following attributes are supported.
-
- - image (string)
- - Binary image that defines an occupancy grid
- - Default: (empty)
-
- - scale (float)
- - Scaling factor
- - Default: 1
-
- - granularity (int)
- - Degree of coarseness when determing if an image area is occupied.
Units are pixels
- - Default: 5
-
- - threshold (unsigned char)
- - Grayscale threshold. A pixel value greater than this amount is
considered free space
- - Default: 200
-
- - negative (bool)
- - True if the image pixel values should be inverted.
- - Default: false
-
- - material (string)
- - Material to apply to the map
- - Default: (empty)
-
-
-
- \par Example
- \verbatim
- <geom:map name="map_geom">
- <image>map.png</image>
- <scale>0.1</scale>
- </geom:map>
- \endverbatim
- */
- /// \}
- /// \addtogroup gazebo_map_geom
- /// \{
-
-
- class SpaceTree;
- class QuadNode;
-
- /// \brief Map geom
- class ODEMapGeom : public ODEGeom
- {
- /// \brief Constructor
- public: ODEMapGeom(Body *body);
-
- /// \brief Destructor
- public: virtual ~ODEMapGeom();
-
- /// \brief Update function
- public: void Update();
-
- /// \brief Load the heightmap
- public: virtual void Load(XMLConfigNode *node);
-
- /// \brief Save child parameters
- public: void Save(std::string &prefix, std::ostream &stream);
-
- /// \brief Build the quadtree
- private: void BuildTree(QuadNode *node);
-
- /// \brief Get the number of free and occupied pixels in a given area
- private: void GetPixelCount(unsigned int xStart, unsigned int yStart,
- unsigned int width, unsigned int height,
- unsigned int &freePixels,
- unsigned int &occPixels );
-
- /// \brief Reduce the number of nodes in the tree.
- private: void ReduceTree(QuadNode *node);
-
- /// \brief Try to merge to nodes
- private: void Merge(QuadNode *nodeA, QuadNode *nodeB);
-
- private: void CreateBox();
-
- /// \brief Create the boxes for the map
- private: void CreateBoxes(QuadNode *node);
-
- // The scale factor to apply to the geoms
- private: ParamT<double> *scaleP;
-
- // Negative image?
- private: ParamT<int> *negativeP;
-
- // Image color threshold used for extrusion
- private: ParamT<double> *thresholdP;
-
- // The color of the walls
- private: ParamT<std::string> *materialP;
-
- // The amount of acceptable error in the model
- private: ParamT<int> *granularityP;
-
- private: ParamT<double> *wallHeightP;
-
- private: Image *mapImage;
-
- private: QuadNode *root;
-
- private: bool merged;
- private: static unsigned int geomCounter;
- };
-
-
- class QuadNode
- {
- public: QuadNode( QuadNode *_parent )
- {
- parent = _parent;
- occupied = false;
- leaf = true;
- valid = true;
- }
-
- public: ~QuadNode()
- {
- std::deque<QuadNode*>::iterator iter;
- for (iter = children.begin(); iter != children.end(); iter++)
- delete (*iter);
- }
-
- public: void Print(std::string space)
- {
- std::deque<QuadNode*>::iterator iter;
-
- printf("%sXY[%d %d] WH[%d %d] O[%d] L[%d]
V[%d]\n",space.c_str(),x,y,width, height, occupied, leaf, valid);
- space += " ";
- for (iter = children.begin(); iter != children.end(); iter++)
- if ((*iter)->occupied)
- (*iter)->Print(space);
- }
-
- public: unsigned int x, y;
- public: unsigned int width, height;
-
- public: QuadNode *parent;
- public: std::deque<QuadNode*> children;
- public: bool occupied;
- public: bool leaf;
-
- public: bool valid;
-
- };
-
- /// \}
-}
-
-#endif
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Register Now for Creativity and Technology (CaT), June 3rd, NYC. CaT
is a gathering of tech-side developers & brand creativity professionals. Meet
the minds behind Google Creative Lab, Visual Complexity, Processing, &
iPhoneDevCamp as they present alongside digital heavyweights like Barbarian
Group, R/GA, & Big Spaceship. http://p.sf.net/sfu/creativitycat-com
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
