Revision: 6875
http://playerstage.svn.sourceforge.net/playerstage/?rev=6875&view=rev
Author: natepak
Date: 2008-07-15 22:49:40 -0700 (Tue, 15 Jul 2008)
Log Message:
-----------
Fixed quadtree map loader
Modified Paths:
--------------
code/gazebo/trunk/server/physics/QuadTreeGeom.cc
code/gazebo/trunk/server/physics/QuadTreeGeom.hh
code/gazebo/trunk/worlds/quadtree.world
Modified: code/gazebo/trunk/server/physics/QuadTreeGeom.cc
===================================================================
--- code/gazebo/trunk/server/physics/QuadTreeGeom.cc 2008-07-16 02:48:43 UTC
(rev 6874)
+++ code/gazebo/trunk/server/physics/QuadTreeGeom.cc 2008-07-16 05:49:40 UTC
(rev 6875)
@@ -30,6 +30,7 @@
#include <string.h>
#include <math.h>
+#include "BoxGeom.hh"
#include "GazeboError.hh"
#include "OgreAdaptor.hh"
#include "Simulator.hh"
@@ -79,6 +80,7 @@
this->threshold = node->GetDouble( "threshold", 200.0);
this->wallHeight = node->GetDouble( "height", 1.0, 0 );
+ this->scale = node->GetDouble("scale",1.0,0);
//this->color = node->GetColor( "color", GzColor(1.0, 1.0, 1.0) );
@@ -98,95 +100,158 @@
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(" ");
+ this->CreateBoxes(this->root);
+ this->root->Print("");
+}
+void QuadTreeGeom::CreateBoxes(QuadNode *node)
+{
+ if (node->leaf)
+ {
+ if (!node->valid || !node->occupied)
+ return;
+
+ std::ostringstream stream;
+
+ // Create the box geometry
+ BoxGeom* newBox = new BoxGeom( body );
+
+ 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->scale;
+ float y = (node->y + node->height / 2.0) * this->scale;
+ float w = (node->width) * this->scale;
+ float h = (node->height) * this->scale;
+
+ stream << "<geom:box name='occ_geom'>";
+ stream << " <mass>0.0</mass>";
+ stream << " <xyz>" << x << " " << y << " " << 0.25 << "</xyz>";
+ stream << " <rpy>0 0 0</rpy>";
+ stream << " <size>" << w << " " << h << " " << 0.5 << "</size>";
+ stream << " <visual>";
+ stream << " <mesh>unit_box</mesh>";
+ stream << " <material>Gazebo/Rocky</material>";
+ stream << " <size>" << w << " " << h << " " << 0.5 << "</size>";
+ stream << " </visual>";
+ stream << "</geom:box>";
+ stream << "</gazebo:world>";
+
+ std::cout << stream.str() << "\n\n";
+ 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);
+ }
+ }
}
void QuadTreeGeom::ReduceTree(QuadNode *node)
{
- std::vector<QuadNode*>::iterator iter;
+ std::deque<QuadNode*>::iterator iter;
if (!node->valid)
return;
if (!node->leaf)
{
- for (iter = node->children.begin(); iter != node->children.end(); iter++)
+ unsigned int count = 0;
+ int size = node->children.size();
+
+ for (int i = 0; i < size; i++)
{
- if ((*iter)->valid)
- this->ReduceTree((*iter));
+ if (node->children[i]->valid)
+ {
+ this->ReduceTree(node->children[i]);
+ }
+ if (node->children[i]->leaf)
+ count++;
}
- }
- else
- {
- this->Merge(node, this->root);
-/* for (unsigned int i=0; i<4; i++)
+ if (node->parent && count == node->children.size())
{
- 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)
+ for (iter = node->children.begin(); iter != node->children.end(); iter++)
{
- node->width += sibling->width;
- sibling->valid = false;
- node->valid = true;
+ 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;
+ }
+ }
+ }
+ }
- printf("Combine XY[%d %d][%d] XY[%d %d][%d]\n",node->x, node->y,
node->occupied, sibling->x, sibling->y, sibling->occupied);
- }
- }*/
}
+ else
+ {
+ this->Merge(node, node->parent);
+ }
}
void QuadTreeGeom::Merge(QuadNode *nodeA, QuadNode *nodeB)
{
- std::vector<QuadNode*>::iterator iter;
+ 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->occupied == nodeA->occupied )
+ nodeB->height == nodeA->height )
{
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)
+ if (nodeB->x == nodeA->x &&
+ nodeB->width == nodeA->width &&
+ nodeB->y == nodeA->y + nodeA->height )
{
- nodeA->height += nodeA->height;
+ nodeA->height += nodeB->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
{
@@ -194,7 +259,9 @@
for (iter = nodeB->children.begin(); iter != nodeB->children.end(); iter++)
{
if ((*iter)->valid)
+ {
this->Merge(nodeA, (*iter));
+ }
}
}
}
@@ -208,7 +275,9 @@
this->GetPixelCount(node->x, node->y, node->width, node->height,
freePixels, occPixels);
- if (freePixels > 0 && occPixels > 0)
+ int diff = labs(freePixels - occPixels);
+
+ if ( diff > 10)
{
float newX, newY;
float newW, newH;
Modified: code/gazebo/trunk/server/physics/QuadTreeGeom.hh
===================================================================
--- code/gazebo/trunk/server/physics/QuadTreeGeom.hh 2008-07-16 02:48:43 UTC
(rev 6874)
+++ code/gazebo/trunk/server/physics/QuadTreeGeom.hh 2008-07-16 05:49:40 UTC
(rev 6875)
@@ -28,7 +28,7 @@
#define QUADTREEGEOM_HH
#include <Ogre.h>
-#include <vector>
+#include <deque>
#include "Vector2.hh"
#include "Geom.hh"
@@ -93,6 +93,8 @@
private: void Merge(QuadNode *nodeA, QuadNode *nodeB);
+ private: void CreateBoxes(QuadNode *node);
+
private: Vector3 mapSize;
// The scale factor to apply to the geoms
@@ -142,29 +144,28 @@
public: ~QuadNode()
{
- std::vector<QuadNode*>::iterator iter;
+ /*std::deque<QuadNode*>::iterator iter;
for (iter = children.begin(); iter != children.end(); iter++)
delete (*iter);
+ */
}
public: void Print(std::string space)
{
- std::vector<QuadNode*>::iterator iter;
+ 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)->valid)
+ if ((*iter)->occupied)
(*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: std::deque<QuadNode*> children;
public: bool occupied;
public: bool leaf;
public: int leaves;
Modified: code/gazebo/trunk/worlds/quadtree.world
===================================================================
--- code/gazebo/trunk/worlds/quadtree.world 2008-07-16 02:48:43 UTC (rev
6874)
+++ code/gazebo/trunk/worlds/quadtree.world 2008-07-16 05:49:40 UTC (rev
6875)
@@ -29,28 +29,6 @@
<rendering:gui>
<size>800 600</size>
<pos>100 100</pos>
- <frames>
- <row height="50%">
- <camera width="50%">
- <xyz>-1 0 4</xyz>
- <rpy>0 34 0</rpy>
- </camera>
- <camera width="50%">
- <xyz>0 0 10</xyz>
- <rpy>0 90 0</rpy>
- </camera>
- </row>
- <row height="50%">
- <camera width="50%">
- <xyz>0 0 0</xyz>
- <rpy>0 0 0</rpy>
- </camera>
- <camera width="50%">
- <xyz>10 0 1</xyz>
- <rpy>0 0 90</rpy>
- </camera>
- </row>
- </frames>
</rendering:gui>
<rendering:ogre>
@@ -89,8 +67,9 @@
<body:occupancy_grid name="grid_body">
<geom:quadtree name="grid_geom">
- <image>map.png</image>
+ <image>willowMap.png</image>
<negative>true</negative>
+ <scale>0.1</scale>
</geom:quadtree>
</body:occupancy_grid>
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
