Revision: 6873
http://playerstage.svn.sourceforge.net/playerstage/?rev=6873&view=rev
Author: natepak
Date: 2008-07-15 19:48:14 -0700 (Tue, 15 Jul 2008)
Log Message:
-----------
Updates to the 2d map loader
Modified Paths:
--------------
code/gazebo/trunk/server/physics/Body.cc
code/gazebo/trunk/server/physics/BoxGeom.cc
code/gazebo/trunk/server/physics/BoxGeom.hh
code/gazebo/trunk/server/physics/Geom.cc
code/gazebo/trunk/server/physics/OccupancyGridGeom.cc
code/gazebo/trunk/server/physics/OccupancyGridGeom.hh
code/gazebo/trunk/server/physics/SConscript
Added Paths:
-----------
code/gazebo/trunk/server/physics/QuadTreeGeom.cc
code/gazebo/trunk/server/physics/QuadTreeGeom.hh
code/gazebo/trunk/server/physics/SpaceTree.cc
code/gazebo/trunk/server/physics/SpaceTree.hh
Modified: code/gazebo/trunk/server/physics/Body.cc
===================================================================
--- code/gazebo/trunk/server/physics/Body.cc 2008-07-16 01:26:03 UTC (rev
6872)
+++ code/gazebo/trunk/server/physics/Body.cc 2008-07-16 02:48:14 UTC (rev
6873)
@@ -29,6 +29,7 @@
#include "GazeboMessage.hh"
#include "HeightmapGeom.hh"
#include "OccupancyGridGeom.hh"
+#include "QuadTreeGeom.hh"
#include "OgreVisual.hh"
#include "Global.hh"
#include "Vector2.hh"
@@ -417,6 +418,10 @@
{
geom = new OccupancyGridGeom(this);
}
+ else if (node->GetName() == "quadtree")
+ {
+ geom = new QuadTreeGeom(this);
+ }
else
{
gzthrow("Unknown Geometry
Type["+node->GetString("name",std::string(),0)+"]");
Modified: code/gazebo/trunk/server/physics/BoxGeom.cc
===================================================================
--- code/gazebo/trunk/server/physics/BoxGeom.cc 2008-07-16 01:26:03 UTC (rev
6872)
+++ code/gazebo/trunk/server/physics/BoxGeom.cc 2008-07-16 02:48:14 UTC (rev
6873)
@@ -64,3 +64,14 @@
this->visualNode->SetMaterial("Gazebo/GreenEmissive");
*/
}
+
+//////////////////////////////////////////////////////////////////////////////
+// Set the size of the box
+void BoxGeom::SetSize( Vector3 size )
+{
+ // Initialize box mass matrix
+ dMassSetBoxTotal(&this->mass, this->dblMass, size.x, size.y, size.z);
+
+ // Create a box geometry with box mass matrix
+ this->SetGeom(dCreateBox( 0, size.x, size.y, size.z), true );
+}
Modified: code/gazebo/trunk/server/physics/BoxGeom.hh
===================================================================
--- code/gazebo/trunk/server/physics/BoxGeom.hh 2008-07-16 01:26:03 UTC (rev
6872)
+++ code/gazebo/trunk/server/physics/BoxGeom.hh 2008-07-16 02:48:14 UTC (rev
6873)
@@ -79,6 +79,10 @@
/// \brief Load the box
protected: void LoadChild(XMLConfigNode *node);
+
+ /// \brief Set the size of the box
+ public: void SetSize( Vector3 size );
+
};
/// \}
Modified: code/gazebo/trunk/server/physics/Geom.cc
===================================================================
--- code/gazebo/trunk/server/physics/Geom.cc 2008-07-16 01:26:03 UTC (rev
6872)
+++ code/gazebo/trunk/server/physics/Geom.cc 2008-07-16 02:48:14 UTC (rev
6873)
@@ -130,7 +130,7 @@
// Create the bounding box
- if (dGeomGetClass(this->geomId) != dPlaneClass)
+ if (this->geomId && dGeomGetClass(this->geomId) != dPlaneClass)
{
dReal aabb[6];
dGeomGetAABB(this->geomId, aabb);
@@ -142,7 +142,8 @@
this->bbVisual->AttachBoundingBox(min,max);
}
- if (dGeomGetClass(this->geomId) != dPlaneClass &&
dGeomGetClass(this->geomId) != dHeightfieldClass)
+ if (this->geomId && dGeomGetClass(this->geomId) != dPlaneClass &&
+ dGeomGetClass(this->geomId) != dHeightfieldClass)
{
World::Instance()->RegisterGeom(this);
this->ShowPhysics(false);
Modified: code/gazebo/trunk/server/physics/OccupancyGridGeom.cc
===================================================================
--- code/gazebo/trunk/server/physics/OccupancyGridGeom.cc 2008-07-16
01:26:03 UTC (rev 6872)
+++ code/gazebo/trunk/server/physics/OccupancyGridGeom.cc 2008-07-16
02:48:14 UTC (rev 6873)
@@ -36,6 +36,7 @@
#include "OgreAdaptor.hh"
#include "Global.hh"
#include "Body.hh"
+#include "SpaceTree.hh"
#include "OccupancyGridGeom.hh"
using namespace gazebo;
@@ -45,6 +46,7 @@
OccupancyGridGeom::OccupancyGridGeom(Body *body)
: Geom(body)
{
+ this->tree = NULL;
}
@@ -52,6 +54,8 @@
// Destructor
OccupancyGridGeom::~OccupancyGridGeom()
{
+ if (this->tree)
+ delete this->tree;
}
//////////////////////////////////////////////////////////////////////////////
@@ -100,16 +104,16 @@
// Create the 2d lines of the map
this->GenerateLines();
- std::cout << "OccupancyGrid: found [%d] walls" << this->wallCount;
-
+ printf("OccupancyGrid: found [%d] walls\n",this->wallCount);
+
+
// Create the quad tree
- /*this->tree = new SpaceTree();
+ this->tree = new SpaceTree();
tree->BuildTree( this->walls, this->wallCount,
this->mapWidth, this->mapHeight );
// Create the extruded geometry
this->GenerateGeometry();
- */
}
//////////////////////////////////////////////////////////////////////////////
@@ -120,7 +124,6 @@
unsigned char v;
MapPoint **map;
MapPoint *pt;
- const unsigned char *imageData;
Ogre::ColourValue pixColor;
this->mapWidth = this->mapImage.getWidth();
@@ -148,54 +151,53 @@
if (this->negative)
v = 255 - v;
- std::cout << "XY[" << x << " " << y << "] Color[" << pixColor[0] << " "
<< pixColor[1] << " " << pixColor[2] << "]";
- printf("V[%d]\n",v);
+ // std::cout << "XY[" << x << " " << y << "] Color[" << pixColor[0] << "
" << pixColor[1] << " " << pixColor[2] << "]"; printf("V[%d]\n",v);
// If the image data is beyond the threshold, then create a new map
// point
- /* if (v >= this->threshold)
+ if (v >= this->threshold)
{
// Create the new point
- map[r*this->mapWidth+c] = new MapPoint( c, r );
+ map[y*this->mapWidth+x] = new MapPoint( x, y );
// Point to the North
- if (r>0 && map[(r-1)*this->mapWidth+c] != NULL)
+ if (y>0 && map[(y-1)*this->mapWidth+x] != NULL)
{
- map[r*this->mapWidth+c]->arcs[0] = map[(r-1)*this->mapWidth+c];
- map[r*this->mapWidth+c]->arcCount++;
+ map[y*this->mapWidth+x]->arcs[0] = map[(y-1)*this->mapWidth+x];
+ map[y*this->mapWidth+x]->arcCount++;
- map[(r-1)*this->mapWidth+c]->arcs[4] = map[r*this->mapWidth+c];
- map[(r-1)*this->mapWidth+c]->arcCount++;
+ map[(y-1)*this->mapWidth+x]->arcs[4] = map[y*this->mapWidth+x];
+ map[(y-1)*this->mapWidth+x]->arcCount++;
}
// Point the NorthWest
- if (c>0 && r>0 && map[(r-1)*this->mapWidth+c-1] != NULL)
+ if (x>0 && y>0 && map[(y-1)*this->mapWidth+x-1] != NULL)
{
- map[r*this->mapWidth+c]->arcs[7] = map[(r-1)*this->mapWidth+c-1];
- map[r*this->mapWidth+c]->arcCount++;
+ map[y*this->mapWidth+x]->arcs[7] = map[(y-1)*this->mapWidth+x-1];
+ map[y*this->mapWidth+x]->arcCount++;
- map[(r-1)*this->mapWidth+c-1]->arcs[3] = map[r*this->mapWidth+c];
- map[(r-1)*this->mapWidth+c-1]->arcCount++;
+ map[(y-1)*this->mapWidth+x-1]->arcs[3] = map[y*this->mapWidth+x];
+ map[(y-1)*this->mapWidth+x-1]->arcCount++;
}
// Point to the West
- if (c>0 && map[r*this->mapWidth+c-1] != NULL)
+ if (x>0 && map[y*this->mapWidth+x-1] != NULL)
{
- map[r*this->mapWidth+c]->arcs[6] = map[r*this->mapWidth+c-1];
- map[r*this->mapWidth+c]->arcCount++;
+ map[y*this->mapWidth+x]->arcs[6] = map[y*this->mapWidth+x-1];
+ map[y*this->mapWidth+x]->arcCount++;
- map[r*this->mapWidth+c-1]->arcs[2] = map[r*this->mapWidth+c];
- map[r*this->mapWidth+c-1]->arcCount++;
+ map[y*this->mapWidth+x-1]->arcs[2] = map[y*this->mapWidth+x];
+ map[y*this->mapWidth+x-1]->arcCount++;
}
// Point to the NorthEast
- if (c+1<this->mapWidth && r>0 && map[(r-1)*this->mapWidth+c+1] != NULL)
+ if (x+1<this->mapWidth && y>0 && map[(y-1)*this->mapWidth+x+1] != NULL)
{
- map[r*this->mapWidth+c]->arcs[1] = map[(r-1)*this->mapWidth+c+1];
- map[r*this->mapWidth+c]->arcCount++;
+ map[y*this->mapWidth+x]->arcs[1] = map[(y-1)*this->mapWidth+x+1];
+ map[y*this->mapWidth+x]->arcCount++;
- map[(r-1)*this->mapWidth+c+1]->arcs[5] = map[r*this->mapWidth+c];
- map[(r-1)*this->mapWidth+c+1]->arcCount++;
+ map[(y-1)*this->mapWidth+x+1]->arcs[5] = map[y*this->mapWidth+x];
+ map[(y-1)*this->mapWidth+x+1]->arcCount++;
}
// Point to the East
@@ -211,20 +213,16 @@
}
else
{
- map[r*this->mapWidth+c] = NULL;
+ map[y*this->mapWidth+x] = NULL;
}
-
- */
}
}
- /*
- int i=0;
- for (r=0; r<this->mapHeight; r++)
+ for (y=0; y<this->mapHeight; y++)
{
- for (c=0; c<this->mapWidth; c++)
+ for (x=0; x<this->mapWidth; x++)
{
- pt = map[r*this->mapWidth+c];
+ pt = map[y*this->mapWidth+x];
// If the map point is valid
if (pt != NULL && pt->arcCount < 8)
@@ -244,19 +242,15 @@
// Free the map data
delete [] map;
- // Free the image
- delete dataSet;
-
// If the user has specified an error bound, then reduce the graph.
// This will only reduce curved surefaces
- if (this->errBound > 0)
+ /*if (this->errBound > 0)
{
ReduceLines();
- }
- */
+ }*/
}
-/*
+
//////////////////////////////////////////////////////////////////////////////
// Create a single line starting from point pt in direction dir
void OccupancyGridGeom::GenerateLine( MapPoint* pt, int dir )
@@ -348,22 +342,23 @@
}
+
//////////////////////////////////////////////////////////////////////////////
// Create the geometry from a list of 2d lines
void OccupancyGridGeom::GenerateGeometry()
{
// We are a space of fixed objects, and shouldnt collide with ourself
- dGeomSetCategoryBits( (dGeomID) this->modelSpaceId, GZ_FIXED_COLLIDE );
- dGeomSetCollideBits( (dGeomID) this->modelSpaceId, ~GZ_FIXED_COLLIDE );
+ //dGeomSetCategoryBits( (dGeomID) this->modelSpaceId, GZ_FIXED_COLLIDE );
+ //dGeomSetCollideBits( (dGeomID) this->modelSpaceId, ~GZ_FIXED_COLLIDE );
- this->tree->GetRoot()->GenerateGeoms( this->modelSpaceId, this->body,
+ this->tree->GetRoot()->GenerateGeoms( this->spaceId, this->body,
this->wallWidth, this->wallHeight, this->pos,
- this->scale, this->color, this->shadeModel, this->polygonMode);
+ this->scale, this->color);
- this->AddSpaceGeoms(this->tree->GetRoot());
+ //this->AddSpaceGeoms(this->tree->GetRoot());
}
-void OccupancyGridGeom::AddSpaceGeoms( SpaceNode *node)
+/*void OccupancyGridGeom::AddSpaceGeoms( SpaceNode *node)
{
//int i = 0;
if (node==NULL)
@@ -379,7 +374,7 @@
//{
// this->AddSpaceGeoms(node->children[i]);
//}
-}
+}*/
//////////////////////////////////////////////////////////////////////////////
@@ -515,5 +510,3 @@
this->mid.y = this->start.y - fabs( this->start.y - this->end.y ) / 2.0;
}
}
-
-*/
Modified: code/gazebo/trunk/server/physics/OccupancyGridGeom.hh
===================================================================
--- code/gazebo/trunk/server/physics/OccupancyGridGeom.hh 2008-07-16
01:26:03 UTC (rev 6872)
+++ code/gazebo/trunk/server/physics/OccupancyGridGeom.hh 2008-07-16
02:48:14 UTC (rev 6873)
@@ -63,6 +63,7 @@
/// \{
+ class SpaceTree;
class Line;
class MapPoint;
@@ -84,9 +85,27 @@
/// \brief Get the 2d lines from an image file
private: void GenerateLines();
+ /// \brief Create a single line starting from point pt in direction dir
+ private: void GenerateLine( MapPoint* pt, int dir );
+
+ /// \brief Reduce the number of lines in the map
+ private: void ReduceLines();
+
+ /// \brief Calculates the shortest distance from a point to a line
+ private: float PointLineDist( const MapPoint &p1, const MapPoint &p2,
+ const MapPoint &p );
+ /// \brief Adds a wall
+ private: void AddWall( Line *line );
+
+ /// \brief Deletes a wall segment
+ private: void EraseWall( int index );
+
+ /// \brief Create the geometry from a list of 2d lines
+ private: void GenerateGeometry();
+
private: Vector3 mapSize;
- //private: Line **walls;
+ private: Line **walls;
private: int wallCount;
private: int wallMaxCount;
@@ -112,7 +131,7 @@
private: float wallHeight;
// The color of the walls
- //private: Vector3 color;
+ private: Vector3 color;
// The amount of acceptable error in the model
private: float errBound;
@@ -122,8 +141,10 @@
private: unsigned int mapHeight;
private: Ogre::Image mapImage;
+
+ private: SpaceTree *tree;
};
-/*
+
class MapPoint
{
public: MapPoint() : x(0), y(0), arcCount(0)
@@ -174,7 +195,6 @@
private: float length;
private: float angle;
};
- */
/// \}
Added: code/gazebo/trunk/server/physics/QuadTreeGeom.cc
===================================================================
--- code/gazebo/trunk/server/physics/QuadTreeGeom.cc
(rev 0)
+++ code/gazebo/trunk/server/physics/QuadTreeGeom.cc 2008-07-16 02:48:14 UTC
(rev 6873)
@@ -0,0 +1,302 @@
+/*
+ * 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: QuadTree geometry
+ * Author: Nate Koenig
+ * Date: 14 July 2008
+ * CVS: $Id:$
+ */
+
+#include <ode/ode.h>
+#include <Ogre.h>
+#include <iostream>
+#include <string.h>
+#include <math.h>
+
+#include "GazeboError.hh"
+#include "OgreAdaptor.hh"
+#include "Simulator.hh"
+#include "OgreAdaptor.hh"
+#include "Global.hh"
+#include "Body.hh"
+#include "SpaceTree.hh"
+#include "QuadTreeGeom.hh"
+
+using namespace gazebo;
+
+//////////////////////////////////////////////////////////////////////////////
+// Constructor
+QuadTreeGeom::QuadTreeGeom(Body *body)
+ : Geom(body)
+{
+ this->root = new QuadNode(NULL);
+}
+
+
+//////////////////////////////////////////////////////////////////////////////
+// Destructor
+QuadTreeGeom::~QuadTreeGeom()
+{
+ if (this->root)
+ delete this->root;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+/// Update function.
+void QuadTreeGeom::UpdateChild()
+{
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Load the heightmap
+void QuadTreeGeom::LoadChild(XMLConfigNode *node)
+{
+ OgreAdaptor *ogreAdaptor;
+
+ ogreAdaptor = Simulator::Instance()->GetRenderEngine();
+
+ std::string imageFilename = node->GetString("image","",1);
+ this->mapSize = node->GetVector3("size",Vector3(10,10,10));
+
+ this->negative = node->GetBool("negative", 0);
+ this->threshold = node->GetDouble( "threshold", 200.0);
+
+ this->wallHeight = node->GetDouble( "height", 1.0, 0 );
+
+ //this->color = node->GetColor( "color", GzColor(1.0, 1.0, 1.0) );
+
+ // Make sure they are ok
+ if (this->scale <= 0) this->scale = 0.1;
+ if (this->threshold <=0) this->threshold = 200;
+ if (this->wallHeight <= 0) this->wallHeight = 1.0;
+ if (this->errBound <= 0) this->errBound = 0.0;
+
+ // Load the image
+ this->mapImage.load(imageFilename,
+ Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
+
+ 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->root->Print(" ");
+
+ printf("\n\n");
+
+ this->merged = true;
+ while (this->merged)
+ {
+ printf("---------------------------\n");
+ this->merged =false;
+ this->ReduceTree(this->root);
+ }
+
+ this->root->Print(" ");
+
+}
+
+void QuadTreeGeom::ReduceTree(QuadNode *node)
+{
+ std::vector<QuadNode*>::iterator iter;
+
+ if (!node->valid)
+ return;
+
+ if (!node->leaf)
+ {
+ for (iter = node->children.begin(); iter != node->children.end(); iter++)
+ {
+ if ((*iter)->valid)
+ this->ReduceTree((*iter));
+ }
+ }
+ else
+ {
+ this->Merge(node, this->root);
+
+/* for (unsigned int i=0; i<4; i++)
+ {
+ QuadNode *sibling = node->parent->children[i];
+ this->Merge(node, sibling);
+
+ if ( sibling->x == node->x+node->width &&
+ sibling->y == node->y &&
+ sibling->occupied == node->occupied)
+ {
+ node->width += sibling->width;
+ sibling->valid = false;
+ node->valid = true;
+
+ printf("Combine XY[%d %d][%d] XY[%d %d][%d]\n",node->x, node->y,
node->occupied, sibling->x, sibling->y, sibling->occupied);
+ }
+ }*/
+ }
+}
+
+void QuadTreeGeom::Merge(QuadNode *nodeA, QuadNode *nodeB)
+{
+ std::vector<QuadNode*>::iterator iter;
+
+ if (!nodeB)
+ return;
+
+ if (nodeB->leaf)
+ {
+ if ( nodeB->x == nodeA->x + nodeA->width &&
+ nodeB->y == nodeA->y &&
+ nodeB->occupied == nodeA->occupied )
+ {
+ nodeA->width += nodeB->width;
+ nodeB->valid = false;
+ nodeA->valid = true;
+
+ this->merged = true;
+
+ printf("Combine XY[%d %d][%d] XY[%d %d][%d]\n",nodeA->x, nodeA->y,
nodeA->occupied, nodeB->x, nodeB->y, nodeB->occupied);
+ }
+
+ /*if (nodeB->x == nodeA->x && nodeB->width == nodeA->width &&
+ nodeB->y == nodeA->y + nodeA->height &&
+ nodeB->occupied == nodeA->occupied)
+ {
+ nodeA->height += nodeA->height;
+ nodeB->valid = false;
+ nodeA->valid = true;
+
+ this->merged = true;
+
+ printf("Combine XY[%d %d][%d] XY[%d %d][%d]\n",nodeA->x, nodeA->y,
nodeA->occupied, nodeB->x, nodeB->y, nodeB->occupied);
+
+ }*/
+ }
+ else
+ {
+
+ for (iter = nodeB->children.begin(); iter != nodeB->children.end(); iter++)
+ {
+ if ((*iter)->valid)
+ this->Merge(nodeA, (*iter));
+ }
+ }
+}
+
+
+void QuadTreeGeom::BuildTree(QuadNode *node)
+{
+ QuadNode *newNode = NULL;
+ unsigned int freePixels, occPixels;
+
+ this->GetPixelCount(node->x, node->y, node->width, node->height,
+ freePixels, occPixels);
+
+ if (freePixels > 0 && occPixels > 0)
+ {
+ 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->leaf = false;
+ }
+ else if (occPixels == 0)
+ {
+ node->occupied = false;
+ node->leaf = true;
+ node->parent->leaves++;
+ }
+ else
+ {
+ node->occupied = true;
+ node->leaf = true;
+ node->parent->leaves++;
+ }
+
+}
+
+void QuadTreeGeom::GetPixelCount(unsigned int xStart, unsigned int yStart,
+ unsigned int width, unsigned int height,
+ unsigned int &freePixels,
+ unsigned int &occPixels )
+{
+ Ogre::ColourValue 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.getColourAt(x, y, 0);
+ v = (unsigned char)(255 * ((pixColor[0] + pixColor[1] + pixColor[2]) /
3.0));
+ if (this->negative)
+ v = 255 - v;
+
+ if (v < this->threshold)
+ freePixels++;
+ else
+ occPixels++;
+ }
+ }
+}
+
Added: code/gazebo/trunk/server/physics/QuadTreeGeom.hh
===================================================================
--- code/gazebo/trunk/server/physics/QuadTreeGeom.hh
(rev 0)
+++ code/gazebo/trunk/server/physics/QuadTreeGeom.hh 2008-07-16 02:48:14 UTC
(rev 6873)
@@ -0,0 +1,178 @@
+/*
+ * 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:$
+ */
+
+#ifndef QUADTREEGEOM_HH
+#define QUADTREEGEOM_HH
+
+#include <Ogre.h>
+#include <vector>
+
+#include "Vector2.hh"
+#include "Geom.hh"
+
+namespace gazebo
+{
+ /// \addtogroup gazebo_physics_geom
+ /// \{
+ /** \defgroup gazebo_occupancy_geom Occupancy grid geom
+ \brief Occupancy grid geom
+
+ \par Attributes
+ The following attributes are supported.
+
+ - image (string)
+ - Binary image that defines an occupancy grid
+ - Default: (empty)
+
+ - size (float tuple)
+ - Size of the height map
+ - Default: 0 0 0
+
+ \par Example
+ \verbatim
+ <geom:occupancygrid name="occ_geom">
+ <image>map.png</image>
+ <size>100 100 1.0</size>
+ </geom:occupancygrid>
+ \endverbatim
+ */
+ /// \}
+ /// \addtogroup gazebo_occupancy_geom
+ /// \{
+
+
+ class SpaceTree;
+ class QuadNode;
+
+ /// \brief Occupancy grid geom
+ class QuadTreeGeom : public Geom
+ {
+ /// \brief Constructor
+ public: QuadTreeGeom(Body *body);
+
+ /// \brief Destructor
+ public: virtual ~QuadTreeGeom();
+
+ /// \brief Update function
+ public: void UpdateChild();
+
+ /// \brief Load the heightmap
+ protected: virtual void LoadChild(XMLConfigNode *node);
+
+ private: void BuildTree(QuadNode *node);
+
+ private: void GetPixelCount(unsigned int xStart, unsigned int yStart,
+ unsigned int width, unsigned int height,
+ unsigned int &freePixels,
+ unsigned int &occPixels );
+
+ private: void ReduceTree(QuadNode *node);
+
+ private: void Merge(QuadNode *nodeA, QuadNode *nodeB);
+
+ private: Vector3 mapSize;
+
+ // The scale factor to apply to the geoms
+ private: float scale;
+
+ // Alignment
+ private: std::string halign, valign;
+
+ // The position of the map
+ private: Vector3 pos;
+
+ // Negative image?
+ private: int negative;
+
+ // Image color threshold used for extrusion
+ private: float threshold;
+
+ // The color of the walls
+ private: Vector3 color;
+
+ // The amount of acceptable error in the model
+ private: float errBound;
+
+ private: float wallHeight;
+
+ // The map dimensions
+ private: unsigned int mapWidth;
+ private: unsigned int mapHeight;
+
+ private: Ogre::Image mapImage;
+
+ private: QuadNode *root;
+ private: bool merged;
+ };
+
+
+ class QuadNode
+ {
+ public: QuadNode( QuadNode *_parent )
+ {
+ parent = _parent;
+ occupied = false;
+ leaf = true;
+ leaves = 0;
+ valid = true;
+ }
+
+ public: ~QuadNode()
+ {
+ std::vector<QuadNode*>::iterator iter;
+ for (iter = children.begin(); iter != children.end(); iter++)
+ delete (*iter);
+ }
+
+ public: void Print(std::string space)
+ {
+ std::vector<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)->valid)
+ (*iter)->Print(space);
+ }
+
+ public: unsigned int x, y;
+ public: unsigned int width, height;
+
+ public: std::vector<Geom*> geoms;
+
+ public: QuadNode *parent;
+ public: std::vector<QuadNode*> children;
+ public: bool occupied;
+ public: bool leaf;
+ public: int leaves;
+
+ public: bool valid;
+ };
+
+ /// \}
+}
+
+#endif
Modified: code/gazebo/trunk/server/physics/SConscript
===================================================================
--- code/gazebo/trunk/server/physics/SConscript 2008-07-16 01:26:03 UTC (rev
6872)
+++ code/gazebo/trunk/server/physics/SConscript 2008-07-16 02:48:14 UTC (rev
6873)
@@ -24,6 +24,8 @@
'TrimeshGeom.cc',
'HeightmapGeom.cc',
'OccupancyGridGeom.cc',
+ 'SpaceTree.cc',
+ 'QuadTreeGeom.cc'
]
headers.append(
@@ -43,8 +45,9 @@
'server/physics/SliderJoint.hh',
'server/physics/SphereGeom.hh',
'server/physics/TrimeshGeom.hh',
- 'server/physics/UniversalJoint.hh'
- 'server/physics/OccupancyGridGeom.hh'
+ 'server/physics/UniversalJoint.hh',
+ 'server/physics/OccupancyGridGeom.hh',
+ 'server/physics/SpaceTree.hh'
] )
staticObjs.append( env.StaticObject(sources) )
Added: code/gazebo/trunk/server/physics/SpaceTree.cc
===================================================================
--- code/gazebo/trunk/server/physics/SpaceTree.cc
(rev 0)
+++ code/gazebo/trunk/server/physics/SpaceTree.cc 2008-07-16 02:48:14 UTC
(rev 6873)
@@ -0,0 +1,316 @@
+/*
+ * 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: A QuadTree to logically segment the geometries in a map
+ * Author: Nate Koenig
+ * Date: 28 July 2003
+ * CVS: $Id: SpaceTree.cc,v 1.14 2004/08/16 18:34:24 natepak Exp $
+ */
+
+#include <GL/gl.h>
+#include <assert.h>
+
+#include "Body.hh"
+#include "SpaceTree.hh"
+#include "BoxGeom.hh"
+
+using namespace gazebo;
+
+//////////////////////////////////////////////////////////////////////////////
+// Constructor
+SpaceNode::SpaceNode( SpaceNode *parent, float x, float y, float hl, float hh )
+ : parent( parent ), isLeaf( false ), cx( x ), cy( y ),
+ halfWidth( hl ), halfHeight( hh ),
+ walls(NULL), wallCount(0), wallMaxCount(0),
+ geoms(NULL), geomCount(0), geomMaxCount(0)
+{
+ for (int i=0; i<4; i++)
+ this->children[i] = NULL;
+}
+
+
+//////////////////////////////////////////////////////////////////////////////
+// Destructor
+SpaceNode::~SpaceNode()
+{
+ int i=0;
+
+ this->parent = NULL;
+
+ for (int i=0; i<this->geomCount; i++)
+ {
+ delete this->geoms[i];
+ }
+
+ free(geoms);
+
+ // Delete all children
+ if (this->isLeaf)
+ {
+ dSpaceDestroy( this->spaceId );
+ }
+ else
+ {
+ // Delete all children
+ for (i=0; i<4; i++)
+ {
+ delete this->children[i];
+ this->children[i] = NULL;
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Add a wall to this space
+void SpaceNode::AddWall( Line *line )
+{
+ if (this->wallCount >= this->wallMaxCount)
+ {
+ this->wallMaxCount += 10;
+ this->walls = (Line**) realloc(this->walls, this->wallMaxCount *
+ sizeof(this->walls[0]));
+
+ assert(this->walls);
+ }
+
+ this->walls[this->wallCount++] = line;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Add a geom to this space
+void SpaceNode::AddGeom( Geom *geom )
+{
+ if (this->geomCount >= this->geomMaxCount)
+ {
+ this->geomMaxCount += 10;
+ this->geoms = (Geom**) realloc(this->geoms, this->geomMaxCount *
+ sizeof(this->geoms[0]));
+
+ assert(this->geoms);
+ }
+
+ this->geoms[this->geomCount++] = geom;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Create the ODE geometries
+void SpaceNode::GenerateGeoms( dSpaceID parentSpace, Body *body,
+ float wallWidth, float wallHeight,
+ Vector3 &pos, float scale,
+ Vector3 color)
+{
+
+ // Generate recursive spaces (maximizes collision detection performance)
+ this->spaceId = dSimpleSpaceCreate( parentSpace );
+
+ // We are a space of fixed objects, and shouldnt collide with ourself
+ dGeomSetCategoryBits( (dGeomID) this->spaceId, GZ_FIXED_COLLIDE );
+ dGeomSetCollideBits( (dGeomID) this->spaceId, ~GZ_FIXED_COLLIDE );
+
+ if (this->isLeaf)
+ {
+
+ for (int i=0; i<this->wallCount; i++)
+ {
+ std::ostringstream stream;
+
+ // Align the walls with the ground properly
+ float z = wallHeight/2.0;
+
+ // Create the box geometry
+ BoxGeom* newBox = new BoxGeom( body );
+
+ XMLConfig *boxConfig = new XMLConfig();
+ Quatern rot;
+ Vector3 rpy;
+ rot.SetFromAxis( 0, 0, 1, this->walls[i]->Angle() );
+ rpy = rot.GetAsEuler();
+
+ stream << "<gazebo:world
xmlns:gazebo=\"http://playerstage.sourceforge.net/gazebo/xmlschema/#gz\";
xmlns:geom=\"http://playerstage.sourceforge.net/gazebo/xmlschema/#geom\";>";
+
+ stream << "<geom:box name='occ_geom'>";
+ stream << " <mass>0.0</mass>";
+ stream << " <xyz>" << this->walls[i]->Mid().x * scale + pos.x << " " <<
+ this->walls[i]->Mid().y * scale + pos.y << " " << z + pos.z
<< "</xyz>";
+ stream << " <rpy>" << RTOD(rpy.x) << " " << RTOD(rpy.y) << " " <<
RTOD(rpy.z) << "</rpy>";
+ stream << " <size>" << this->walls[i]->Length() * scale + wallWidth *
scale << " " << wallWidth * scale << " " << wallHeight << "</size>";
+ stream << " <visual>";
+ stream << " <mesh>unit_box</mesh>";
+ stream << " <material>Gazebo/Rocky</material>";
+ stream << " <size>" << this->walls[i]->Length() * scale + wallWidth *
scale << " " << wallWidth * scale << " " << wallHeight << "</size>";
+ stream << " </visual>";
+ stream << "</geom:box>";
+ stream << "</gazebo:world>";
+
+ boxConfig->LoadString( stream.str() );
+
+ newBox->Load( boxConfig->GetRootNode()->GetChild() );
+ delete boxConfig;
+ }
+ }
+ else if (children[0])
+ {
+ for (int i=0; i<4; i++)
+ this->children[i]->GenerateGeoms( this->spaceId, body, wallWidth,
+ wallHeight, pos, scale,color);
+ }
+
+ /* TESTING
+ // Display the actual bounding box of the space
+ if (dSpaceGetNumGeoms(this->spaceId) > 0)
+ {
+ dReal box[6];
+ dGeomGetAABB((dGeomID) this->spaceId, box);
+
+ BoxGeom *bBox = new BoxGeom( 0,
+ box[1] - box[0],
+ box[3] - box[2],
+ box[5] - box[4]);
+ bBox->SetPosition((box[1] + box[0]) / 2,
+ (box[3] + box[2]) / 2,
+ (box[5] + box[4]) / 2);
+
+ bBox->SetColor( 1, 0, 0);
+
+ bBox->SetCategoryBits( 0 );
+ bBox->SetCollideBits( 0 );
+
+ this->geoms.PushBack( bBox );
+ }
+ */
+
+ // Destroy the newly created space if it has nothing in it. This is
+ // particularly important for getting the recursive spaces right.
+ if (dSpaceGetNumGeoms(this->spaceId) == 0)
+ {
+ dSpaceDestroy(this->spaceId);
+ this->spaceId = 0;
+ }
+
+ return;
+}
+
+
+
+//////////////////////////////////////////////////////////////////////////////
+// Constructor
+SpaceTree::SpaceTree()
+ : root( NULL ), linesPerNode( 10 )
+{
+}
+
+
+//////////////////////////////////////////////////////////////////////////////
+// Destructor
+SpaceTree::~SpaceTree()
+{
+ // Delete the entire tree
+ if (this->root)
+ delete this->root;
+}
+
+
+//////////////////////////////////////////////////////////////////////////////
+// Return the root node
+SpaceNode *SpaceTree::GetRoot() const
+{
+ return this->root;
+}
+
+
+//////////////////////////////////////////////////////////////////////////////
+// Build the tree from a list of line segements.
+void SpaceTree::BuildTree( Line **lines, unsigned int lineCount,
+ float width, float height )
+{
+
+ if (lineCount <= this->linesPerNode)
+ {
+ // Create the root node, and make it a leaf
+ this->root = new SpaceNode( NULL, width/2.0, height/2.0,
+ width/2.0, height/2.0);
+ this->root->isLeaf = true;
+
+ // Add all the lines to the root node
+ for (unsigned int i=0; i<lineCount; i++)
+ this->root->AddWall( lines[i] );
+ }
+ else
+ {
+
+ // Recursibely create the tree
+ this->root = this->Construct( NULL, width/2.0, height/2.0, width/2.0,
+ height/2.0, lines, lineCount );
+ }
+
+}
+
+
+//////////////////////////////////////////////////////////////////////////////
+// Called from BuildTree to recursively build the tree
+SpaceNode *SpaceTree::Construct( SpaceNode *parent, float cx, float cy,
+ float hw, float hh,
+ Line** lines, unsigned int lineCount )
+{
+
+ SpaceNode *node = new SpaceNode( parent, cx, cy, hw, hh );
+
+ // If the count is within range, then create a leaf node
+ if (lineCount < this->linesPerNode)
+ {
+ node->isLeaf = true;
+
+ for (unsigned int i=0; i<lineCount; i++)
+ node->AddWall( lines[i] );
+
+ }
+
+ // Otherwise create a new node and its children
+ else if (lineCount > 0)
+ {
+ delete node->walls;
+ unsigned int i=0;
+
+ // Grab the lines contained within this node
+ while (i != lineCount)
+ {
+ if (lines[i]->Mid().x < cx+hw && lines[i]->Mid().x >= cx-hw &&
+ lines[i]->Mid().y < cy+hh && lines[i]->Mid().y >= cy-hh )
+ {
+ node->AddWall( lines[i] );
+ //iter = lines.Erase( iter );
+ }
+ i++;
+ }
+
+ // Create the node's children
+ node->children[0] = this->Construct( node, cx+hw/2.0, cy+hh/2.0, hw/2.0,
+ hh/2.0, node->walls, node->wallCount
);
+ node->children[1] = this->Construct( node, cx-hw/2.0, cy+hh/2.0, hw/2.0,
+ hh/2.0, node->walls, node->wallCount
);
+ node->children[2] = this->Construct( node, cx-hw/2.0, cy-hh/2.0, hw/2.0,
+ hh/2.0, node->walls, node->wallCount);
+ node->children[3] = this->Construct( node, cx+hw/2.0, cy-hh/2.0, hw/2.0,
+ hh/2.0, node->walls, node->wallCount
);
+ }
+
+ return node;
+}
+
Added: code/gazebo/trunk/server/physics/SpaceTree.hh
===================================================================
--- code/gazebo/trunk/server/physics/SpaceTree.hh
(rev 0)
+++ code/gazebo/trunk/server/physics/SpaceTree.hh 2008-07-16 02:48:14 UTC
(rev 6873)
@@ -0,0 +1,120 @@
+/*
+ * 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: A QuadTree to logically segment the geometries in a map
+ * Author: Nate Koenig
+ * Date: 28 July 2003
+ * CVS: $Id: SpaceTree.hh,v 1.11 2004/10/10 17:57:29 inspectorg Exp $
+ */
+
+#ifndef SPACETREE_HH
+#define SPACETREE_HH
+
+#include "OccupancyGridGeom.hh"
+#include "Global.hh"
+
+namespace gazebo
+{
+
+ // Forward declarations
+ class Body;
+
+ class SpaceNode
+ {
+ // Constructor
+ public: SpaceNode( SpaceNode *parent, float x, float y, float hw, float hh
);
+
+ // Destructor
+ public: virtual ~SpaceNode();
+
+ // Add a wall to the space
+ public: void AddWall( Line *line );
+
+ // Add a geom to the space
+ public: void AddGeom( Geom *geom );
+
+ // Create the ODE geometries
+ public: void GenerateGeoms(dSpaceID spaceId, Body *body, float wallWidth,
+ float wallHeight, Vector3 &pos, float scale,
+ Vector3 color);
+
+ // The parent of this node
+ public: SpaceNode *parent;
+
+ // Is this node a leaf??
+ public: bool isLeaf;
+
+ // Each node has 4 children since this is a quad tree
+ public: SpaceNode *children[4];
+
+ // The center location of this node
+ public: float cx, cy;
+
+ // The half length of the rectangle (in x direction)
+ public: float halfWidth;
+
+ // The half height of the rectangle (in y direction)
+ public: float halfHeight;
+
+ // The lines representing walls contained in this space
+ public: Line** walls;
+ public: int wallCount;
+ public: int wallMaxCount;
+
+ public: Geom** geoms;
+ public: int geomCount;
+ public: int geomMaxCount;
+
+ // The spaceId for this node
+ private: dSpaceID spaceId;
+ };
+
+
+ class SpaceTree
+ {
+ // Constructor
+ public: SpaceTree();
+
+ // Destructor
+ public: virtual ~SpaceTree();
+
+ // Return the root node of the tree
+ public: SpaceNode *GetRoot() const;
+
+ // Build the tree from a list of line segments
+ public: void BuildTree( Line** lines, unsigned int numLines,
+ float width, float height );
+
+ // Called from BuildTree to recursively build the tree
+ private: SpaceNode *Construct( SpaceNode *parent, float cx, float cy,
+ float hw, float hh,
+ Line** lines, unsigned int lineCount );
+
+ // The root node
+ private: SpaceNode *root;
+
+ // Then maxmimum number of lines per node
+ private: unsigned int linesPerNode;
+
+ };
+
+}
+
+#endif
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
-------------------------------------------------------------------------
This SF.Net email is sponsored by the Moblin Your Move Developer's challenge
Build the coolest Linux based applications with Moblin SDK & win great prizes
Grand prize is a trip for two to an Open Source event anywhere in the world
http://moblin-contest.org/redirect.php?banner_id=100&url=/
_______________________________________________
Playerstage-commit mailing list
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
