From: <na...@us...> - 2008-07-16 18:42:42
|
Revision: 6879 http://playerstage.svn.sourceforge.net/playerstage/?rev=6879&view=rev Author: natepak Date: 2008-07-17 01:42:48 +0000 (Thu, 17 Jul 2008) Log Message: ----------- Update MapGeom Modified Paths: -------------- code/gazebo/trunk/server/physics/Body.cc code/gazebo/trunk/server/physics/Geom.cc code/gazebo/trunk/server/physics/SConscript code/gazebo/trunk/server/rendering/OgreCreator.cc code/gazebo/trunk/server/rendering/OgreVisual.cc code/gazebo/trunk/server/rendering/OgreVisual.hh Added Paths: ----------- code/gazebo/trunk/server/physics/MapGeom.cc code/gazebo/trunk/server/physics/MapGeom.hh code/gazebo/trunk/worlds/map.world Removed Paths: ------------- code/gazebo/trunk/server/physics/OccupancyGridGeom.cc code/gazebo/trunk/server/physics/OccupancyGridGeom.hh 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 code/gazebo/trunk/worlds/occupancygrid.world code/gazebo/trunk/worlds/quadtree.world Modified: code/gazebo/trunk/server/physics/Body.cc =================================================================== --- code/gazebo/trunk/server/physics/Body.cc 2008-07-17 01:31:38 UTC (rev 6878) +++ code/gazebo/trunk/server/physics/Body.cc 2008-07-17 01:42:48 UTC (rev 6879) @@ -28,8 +28,7 @@ #include "GazeboMessage.hh" #include "HeightmapGeom.hh" -#include "OccupancyGridGeom.hh" -#include "QuadTreeGeom.hh" +#include "MapGeom.hh" #include "OgreVisual.hh" #include "Global.hh" #include "Vector2.hh" @@ -414,14 +413,11 @@ this->SetStatic(true); geom = new HeightmapGeom(this); } - else if (node->GetName() == "occupancy_grid") + else if (node->GetName() == "map") { - geom = new OccupancyGridGeom(this); + this->SetStatic(true); + geom = new MapGeom(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/Geom.cc =================================================================== --- code/gazebo/trunk/server/physics/Geom.cc 2008-07-17 01:31:38 UTC (rev 6878) +++ code/gazebo/trunk/server/physics/Geom.cc 2008-07-17 01:42:48 UTC (rev 6879) @@ -128,6 +128,10 @@ childNode = childNode->GetNext("visual"); } + if (this->IsStatic()) + { + this->visualNode->MakeStatic(); + } // Create the bounding box if (this->geomId && dGeomGetClass(this->geomId) != dPlaneClass) @@ -175,6 +179,7 @@ } + //////////////////////////////////////////////////////////////////////////////// // Set the encapsulated geometry object void Geom::SetGeom(dGeomID geomId, bool placeable) Added: code/gazebo/trunk/server/physics/MapGeom.cc =================================================================== --- code/gazebo/trunk/server/physics/MapGeom.cc (rev 0) +++ code/gazebo/trunk/server/physics/MapGeom.cc 2008-07-17 01:42:48 UTC (rev 6879) @@ -0,0 +1,370 @@ +/* + * 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:$ + */ + +#include <ode/ode.h> +#include <Ogre.h> +#include <iostream> +#include <string.h> +#include <math.h> + +#include "BoxGeom.hh" +#include "GazeboError.hh" +#include "OgreAdaptor.hh" +#include "Simulator.hh" +#include "OgreAdaptor.hh" +#include "Global.hh" +#include "Body.hh" +#include "MapGeom.hh" + +using namespace gazebo; + +////////////////////////////////////////////////////////////////////////////// +// Constructor +MapGeom::MapGeom(Body *body) + : Geom(body) +{ + this->root = new QuadNode(NULL); +} + + +////////////////////////////////////////////////////////////////////////////// +// Destructor +MapGeom::~MapGeom() +{ + if (this->root) + delete this->root; +} + +////////////////////////////////////////////////////////////////////////////// +/// Update function. +void MapGeom::UpdateChild() +{ +} + +//////////////////////////////////////////////////////////////////////////////// +/// Load the heightmap +void MapGeom::LoadChild(XMLConfigNode *node) +{ + OgreAdaptor *ogreAdaptor; + + ogreAdaptor = Simulator::Instance()->GetRenderEngine(); + + std::string imageFilename = node->GetString("image","",1); + + this->negative = node->GetBool("negative", 0); + + this->threshold = node->GetDouble( "threshold", 200.0); + + this->wallHeight = node->GetDouble( "height", 1.0, 0 ); + + this->scale = node->GetDouble("scale",1.0,0); + + this->material = node->GetString("material", "", 0); + + this->granularity = node->GetInt("granularity", 5, 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; + + // 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->merged = true; + while (this->merged) + { + this->merged =false; + this->ReduceTree(this->root); + } + + this->CreateBoxes(this->root); +} + +void MapGeom::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 z = this->wallHeight / 2.0; + float xSize = (node->width) * this->scale; + float ySize = (node->height) * this->scale; + float zSize = this->wallHeight; + + stream << "<geom:box name='map_geom'>"; + 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->material << "</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); + } + } +} + +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); + } +} + +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)); + } + } + } +} + + +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 (diff > this->granularity) + { + 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; + } + else + { + node->occupied = true; + node->leaf = true; + } + +} + +void MapGeom::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/MapGeom.hh =================================================================== --- code/gazebo/trunk/server/physics/MapGeom.hh (rev 0) +++ code/gazebo/trunk/server/physics/MapGeom.hh 2008-07-17 01:42:48 UTC (rev 6879) @@ -0,0 +1,188 @@ +/* + * 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 MAPGEOM_HH +#define MAPGEOM_HH + +#include <Ogre.h> +#include <deque> + +#include "Vector2.hh" +#include "Geom.hh" + +namespace gazebo +{ + /// \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 MapGeom : public Geom + { + /// \brief Constructor + public: MapGeom(Body *body); + + /// \brief Destructor + public: virtual ~MapGeom(); + + /// \brief Update function + public: void UpdateChild(); + + /// \brief Load the heightmap + protected: virtual void LoadChild(XMLConfigNode *node); + + /// \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); + + /// \brief Create the boxes for the map + private: void CreateBoxes(QuadNode *node); + + // The scale factor to apply to the geoms + private: float scale; + + // Negative image? + private: int negative; + + // Image color threshold used for extrusion + private: float threshold; + + // The color of the walls + private: std::string material; + + // The amount of acceptable error in the model + private: int granularity; + + private: float wallHeight; + + private: Ogre::Image mapImage; + + private: QuadNode *root; + + private: bool merged; + }; + + + 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/trunk/server/physics/OccupancyGridGeom.cc =================================================================== --- code/gazebo/trunk/server/physics/OccupancyGridGeom.cc 2008-07-17 01:31:38 UTC (rev 6878) +++ code/gazebo/trunk/server/physics/OccupancyGridGeom.cc 2008-07-17 01:42:48 UTC (rev 6879) @@ -1,512 +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: OccupancyGrid 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 "OccupancyGridGeom.hh" - -using namespace gazebo; - -////////////////////////////////////////////////////////////////////////////// -// Constructor -OccupancyGridGeom::OccupancyGridGeom(Body *body) - : Geom(body) -{ - this->tree = NULL; -} - - -////////////////////////////////////////////////////////////////////////////// -// Destructor -OccupancyGridGeom::~OccupancyGridGeom() -{ - if (this->tree) - delete this->tree; -} - -////////////////////////////////////////////////////////////////////////////// -/// Update function. -void OccupancyGridGeom::UpdateChild() -{ -} - -//////////////////////////////////////////////////////////////////////////////// -/// Load the heightmap -void OccupancyGridGeom::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->wallWidth = node->GetDouble( "width", 0.1, 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->wallWidth <=0) this->wallWidth = 0.1; - 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); - - - // Limit the format to 8-bit grayscale - if (this->mapImage.getFormat() != Ogre::PF_L8) - { - gzerr(0) << "Invalid image format[" << this->mapImage.getFormat() << "]\n"; - } - - // Create the 2d lines of the map - this->GenerateLines(); - printf("OccupancyGrid: found [%d] walls\n",this->wallCount); - - - // Create the quad tree - this->tree = new SpaceTree(); - tree->BuildTree( this->walls, this->wallCount, - this->mapWidth, this->mapHeight ); - - // Create the extruded geometry - this->GenerateGeometry(); -} - -////////////////////////////////////////////////////////////////////////////// -// Get the 2d lines from an image file -void OccupancyGridGeom::GenerateLines() -{ - unsigned int x, y; - unsigned char v; - MapPoint **map; - MapPoint *pt; - Ogre::ColourValue pixColor; - - this->mapWidth = this->mapImage.getWidth(); - this->mapHeight = this->mapImage.getHeight(); - - //imageData = this->mapImage->getData(); - - // We may need to change the position of the map - if (this->halign == "center") - this->pos.x -= this->mapWidth / 2.0 * this->scale; - if (this->valign == "center") - this->pos.y -= this->mapHeight / 2.0 * this->scale; - - // Allocate on heap (some platforms have stack-size limit) - map = new MapPoint*[this->mapWidth * this->mapHeight]; - - for (y=0; y< this->mapHeight; y++) - { - for (x=0; x<this->mapWidth; x++) - { - // Compute the pixel value - 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; - - // 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) - { - // Create the new point - map[y*this->mapWidth+x] = new MapPoint( x, y ); - - // Point to the North - if (y>0 && map[(y-1)*this->mapWidth+x] != NULL) - { - map[y*this->mapWidth+x]->arcs[0] = map[(y-1)*this->mapWidth+x]; - map[y*this->mapWidth+x]->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 (x>0 && y>0 && map[(y-1)*this->mapWidth+x-1] != NULL) - { - map[y*this->mapWidth+x]->arcs[7] = map[(y-1)*this->mapWidth+x-1]; - map[y*this->mapWidth+x]->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 (x>0 && map[y*this->mapWidth+x-1] != NULL) - { - map[y*this->mapWidth+x]->arcs[6] = map[y*this->mapWidth+x-1]; - map[y*this->mapWidth+x]->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 (x+1<this->mapWidth && y>0 && map[(y-1)*this->mapWidth+x+1] != NULL) - { - map[y*this->mapWidth+x]->arcs[1] = map[(y-1)*this->mapWidth+x+1]; - map[y*this->mapWidth+x]->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 - // if (c+1<this->mapWidth && map[r*this->mapWidth+c+1] != NULL) - // { - // map[r*this->mapWidth+c]->arcs[2] = map[r*this->mapWidth+c+1]; - // map[r*this->mapWidth+c]->arcCount++; - - // map[r*this->mapWidth+c+1]->arcs[6] = map[r*this->mapWidth+c]; - // map[r*this->mapWidth+c+1]->arcCount++; - // } - - } - else - { - map[y*this->mapWidth+x] = NULL; - } - } - } - - for (y=0; y<this->mapHeight; y++) - { - for (x=0; x<this->mapWidth; x++) - { - pt = map[y*this->mapWidth+x]; - - // If the map point is valid - if (pt != NULL && pt->arcCount < 8) - { - // Generate a line to the east - this->GenerateLine(pt,2); - - // Generate a line to the south - this->GenerateLine(pt,4); - - delete pt; - pt = NULL; - } - } - } - - // Free the map data - delete [] map; - - // If the user has specified an error bound, then reduce the graph. - // This will only reduce curved surefaces - /*if (this->errBound > 0) - { - ReduceLines(); - }*/ - -} - -////////////////////////////////////////////////////////////////////////////// -// Create a single line starting from point pt in direction dir -void OccupancyGridGeom::GenerateLine( MapPoint* pt, int dir ) -{ - assert( dir>=0 && dir<=7 ); - - if (pt->arcs[dir] != NULL) - { - MapPoint* next = pt; - MapPoint* tmp; - //float err=0; - - while (next->arcs[dir] != NULL && next->arcs[dir]->arcCount < 8) - { - tmp = next->arcs[dir]; - next->arcCount=8; - next->arcs[dir] = NULL; - tmp->arcs[(dir+4)%8] = NULL; - next = tmp; - } - - if (next != pt) - { - Line* newLine = new Line(); - newLine->Set( *pt, *next ); - this->AddWall( newLine ); - } - } - -} - - -////////////////////////////////////////////////////////////////////////////// -// Reduce the number of lines in the map -void OccupancyGridGeom::ReduceLines() -{ - int i,j; - Line *line1 = NULL; - Line *line2 = NULL; - - - // Loop through all the walls - for( i=0; i<this->wallCount; i++) - { - line1 = this->walls[i]; - - // Loop through all the walls - for (j=i + 1; j<this->wallCount; j++) - { - line2 = this->walls[j]; - - // If the end point of one line connects with the start point of - // another, then try to reduce - if ((line1->End() == line2->Start() && - (fabs( line1->Start().x - line2->End().x ) + - fabs( line1->Start().y - line2->End().y ) <= this->errBound))) - - { - line1->End( line2->End() ); - this->EraseWall(j); - j--; - } - - // If the start point of one line connects with the start point of - // another, then try to reduce - else if (i != j && (line1->Start() == line2->Start()) && - (fabs( line1->End().x - line2->End().x ) + - fabs( line1->End().y - line2->End().y ) <= this->errBound)) - { - - line1->Set( line1->End(), line2->End() ); - this->EraseWall(j); - j--; - } - - // If the end point of one line connects with the end point of - // another, then try to reduce - else if (i != j && (line1->End() == line2->End()) && - (fabs( line1->Start().x - line2->Start().x ) + - fabs( line1->Start().y - line2->Start().y ) <= this->errBound)) - { - line1->End( line2->Start() ); - this->EraseWall(j); - j--; - } - } - - } - -} - - -////////////////////////////////////////////////////////////////////////////// -// 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 ); - - this->tree->GetRoot()->GenerateGeoms( this->spaceId, this->body, - this->wallWidth, this->wallHeight, this->pos, - this->scale, this->color); - - //this->AddSpaceGeoms(this->tree->GetRoot()); -} - -/*void OccupancyGridGeom::AddSpaceGeoms( SpaceNode *node) -{ - //int i = 0; - if (node==NULL) - return; - - // FIX - //for (i=0; i<node->geomCount; i++) - //{ - // this->AddGeom(node->geoms[i]); - //} - - //for (i=0; i<4; i++) - //{ - // this->AddSpaceGeoms(node->children[i]); - //} -}*/ - - -////////////////////////////////////////////////////////////////////////////// -// Calculates the shortest distance from a point to a line -float OccupancyGridGeom::PointLineDist( const MapPoint &p1, const MapPoint &p2, - const MapPoint &p ) -{ - return fabs( (p2.x-p1.x)*(p1.y-p.y) - (p1.x-p.x)*(p2.y-p1.y) ) / - sqrt( (p2.x-p1.x)*(p2.x-p1.x) + (p2.y-p1.y)*(p2.y-p1.y) ); -} - -////////////////////////////////////////////////////////////////////////////// -// Adds a wall -void OccupancyGridGeom::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; -} - -////////////////////////////////////////////////////////////////////////////// -// Deletes a wall segment -void OccupancyGridGeom::EraseWall( int index ) -{ - assert( index >=0 && index < this->wallCount ); - - delete this->walls[index]; - this->walls[index] = 0; - - this->wallCount--; - - for (int i=index; i<this->wallCount; i++) - { - this->walls[i] = this->walls[i+1]; - } - -} - - -Line::Line() -{ -} - -Line::~Line() -{ -} - -bool Line::operator==( const Line& l ) -{ - return this->start == l.start && this->end == l.end; -} - -void Line::Set( const MapPoint &start, const MapPoint &end ) -{ - this->start = start; - this->end = end; - - this->Calc(); -} - -void Line::Start( const MapPoint &start ) -{ - this->start = start; - this->Calc(); -} - -void Line::End( const MapPoint &end ) -{ - this->end = end; - this->Calc(); -} - -const MapPoint& Line::Start() const -{ - return this->start; -} - -const MapPoint& Line::End() const -{ - return this->end; -} - -const MapPoint& Line::Mid() const -{ - return this->mid; -} - -float Line::Length() const -{ - return this->length; -} - -float Line::Angle() const -{ - return this->angle; -} - -void Line::Calc() -{ - - // The length of the line segment - this->length = sqrt( pow( this->start.x - this->end.x,2 ) + - pow(this->start.y - this->end.y,2 ) ); - - // The angle of the line segement - if (this->start.x == this->end.x) - this->angle = M_PI/2.0; - else - this->angle = atan2( (this->start.y - this->end.y), - (this->start.x - this->end.x) ); - - // Calc. the center x location - if (this->start.x - this->end.x <= 0) - { - this->mid.x = this->start.x + fabs( this->start.x - this->end.x ) / 2.0; - } else { - this->mid.x = this->start.x - fabs(this->start.x - this->end.x ) / 2.0; - } - - // Calc. the center y location - if (this->start.y - this->end.y <= 0) - { - this->mid.y = this->start.y + fabs( this->start.y - this->end.y ) / 2.0; - } else { - this->mid.y = this->start.y - fabs( this->start.y - this->end.y ) / 2.0; - } -} Deleted: code/gazebo/trunk/server/physics/OccupancyGridGeom.hh =================================================================== --- code/gazebo/trunk/server/physics/OccupancyGridGeom.hh 2008-07-17 01:31:38 UTC (rev 6878) +++ code/gazebo/trunk/server/physics/OccupancyGridGeom.hh 2008-07-17 01:42:48 UTC (rev 6879) @@ -1,203 +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:$ - */ - -#ifndef OCCUPANCYGRIDGEOM_HH -#define OCCUPANCYGRIDGEOM_HH - -#include <Ogre.h> - -#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 Line; - class MapPoint; - - /// \brief Occupancy grid geom - class OccupancyGridGeom : public Geom - { - /// \brief Constructor - public: OccupancyGridGeom(Body *body); - - /// \brief Destructor - public: virtual ~OccupancyGridGeom(); - - /// \brief Update function - public: void UpdateChild(); - - /// \brief Load the heightmap - protected: virtual void LoadChild(XMLConfigNode *node); - - /// \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: int wallCount; - private: int wallMaxCount; - - // 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 width( thickness ) of the walls in meters - private: float wallWidth; - - // The height of the walls in meters - private: float wallHeight; - - // The color of the walls - private: Vector3 color; - - // The amount of acceptable error in the model - private: float errBound; - - // The map dimensions - private: unsigned int mapWidth; - private: unsigned int mapHeight; - - private: Ogre::Image mapImage; - - private: SpaceTree *tree; - }; - - class MapPoint - { - public: MapPoint() : x(0), y(0), arcCount(0) - {for (int i=0;i<8;i++) this->arcs[i] = NULL;} - - public: MapPoint( float x,float y ) : x(x), y(y), arcCount(0) - {for (int i=0; i<8; i++) this->arcs[i] = NULL;} - - public: MapPoint( const MapPoint &o) : x(o.x),y(o.y), arcCount(o.arcCount) - { for (int i=0; i<8; i++) this->arcs[i] = o.arcs[i]; } - - public: bool operator==( const MapPoint &p ) const - { return p.x == this->x && p.y == this->y; } - - public: MapPoint &operator= ( const MapPoint &p ) - {this->x = p.x; this->y = p.y; this->arcCount = p.arcCount; - for (int i=0; i<8; i++) this->arcs[i] = p.arcs[i]; return *this;} - - public: float x; - public: float y; - - public: MapPoint *arcs[8]; - public: unsigned short arcCount; - }; - - class Line - { - public: Line(); - public: virtual ~Line(); - - public: bool operator==(const Line& l); - - public: void Set( const MapPoint &start, const MapPoint &end ); - public: void Start( const MapPoint &start ); - public: void End( const MapPoint &end ); - - public: const MapPoint& Start() const; - public: const MapPoint& End() const; - public: const MapPoint& Mid() const; - public: float Length() const; - public: float Angle() const; - - private: void Calc(); - - private: MapPoint start; - private: MapPoint end; - private: MapPoint mid; - private: float length; - private: float angle; - }; - - - /// \} -} - -#endif Deleted: code/gazebo/trunk/server/physics/QuadTreeGeom.cc =================================================================== --- code/gazebo/trunk/server/physics/QuadTreeGeom.cc 2008-07-17 01:31:38 UTC (rev 6878) +++ code/gazebo/trunk/server/physics/QuadTreeGeom.cc 2008-07-17 01:42:48 UTC (rev 6879) @@ -1,371 +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: 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 "BoxGeom.hh" -#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->scale = node->GetDouble("scale",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->merged = true; - while (this->merged) - { - this->merged =false; - this->ReduceTree(this->root); - } - - 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::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); - } -} - -void QuadTreeGeom::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)); - } - } - } -} - - -void QuadTreeGeom::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 ( diff > 10) - { - 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++; - } - } -} - Deleted: code/gazebo/trunk/server/physics/QuadTreeGeom.hh =================================================================== --- code/gazebo/trunk/server/physics/QuadTreeGeom.hh 2008-07-17 01:31:38 UTC (rev 6878) +++ code/gazebo/trunk/server/physics/QuadTreeGeom.hh 2008-07-17 01:42:48 UTC (rev 6879) @@ -1,179 +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:$ - */ - -#ifndef QUADTREEGEOM_HH -#define QUADTREEGEOM_HH - -#include <Ogre.h> -#include <deque> - -#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: void CreateBoxes(QuadNode *node); - - 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::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: int leaves; - - public: bool valid; - }; - - /// \} -} - -#endif Modified: code/gazebo/trunk/server/physics/SConscript =================================================================== --- code/gazebo/trunk/server/physics/SConscript 2008-07-17 01:31:38 UTC (rev 6878) +++ code/gazebo/trunk/server/physics/SConscript 2008-07-17 01:42:48 UTC (rev 6879) @@ -23,9 +23,7 @@ 'RayGeom.cc', 'TrimeshGeom.cc', 'HeightmapGeom.cc', - 'OccupancyGridGeom.cc', - 'SpaceTree.cc', - 'QuadTreeGeom.cc' + 'MapGeom.cc' ] headers.append( @@ -46,8 +44,7 @@ 'server/physics/SphereGeom.hh', 'server/physics/TrimeshGeom.hh', 'server/physics/UniversalJoint.hh', - 'server/physics/OccupancyGridGeom.hh', - 'server/physics/SpaceTree.hh' + 'server/physics/MapGeom.hh' ] ) staticObjs.append( env.StaticObject(sources) ) Deleted: code/gazebo/trunk/server/physics/SpaceTree.cc =================================================================== --- code/gazebo/trunk/server/physics/SpaceTree.cc 2008-07-17 01:31:38 UTC (rev 6878) +++ code/gazebo/trunk/server/physics/SpaceTree.cc 2008-07-17 01:42:48 UTC (rev 6879) @@ -1,316 +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: 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; -} - - -////////////////... [truncated message content] |
From: <na...@us...> - 2008-07-20 22:27:27
|
Revision: 6891 http://playerstage.svn.sourceforge.net/playerstage/?rev=6891&view=rev Author: natepak Date: 2008-07-21 05:27:12 +0000 (Mon, 21 Jul 2008) Log Message: ----------- Fixed seg fault when closing. Removed stringvalue class. Stereocamera controller now outputs two images. Modified Paths: -------------- code/gazebo/trunk/libgazebo/Iface.cc code/gazebo/trunk/libgazebo/gazebo.h code/gazebo/trunk/server/GazeboMessage.cc code/gazebo/trunk/server/Model.cc code/gazebo/trunk/server/SConscript code/gazebo/trunk/server/Simulator.cc code/gazebo/trunk/server/XMLConfig.cc code/gazebo/trunk/server/XMLConfig.hh code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.hh code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.cc code/gazebo/trunk/server/gui/GLWindow.cc code/gazebo/trunk/server/gui/Gui.cc code/gazebo/trunk/server/gui/Gui.hh code/gazebo/trunk/server/main.cc code/gazebo/trunk/server/physics/Body.cc code/gazebo/trunk/server/physics/Geom.cc code/gazebo/trunk/server/physics/MapGeom.cc code/gazebo/trunk/server/physics/ode/ODEPhysics.cc code/gazebo/trunk/server/rendering/OgreAdaptor.cc code/gazebo/trunk/server/rendering/OgreAdaptor.hh code/gazebo/trunk/server/rendering/OgreCreator.cc code/gazebo/trunk/server/rendering/OgreVisual.cc code/gazebo/trunk/worlds/models/bandit.model Removed Paths: ------------- code/gazebo/trunk/server/StringValue.cc code/gazebo/trunk/server/StringValue.hh Modified: code/gazebo/trunk/libgazebo/Iface.cc =================================================================== --- code/gazebo/trunk/libgazebo/Iface.cc 2008-07-18 23:57:33 UTC (rev 6890) +++ code/gazebo/trunk/libgazebo/Iface.cc 2008-07-21 05:27:12 UTC (rev 6891) @@ -113,22 +113,24 @@ this->server = server; + this->id = id; + // Went cant have null id's - if (id.empty()) + if (this->id.empty()) { stream << "interface [" << this->type << "] id is NULL"; throw(stream.str()); } // We cannot have id with '.' - if (strchr(id.c_str(), '.')) + if (strchr(this->id.c_str(), '.')) { - stream << "invalid id [" << id << "] (must not contain '.')"; + stream << "invalid id [" << this->id << "] (must not contain '.')"; throw(stream.str()); } // Work out the filename - this->Filename(id); + this->Filename(this->id); // Create and open the file this->mmapFd = open(this->filename.c_str(), O_RDWR | O_CREAT | O_TRUNC, S_IREAD | S_IWRITE); @@ -364,6 +366,13 @@ } ////////////////////////////////////////////////////////////////////////////// +/// Get the ID of the inteface +std::string Iface::GetId() const +{ + return this->id; +} + +////////////////////////////////////////////////////////////////////////////// /// Get the number of connections int Iface::GetOpenCount() { Modified: code/gazebo/trunk/libgazebo/gazebo.h =================================================================== --- code/gazebo/trunk/libgazebo/gazebo.h 2008-07-18 23:57:33 UTC (rev 6890) +++ code/gazebo/trunk/libgazebo/gazebo.h 2008-07-21 05:27:12 UTC (rev 6891) @@ -297,6 +297,9 @@ /// \return The type of interface public: std::string GetType() const; + /// \brief Get the ID of the inteface + public: std::string GetId() const; + private: std::string Filename(std::string id); /// The server we are associated with @@ -317,6 +320,8 @@ /// type of interface protected: std::string type; + protected: std::string id; + private: bool creator; private: size_t size; Modified: code/gazebo/trunk/server/GazeboMessage.cc =================================================================== --- code/gazebo/trunk/server/GazeboMessage.cc 2008-07-18 23:57:33 UTC (rev 6890) +++ code/gazebo/trunk/server/GazeboMessage.cc 2008-07-21 05:27:12 UTC (rev 6891) @@ -97,18 +97,17 @@ void GazeboMessage::Save(XMLConfigNode *node) { - node->SetValue("verbosity", this->level); + /*node->SetValue("verbosity", this->level); node->SetValue("logData", this->logData); - /* - node->NewElement("verbosity", String(this->level)); //std::ostringstream << this->level); + //node->NewElement("verbosity", String(this->level)); //std::ostringstream << this->level); - node->NewElement("logData", gazebo::String(this->logData)); + //node->NewElement("logData", gazebo::String(this->logData)); - if (this->logData) - node->NewElement("logData", std::ostringstream << "true"); - else - node->NewElement("logData", std::ostringstream << "true"); + //if (this->logData) + // node->NewElement("logData", std::ostringstream << "true"); + //else + // node->NewElement("logData", std::ostringstream << "true"); */ } Modified: code/gazebo/trunk/server/Model.cc =================================================================== --- code/gazebo/trunk/server/Model.cc 2008-07-18 23:57:33 UTC (rev 6890) +++ code/gazebo/trunk/server/Model.cc 2008-07-21 05:27:12 UTC (rev 6891) @@ -181,7 +181,7 @@ void Model::Save() { - std::map<std::string, Body* >::iterator bodyIter; + /*std::map<std::string, Body* >::iterator bodyIter; std::map<std::string, Controller* >::iterator contIter; std::map<std::string, Joint* >::iterator jointIter; @@ -225,6 +225,7 @@ contIter->second->Save(); } + */ } //////////////////////////////////////////////////////////////////////////////// Modified: code/gazebo/trunk/server/SConscript =================================================================== --- code/gazebo/trunk/server/SConscript 2008-07-18 23:57:33 UTC (rev 6890) +++ code/gazebo/trunk/server/SConscript 2008-07-21 05:27:12 UTC (rev 6891) @@ -19,7 +19,6 @@ 'GazeboMessage.cc', 'Model.cc', 'Simulator.cc', - 'StringValue.cc' ] headers.append( @@ -33,7 +32,6 @@ '#/server/Simulator.hh', '#/server/SingletonT.hh', '#/server/StaticPluginRegister.hh', - '#/server/StringValue.hh', '#/server/Time.hh', '#/server/Vector2.hh', '#/server/Vector3.hh', Modified: code/gazebo/trunk/server/Simulator.cc =================================================================== --- code/gazebo/trunk/server/Simulator.cc 2008-07-18 23:57:33 UTC (rev 6890) +++ code/gazebo/trunk/server/Simulator.cc 2008-07-21 05:27:12 UTC (rev 6891) @@ -419,16 +419,16 @@ void Simulator::SaveGui(XMLConfigNode *node) { - Vector2<int> size; + /*Vector2<int> size; XMLConfigNode* childNode = node->GetChild("gui"); if (childNode && this->gui) { size.x = this->gui->GetWidth(); size.y = this->gui->GetHeight(); - childNode->SetValue("size", size); + //childNode->SetValue("size", size); //TODO: node->SetValue("pos", Vector2<int>(x,y)); - } + }*/ } Deleted: code/gazebo/trunk/server/StringValue.cc =================================================================== --- code/gazebo/trunk/server/StringValue.cc 2008-07-18 23:57:33 UTC (rev 6890) +++ code/gazebo/trunk/server/StringValue.cc 2008-07-21 05:27:12 UTC (rev 6891) @@ -1,150 +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: A helpfull StringValue class, mostly a converter - * Author: Jordi Polo - * Date: 3 Jan 2008 - * SVN: $Id:$ - */ - -#include "Vector3.hh" -#include "Vector2.hh" -#include "Quatern.hh" -#include <OgreColourValue.h> - -#include "StringValue.hh" - -using namespace gazebo; - -//////////////////////////////////////////////////////////////////////////////// -/// Copy Constructor -StringValue::StringValue(const StringValue& data) -{ - stream << data.GetStr(); -} - -//////////////////////////////////////////////////////////////////////////////// -/// Convert Vector3 to string -StringValue::StringValue(Vector3 data) -{ - stream << data; -} - -//////////////////////////////////////////////////////////////////////////////// -/// Convert Vector2<int> to string -StringValue::StringValue(Vector2<int> data) -{ - stream << data; -} - -//////////////////////////////////////////////////////////////////////////////// -/// Convert Vector2<double> to string -StringValue::StringValue(Vector2<double> data) -{ - stream << data; -} - -//////////////////////////////////////////////////////////////////////////////// -/// Convert Quatern to string -StringValue::StringValue(Quatern data) -{ - stream << data; -} - -//////////////////////////////////////////////////////////////////////////////// -/// Convert bool to string -StringValue::StringValue(bool data) -{ - if (data) - stream << "true"; - else - stream << "false"; -} - - -//////////////////////////////////////////////////////////////////////////////// -/// Convert int to string -StringValue::StringValue(int data) -{ - stream << data; -} - -//////////////////////////////////////////////////////////////////////////////// -/// Convert double to string -StringValue::StringValue(double data) -{ - stream << data; -} - -//////////////////////////////////////////////////////////////////////////////// -/// Convert float to string -StringValue::StringValue(float data) -{ - stream << data; -} - -//////////////////////////////////////////////////////////////////////////////// -/// Convert float to string -StringValue::StringValue(const char* data) -{ - stream << data; -} - -//////////////////////////////////////////////////////////////////////////////// -/// Ogre colour value to string -StringValue::StringValue(std::string data) -{ - stream << data; -} - -//////////////////////////////////////////////////////////////////////////////// -/// A std string -StringValue::StringValue(Ogre::ColourValue* data) -{ - stream << data->r << " " <<data->g <<" " << data->b << " " << data->a; -} - -//////////////////////////////////////////////////////////////////////////////// -/// destructor -StringValue::~StringValue() -{} - -//////////////////////////////////////////////////////////////////////////////// -/// True if the string is null -bool StringValue::IsNull() const -{ - bool null=false; - if (stream.str()==std::string()) - null=true; - return null; -} - -//////////////////////////////////////////////////////////////////////////////// -/// Get the value as a std string -std::string StringValue::GetStr() const -{ - return stream.str(); -} - -//////////////////////////////////////////////////////////////////////////////// -/// Get the value as a char string -const char* StringValue::GetCharStr() const -{ - return stream.str().c_str(); -} Deleted: code/gazebo/trunk/server/StringValue.hh =================================================================== --- code/gazebo/trunk/server/StringValue.hh 2008-07-18 23:57:33 UTC (rev 6890) +++ code/gazebo/trunk/server/StringValue.hh 2008-07-21 05:27:12 UTC (rev 6891) @@ -1,106 +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: A helpfull StringValue class, mostly a converter - * Author: Jordi Polo - * Date: 3 Jan 2008 - * SVN: $Id:$ - */ - -#ifndef STRING_HH -#define STRING_HH - -#include <iostream> -#include <sstream> -#include <string> - -#include "Vector3.hh" -#include "Vector2.hh" -#include "Quatern.hh" - -namespace Ogre -{ - class ColourValue; -} - -namespace gazebo -{ -/// \addtogroup gazebo_server -/// \brief StringValue conversions and tools -/// \{ - - class StringValue - { - /// \brief Copy Constructor - public: StringValue(const StringValue& data); - - /// \brief Convert Vector3 to string - public: StringValue(Vector3 data); - - /// \brief Convert Vector2<int> to string - public: StringValue(Vector2<int> data); - - /// \brief Convert Vector2<double> to string - public: StringValue(Vector2<double> data); - - /// \brief Convert Quatern to string - public: StringValue(Quatern data); - - /// \brief Convert bool to string - public: StringValue(bool data); - - /// \brief Convert int to string - public: StringValue(int data); - - /// \brief Convert double to string - public: StringValue(double data); - - /// \brief Convert float to string - public: StringValue(float data); - - /// \brief Convert float to string - public: StringValue(const char* data); - - /// \brief Ogre colour value to string - public: StringValue(Ogre::ColourValue* data); - - /// \brief A std string - public: StringValue(std::string data); - - /// \brief destructor - public: ~StringValue(); - - /// \brief True if the string is null - public: bool IsNull() const; - - /// \brief Get the value as a std string - public: std::string GetStr() const; - - /// \brief Get the value as a char string - public: const char* GetCharStr() const; - - //private: std::string str; - private: std::ostringstream stream; - }; - - -/// \} -} - -#endif Modified: code/gazebo/trunk/server/XMLConfig.cc =================================================================== --- code/gazebo/trunk/server/XMLConfig.cc 2008-07-18 23:57:33 UTC (rev 6890) +++ code/gazebo/trunk/server/XMLConfig.cc 2008-07-21 05:27:12 UTC (rev 6891) @@ -836,7 +836,7 @@ //////////////////////////////////////////////////////////////////////////// // Set the value associated with a node. -void XMLConfigNode::SetValue(const std::string &key, const StringValue &data, int require, int type) +/*void XMLConfigNode::SetValue(const std::string &key, const StringValue &data, int require, int type) { bool success; @@ -847,7 +847,7 @@ this->NewNode(key.c_str(), data.GetCharStr(), type); } -} +}*/ //////////////////////////////////////////////////////////////////////////// Modified: code/gazebo/trunk/server/XMLConfig.hh =================================================================== --- code/gazebo/trunk/server/XMLConfig.hh 2008-07-18 23:57:33 UTC (rev 6890) +++ code/gazebo/trunk/server/XMLConfig.hh 2008-07-21 05:27:12 UTC (rev 6891) @@ -35,7 +35,6 @@ #include "Vector2.hh" #include "Quatern.hh" #include "Time.hh" -#include "StringValue.hh" namespace gazebo { @@ -195,7 +194,7 @@ /// \param value : the name of the element or attribute to write /// \param require : Require=1 means that if not found a new node will be created /// \param type : Only if a new node is created, the type must be specified - public: void SetValue(const std::string &key, const StringValue &data, int require =0, int type=0); + //public: void SetValue(const std::string &key, const StringValue &data, int require =0, int type=0); /// \brief Get a node's value, which is either a attribute or child node value. protected: xmlChar* GetNodeValue( const std::string &key ) const; Modified: code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc =================================================================== --- code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc 2008-07-18 23:57:33 UTC (rev 6890) +++ code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc 2008-07-21 05:27:12 UTC (rev 6891) @@ -70,9 +70,15 @@ if ((*iter)->GetType() == "stereo") this->stereoIface = dynamic_cast<StereoCameraIface*>(*iter); else if ((*iter)->GetType() == "camera") - this->cameraIface = dynamic_cast<CameraIface*>(*iter); + { + CameraIface *ciface = dynamic_cast<CameraIface*>(*iter); + this->cameraIfaces[ciface->GetId()] = ciface; + } } + this->leftCameraName = node->GetString("leftcamera","", 1); + this->rightCameraName = node->GetString("rightcamera","", 1); + if (!this->stereoIface) gzthrow("Stereo_Camera controller requires a StereoCameraIface"); } @@ -87,15 +93,23 @@ // Update the controller void Stereo_Camera::UpdateChild() { - if (this->cameraIface) + std::map< std::string, CameraIface*>::iterator iter; + + for (iter = this->cameraIfaces.begin(); + iter != this->cameraIfaces.end(); iter++) { - this->cameraIface->Lock(1); - if (this->cameraIface->data->head.openCount > 0) - this->PutCameraData(); - this->cameraIface->Unlock(); + iter->second->Lock(1); - // New data is available - this->cameraIface->Post(); + if (iter->second->data->head.openCount > 0) + { + if (this->leftCameraName == iter->first) + this->PutCameraData( iter->second->data, 0 ); + else + this->PutCameraData( iter->second->data, 1 ); + } + + iter->second->Unlock(); + iter->second->Post(); } if (this->stereoIface) @@ -175,9 +189,9 @@ //////////////////////////////////////////////////////////////////////////////// // Put camera data to the interface -void Stereo_Camera::PutCameraData() +void Stereo_Camera::PutCameraData(CameraData *camera_data, unsigned int camera) { - CameraData *camera_data = this->cameraIface->data; + //CameraData *camera_data = this->cameraIface->data; const unsigned char *rgb_src = NULL; unsigned char *rgb_dst = NULL; Pose3d cameraPose; @@ -202,10 +216,9 @@ camera_data->camera_pose.yaw = cameraPose.rot.GetYaw(); // Copy the pixel data to the interface - rgb_src = this->myParent->GetImageData(0); + rgb_src = this->myParent->GetImageData(camera); rgb_dst = camera_data->image; memcpy(rgb_dst, rgb_src, camera_data->image_size); - } Modified: code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.hh =================================================================== --- code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.hh 2008-07-18 23:57:33 UTC (rev 6890) +++ code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.hh 2008-07-21 05:27:12 UTC (rev 6891) @@ -28,6 +28,8 @@ #ifndef STEREO_CAMERA_HH #define STEREO_CAMERA_HH +#include <map> + #include "Controller.hh" namespace gazebo @@ -93,12 +95,15 @@ private: void PutStereoData(); /// \brief Put camera data to the iface - private: void PutCameraData(); + private: void PutCameraData(CameraData *camera_data, unsigned int camera); /// The camera interface private: StereoCameraIface *stereoIface; - private: CameraIface *cameraIface; + private: std::map< std::string, CameraIface*> cameraIfaces; + private: std::string leftCameraName; + private: std::string rightCameraName; + /// The parent sensor private: StereoCameraSensor *myParent; Modified: code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.cc =================================================================== --- code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.cc 2008-07-18 23:57:33 UTC (rev 6890) +++ code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.cc 2008-07-21 05:27:12 UTC (rev 6891) @@ -113,11 +113,12 @@ // Load the controller void Differential_Position2d::SaveChild(XMLConfigNode *node) { - node->SetValue("wheelSeparation",this->wheelSep); + /*node->SetValue("wheelSeparation",this->wheelSep); node->SetValue("wheelDiameter",this->wheelDiam); node->SetValue("torque",this->torque); // node->SetValue("leftJoint",this->XMLData["leftJointName"]); // node->SetValue("rightJoint",this->XMLData["rightJointName"]); +*/ } Modified: code/gazebo/trunk/server/gui/GLWindow.cc =================================================================== --- code/gazebo/trunk/server/gui/GLWindow.cc 2008-07-18 23:57:33 UTC (rev 6890) +++ code/gazebo/trunk/server/gui/GLWindow.cc 2008-07-21 05:27:12 UTC (rev 6891) @@ -122,17 +122,6 @@ // Update function void GLWindow::Update() { - /*if (this->userCamera && this->userCamera->GetUserMovable()) - { - this->userCamera->Translate( - this->directionVec * (Simulator::Instance()->GetRealTime() - this->lastUpdateTime) ); - this->directionVec.Set(0,0,0); - } - - this->lastUpdateTime = Simulator::Instance()->GetRealTime(); - - this->userCamera->Update(); - */ if (this->activeCamera && this->activeCamera->GetUserMovable()) { this->activeCamera->Translate( @@ -216,17 +205,6 @@ /// Handle a mouse drag void GLWindow::HandleMouseDrag() { - /*if (this->userCamera && this->userCamera->GetUserMovable()) - { - if (this->leftMousePressed) - { - Vector2<int> d = this->mousePos - this->prevMousePos; - this->userCamera->RotateYaw(DTOR(-d.x * this->rotateAmount)); - this->userCamera->RotatePitch(DTOR(d.y * this->rotateAmount)); - } - } - */ - if (this->activeCamera && this->activeCamera->GetUserMovable()) { if (this->leftMousePressed) Modified: code/gazebo/trunk/server/gui/Gui.cc =================================================================== --- code/gazebo/trunk/server/gui/Gui.cc 2008-07-18 23:57:33 UTC (rev 6890) +++ code/gazebo/trunk/server/gui/Gui.cc 2008-07-21 05:27:12 UTC (rev 6891) @@ -79,6 +79,8 @@ // Create a dummy rendering window. This creates a context, and allows Ogre // to initialize properly OgreCreator::CreateWindow(this, 1, 1); + + this->hasFocus = true; } //////////////////////////////////////////////////////////////////////////////// @@ -133,12 +135,18 @@ /// Handle an event int Gui::handle(int event) { - - switch (event) + switch(event) { + case FL_FOCUS: + this->hasFocus = true; + break; + case FL_UNFOCUS: + this->hasFocus = false; + break; case FL_HIDE: - Simulator::Instance()->SetUserQuit(); - return 1; + if (this->hasFocus) + Simulator::Instance()->SetUserQuit(); + break; } return Fl_Window::handle(event); Modified: code/gazebo/trunk/server/gui/Gui.hh =================================================================== --- code/gazebo/trunk/server/gui/Gui.hh 2008-07-18 23:57:33 UTC (rev 6890) +++ code/gazebo/trunk/server/gui/Gui.hh 2008-07-21 05:27:12 UTC (rev 6891) @@ -76,6 +76,8 @@ private: Toolbar *toolbar; private: StatusBar *statusbar; private: GLFrameManager *frameMgr; + + private: bool hasFocus; }; } Modified: code/gazebo/trunk/server/main.cc =================================================================== --- code/gazebo/trunk/server/main.cc 2008-07-18 23:57:33 UTC (rev 6890) +++ code/gazebo/trunk/server/main.cc 2008-07-21 05:27:12 UTC (rev 6891) @@ -311,5 +311,6 @@ return -1; } + printf("Done.\n"); return 0; } Modified: code/gazebo/trunk/server/physics/Body.cc =================================================================== --- code/gazebo/trunk/server/physics/Body.cc 2008-07-18 23:57:33 UTC (rev 6890) +++ code/gazebo/trunk/server/physics/Body.cc 2008-07-21 05:27:12 UTC (rev 6891) @@ -127,7 +127,7 @@ // Save the body based on our XMLConfig node void Body::Save() { - std::vector< Geom* >::iterator giter; + /*std::vector< Geom* >::iterator giter; std::vector< Sensor* >::iterator siter; this->xmlNode->SetValue("name", this->GetName()); @@ -143,7 +143,7 @@ { //(*siter)->Save(); } - + */ } Modified: code/gazebo/trunk/server/physics/Geom.cc =================================================================== --- code/gazebo/trunk/server/physics/Geom.cc 2008-07-18 23:57:33 UTC (rev 6890) +++ code/gazebo/trunk/server/physics/Geom.cc 2008-07-21 05:27:12 UTC (rev 6891) @@ -128,10 +128,10 @@ childNode = childNode->GetNext("visual"); } - if (this->IsStatic()) + /*if (this->IsStatic()) { this->visualNode->MakeStatic(); - } + }*/ // Create the bounding box if (this->geomId && dGeomGetClass(this->geomId) != dPlaneClass) @@ -161,7 +161,7 @@ // Save the body based on our XMLConfig node void Geom::Save() { - std::vector<OgreVisual*>::iterator iter; + /*std::vector<OgreVisual*>::iterator iter; Pose3d pose= this->GetPose(); this->xmlNode->SetValue("name", this->GetName()); @@ -176,8 +176,7 @@ { (*iter)->Save(); } - - + */ } //////////////////////////////////////////////////////////////////////////////// Modified: code/gazebo/trunk/server/physics/MapGeom.cc =================================================================== --- code/gazebo/trunk/server/physics/MapGeom.cc 2008-07-18 23:57:33 UTC (rev 6890) +++ code/gazebo/trunk/server/physics/MapGeom.cc 2008-07-21 05:27:12 UTC (rev 6891) @@ -35,6 +35,7 @@ #include "OgreAdaptor.hh" #include "Simulator.hh" #include "OgreAdaptor.hh" +#include "OgreVisual.hh" #include "Global.hh" #include "Body.hh" #include "MapGeom.hh" @@ -110,6 +111,8 @@ } this->CreateBoxes(this->root); + + this->visualNode->MakeStatic(); } void MapGeom::CreateBoxes(QuadNode *node) Modified: code/gazebo/trunk/server/physics/ode/ODEPhysics.cc =================================================================== --- code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2008-07-18 23:57:33 UTC (rev 6890) +++ code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2008-07-21 05:27:12 UTC (rev 6891) @@ -108,7 +108,7 @@ // Save the ODE engine void ODEPhysics::Save(XMLConfigNode *node) { - XMLConfigNode *cnode = node->GetChild("ode"); + /*XMLConfigNode *cnode = node->GetChild("ode"); if (cnode == NULL) gzthrow("No <physics:ode> node in the XML, can't write back the data"); @@ -116,6 +116,7 @@ cnode->SetValue("stepTime", this->stepTime); cnode->SetValue("cfm", this->globalCFM); cnode->SetValue("erp", this->globalERP); + */ } //////////////////////////////////////////////////////////////////////////////// Modified: code/gazebo/trunk/server/rendering/OgreAdaptor.cc =================================================================== --- code/gazebo/trunk/server/rendering/OgreAdaptor.cc 2008-07-18 23:57:33 UTC (rev 6890) +++ code/gazebo/trunk/server/rendering/OgreAdaptor.cc 2008-07-21 05:27:12 UTC (rev 6891) @@ -67,6 +67,8 @@ this->root=NULL; this->updateRate = 0; + + this->dummyDisplay = false; } //////////////////////////////////////////////////////////////////////////////// @@ -269,12 +271,15 @@ this->root->addFrameListener(this->frameListener); this->updateRate = node->GetDouble("maxUpdateRate",0,0); + + this->raySceneQuery = this->sceneMgr->createRayQuery( Ogre::Ray() ); } //////////////////////////////////////////////////////////////////////////////// // Save void OgreAdaptor::Save(XMLConfigNode *node) { + /* //Video information is not modified so we don't need to rewrite it. //Sky is not modified, not rewritten XMLConfigNode *rnode; @@ -290,7 +295,7 @@ cnode = rnode->GetChild("fog"); if (cnode) OgreCreator::SaveFog(cnode); - + */ } @@ -460,6 +465,32 @@ } //////////////////////////////////////////////////////////////////////////////// +/// Get an entity at a pixel location using a camera. Used for mouse picking. +Entity *OgreAdaptor::GetEntityAt(OgreCamera *camera, int x, int y) +{ + /* + Ogre::Vector3 camPos = camera->getPosition(); + + Ogre::Ray mouseRay = camera->getCameraToViewportRay( + x/camera->GetViewportWidth(), y/camera->GetViewportHeight()); + + this->raySceneQuery->setRay( mouseRay ); + + // Perform the scene query + Ogre::RaySceneQueryResult &result = this->raySceneQuery->execute(); + Ogre::RaySceneQueryResult::iterator iter = result.begin(); + + // Get the results, set the camera height + if (iter != result.end() && iter->worldFragment) + { + Ogre::Real terrainHeight = iter->worldFragment->singleIntersection.y; + if ((terrainHeight + 10.0f) > camPos.y) + camera->setPosition( camPos.x, terrainHeight + 10.0f, camPos.z); + } + */ +} + +//////////////////////////////////////////////////////////////////////////////// /// Get the desired update rate double OgreAdaptor::GetUpdateRate() { Modified: code/gazebo/trunk/server/rendering/OgreAdaptor.hh =================================================================== --- code/gazebo/trunk/server/rendering/OgreAdaptor.hh 2008-07-18 23:57:33 UTC (rev 6890) +++ code/gazebo/trunk/server/rendering/OgreAdaptor.hh 2008-07-21 05:27:12 UTC (rev 6891) @@ -51,6 +51,7 @@ class RenderTarget; class ColourValue; class RenderSystem; + class RaySceneQuery; } @@ -94,6 +95,10 @@ /// \brief Update a window public: void UpdateWindow(Ogre::RenderWindow *window, OgreCamera *camera); + /// \brief Get an entity at a pixel location using a camera. Used for + /// mouse picking. + public: Entity *GetEntityAt(OgreCamera *camera, int x, int y); + private: void LoadPlugins(); private: void SetupResources(); private: void SetupRenderSystem(bool create); @@ -116,6 +121,7 @@ private: std::string videoMode; + private: Ogre::RaySceneQuery *raySceneQuery; //bsp attributes saved to write XML file back private: int sceneType; @@ -145,6 +151,7 @@ /// GLX context used to render the scenes.Used for gui-less operation protected: GLXContext dummyContext; + }; Modified: code/gazebo/trunk/server/rendering/OgreCreator.cc =================================================================== --- code/gazebo/trunk/server/rendering/OgreCreator.cc 2008-07-18 23:57:33 UTC (rev 6890) +++ code/gazebo/trunk/server/rendering/OgreCreator.cc 2008-07-21 05:27:12 UTC (rev 6891) @@ -258,7 +258,7 @@ //////////////////////////////////////////////////////////////////////////////// void OgreCreator::SaveFog(XMLConfigNode *node) { - Ogre::ColourValue color=OgreAdaptor::Instance()->sceneMgr->getFogColour(); + /*Ogre::ColourValue color=OgreAdaptor::Instance()->sceneMgr->getFogColour(); Ogre::Real start = OgreAdaptor::Instance()->sceneMgr->getFogStart(); Ogre::Real end = OgreAdaptor::Instance()->sceneMgr->getFogEnd(); //Ogre::Real density = OgreAdaptor::Instance()->sceneMgr->getFogDensity(); @@ -286,6 +286,7 @@ node->SetValue("linearEnd", end); //node->SetValue("density", density); + */ } //////////////////////////////////////////////////////////////////////////////// Modified: code/gazebo/trunk/server/rendering/OgreVisual.cc =================================================================== --- code/gazebo/trunk/server/rendering/OgreVisual.cc 2008-07-18 23:57:33 UTC (rev 6890) +++ code/gazebo/trunk/server/rendering/OgreVisual.cc 2008-07-21 05:27:12 UTC (rev 6891) @@ -151,9 +151,10 @@ void OgreVisual::Save() { - this->xmlNode->SetValue("xyz", this->GetPosition()); + /*this->xmlNode->SetValue("xyz", this->GetPosition()); this->xmlNode->SetValue("rpy", this->GetRotation()); //TODO: A lot of information! + */ } Modified: code/gazebo/trunk/worlds/models/bandit.model =================================================================== --- code/gazebo/trunk/worlds/models/bandit.model 2008-07-18 23:57:33 UTC (rev 6890) +++ code/gazebo/trunk/worlds/models/bandit.model 2008-07-21 05:27:12 UTC (rev 6891) @@ -67,7 +67,11 @@ <controller:stereocamera name="stereo_camera_controller"> <interface:stereocamera name="bandit_stereo_iface" /> - <interface:camera name="bandit_stereo_camera_iface" /> + <interface:camera name="bandit_stereo_camera_left_iface" /> + <interface:camera name="bandit_stereo_camera_right_iface" /> + + <leftcamera>bandit_stereo_camera_left_iface</leftcamera> + <rightcamera>bandit_stereo_camera_right_iface</rightcamera> </controller:stereocamera> </sensor:stereocamera> </body:box> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-07-21 08:59:52
|
Revision: 6894 http://playerstage.svn.sourceforge.net/playerstage/?rev=6894&view=rev Author: natepak Date: 2008-07-21 16:00:01 +0000 (Mon, 21 Jul 2008) Log Message: ----------- Updates to stereo vision Modified Paths: -------------- code/gazebo/trunk/examples/libgazebo/stereo/stereo.cc code/gazebo/trunk/worlds/stereocamera.world Modified: code/gazebo/trunk/examples/libgazebo/stereo/stereo.cc =================================================================== --- code/gazebo/trunk/examples/libgazebo/stereo/stereo.cc 2008-07-21 06:08:31 UTC (rev 6893) +++ code/gazebo/trunk/examples/libgazebo/stereo/stereo.cc 2008-07-21 16:00:01 UTC (rev 6894) @@ -21,16 +21,24 @@ return; } - fprintf( fp, "P6\n# Gazebo\n%d %d\n255\n", stereoIface->data->width, stereoIface->data->height); + fprintf( fp, "P5\n# Gazebo\n%d %d\n32768\n", stereoIface->data->width, stereoIface->data->height); + double max = 0; + for (unsigned int i=0; i<stereoIface->data->height*stereoIface->data->width; i++) + { + double v = stereoIface->data->left_disparity[i]; + if (v > max) + max = v; + } + + printf("Max[%f]\n",max); for (unsigned int i = 0; i<stereoIface->data->height; i++) { for (unsigned int j =0; j<stereoIface->data->width; j++) { - unsigned char value = stereoIface->data->left_disparity[i*stereoIface->data->width+j] * 255; - fwrite( &value, 1, 1, fp ); - fwrite( &value, 1, 1, fp ); - fwrite( &value, 1, 1, fp ); + double v = stereoIface->data->left_disparity[i*stereoIface->data->width+j]; + unsigned int value = (unsigned int)((v/max) * 32767); + fwrite( &value, 2, 1, fp ); } } Modified: code/gazebo/trunk/worlds/stereocamera.world =================================================================== --- code/gazebo/trunk/worlds/stereocamera.world 2008-07-21 06:08:31 UTC (rev 6893) +++ code/gazebo/trunk/worlds/stereocamera.world 2008-07-21 16:00:01 UTC (rev 6894) @@ -100,7 +100,7 @@ <imageSize>320 240</imageSize> <hfov>60</hfov> <nearClip>0.1</nearClip> - <farClip>100</farClip> + <farClip>1000</farClip> <saveFrames>false</saveFrames> <saveFramePath>frames</saveFramePath> <baseline>0.2</baseline> @@ -108,6 +108,10 @@ <controller:stereocamera name="stereo_camera_controller"> <interface:stereocamera name="stereo_iface_0" /> <interface:camera name="camera_iface_0" /> + <interface:camera name="camera_iface_1" /> + + <leftcamera>camera_iface_0</leftcamera> + <rightcamera>camera_iface_1</rightcamera> </controller:stereocamera> </sensor:stereocamera> </body:empty> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-07-21 09:12:07
|
Revision: 6895 http://playerstage.svn.sourceforge.net/playerstage/?rev=6895&view=rev Author: natepak Date: 2008-07-21 16:12:15 +0000 (Mon, 21 Jul 2008) Log Message: ----------- Changed disparity to depth images in stereo code Modified Paths: -------------- code/gazebo/trunk/examples/libgazebo/stereo/stereo.cc code/gazebo/trunk/libgazebo/gazebo.h code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh Modified: code/gazebo/trunk/examples/libgazebo/stereo/stereo.cc =================================================================== --- code/gazebo/trunk/examples/libgazebo/stereo/stereo.cc 2008-07-21 16:00:01 UTC (rev 6894) +++ code/gazebo/trunk/examples/libgazebo/stereo/stereo.cc 2008-07-21 16:12:15 UTC (rev 6895) @@ -26,7 +26,7 @@ double max = 0; for (unsigned int i=0; i<stereoIface->data->height*stereoIface->data->width; i++) { - double v = stereoIface->data->left_disparity[i]; + double v = stereoIface->data->left_depth[i]; if (v > max) max = v; } @@ -36,7 +36,7 @@ { for (unsigned int j =0; j<stereoIface->data->width; j++) { - double v = stereoIface->data->left_disparity[i*stereoIface->data->width+j]; + double v = stereoIface->data->left_depth[i*stereoIface->data->width+j]; unsigned int value = (unsigned int)((v/max) * 32767); fwrite( &value, 2, 1, fp ); } Modified: code/gazebo/trunk/libgazebo/gazebo.h =================================================================== --- code/gazebo/trunk/libgazebo/gazebo.h 2008-07-21 16:00:01 UTC (rev 6894) +++ code/gazebo/trunk/libgazebo/gazebo.h 2008-07-21 16:12:15 UTC (rev 6895) @@ -1377,17 +1377,17 @@ /// Right image (R8G8B8) public: unsigned char right_rgb[GAZEBO_STEREO_CAMERA_MAX_RGB_SIZE]; - /// Left disparity size - public: unsigned int left_disparity_size; + /// Left depth map size + public: unsigned int left_depth_size; - /// Left disparity (float) - public: float left_disparity[GAZEBO_STEREO_CAMERA_MAX_DISPARITY_SIZE]; + /// Left depth map (float) + public: float left_depth[GAZEBO_STEREO_CAMERA_MAX_DISPARITY_SIZE]; - /// Right Disparity size - public: unsigned int right_disparity_size; + /// Right depth map size + public: unsigned int right_depth_size; - /// Right disparity (float) - public: float right_disparity[GAZEBO_STEREO_CAMERA_MAX_DISPARITY_SIZE]; + /// Right depth map (float) + public: float right_depth[GAZEBO_STEREO_CAMERA_MAX_DISPARITY_SIZE]; }; Modified: code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc =================================================================== --- code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc 2008-07-21 16:00:01 UTC (rev 6894) +++ code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc 2008-07-21 16:12:15 UTC (rev 6895) @@ -154,15 +154,15 @@ //stereo_data->right_rgb_size = stereo_data->width * stereo_data->height * 3; //stereo_data->left_rgb_size = stereo_data->width * stereo_data->height * 3; - stereo_data->right_disparity_size = stereo_data->width * stereo_data->height * sizeof(float); - stereo_data->left_disparity_size = stereo_data->width * stereo_data->height * sizeof(float); + stereo_data->right_depth_size = stereo_data->width * stereo_data->height * sizeof(float); + stereo_data->left_depth_size = stereo_data->width * stereo_data->height * sizeof(float); // Make sure there is room to store the image //assert (stereo_data->right_rgb_size <= sizeof(stereo_data->right_rgb)); //assert (stereo_data->left_rgb_size <= sizeof(stereo_data->left_rgb)); - assert (stereo_data->right_disparity_size <= sizeof(stereo_data->right_disparity)); - assert (stereo_data->left_disparity_size <= sizeof(stereo_data->left_disparity)); + assert (stereo_data->right_depth_size <= sizeof(stereo_data->right_depth)); + assert (stereo_data->left_depth_size <= sizeof(stereo_data->left_depth)); // Copy the left pixel data to the interface /*rgb_src = this->myParent->GetImageData(0); @@ -175,15 +175,15 @@ memcpy(rgb_dst, rgb_src, stereo_data->right_rgb_size); */ - // Copy the left disparity data to the interface - disp_src = this->myParent->GetDisparityData(0); - disp_dst = stereo_data->left_disparity; - memcpy(disp_dst, disp_src, stereo_data->left_disparity_size); + // Copy the left depth data to the interface + disp_src = this->myParent->GetDepthData(0); + disp_dst = stereo_data->left_depth; + memcpy(disp_dst, disp_src, stereo_data->left_depth_size); - // Copy the right disparity data to the interface - disp_src = this->myParent->GetDisparityData(1); - disp_dst = stereo_data->right_disparity; - memcpy(disp_dst, disp_src, stereo_data->right_disparity_size); + // Copy the right depth data to the interface + disp_src = this->myParent->GetDepthData(1); + disp_dst = stereo_data->right_depth; + memcpy(disp_dst, disp_src, stereo_data->right_depth_size); } Modified: code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc =================================================================== --- code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc 2008-07-21 16:00:01 UTC (rev 6894) +++ code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc 2008-07-21 16:12:15 UTC (rev 6895) @@ -291,23 +291,23 @@ { if (i > 1) - gzthrow("Index must be 0 for left, or 1 for right disparity image\n"); + gzthrow("Index must be 0 for left, or 1 for right depth image\n"); return this->rgbBuffer[i]; } ////////////////////////////////////////////////////////////////////////////// -// Get a pointer to the disparity data -const float *StereoCameraSensor::GetDisparityData(unsigned int i) +// Get a pointer to the depth data +const float *StereoCameraSensor::GetDepthData(unsigned int i) { if (i > 1) - gzthrow("Index must be 0 for left, or 1 for right disparity image\n"); + gzthrow("Index must be 0 for left, or 1 for right depth image\n"); return this->depthBuffer[i]; } ////////////////////////////////////////////////////////////////////////////// -// Fill all RGB and Disparity buffers +// Fill all RGB and depth buffers void StereoCameraSensor::FillBuffers() { Ogre::HardwarePixelBufferSharedPtr hardwareBuffer; Modified: code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh =================================================================== --- code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh 2008-07-21 16:00:01 UTC (rev 6894) +++ code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh 2008-07-21 16:12:15 UTC (rev 6895) @@ -86,9 +86,9 @@ /// \param i 0=left, 1=right public: virtual const unsigned char *GetImageData(unsigned int i=0); - /// \brief Get a point to the disparity data + /// \brief Get a point to the depth data /// \param i 0=left, 1=right - public: const float *GetDisparityData(unsigned int i=0); + public: const float *GetDepthData(unsigned int i=0); /// \brief Get the baselien of the camera public: double GetBaseline() const; This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-07-21 11:16:42
|
Revision: 6896 http://playerstage.svn.sourceforge.net/playerstage/?rev=6896&view=rev Author: natepak Date: 2008-07-21 18:16:50 +0000 (Mon, 21 Jul 2008) Log Message: ----------- Add simulation interface request/response queue Modified Paths: -------------- code/gazebo/trunk/examples/libgazebo/simiface/simiface.cc code/gazebo/trunk/libgazebo/gazebo.h code/gazebo/trunk/player/SimulationInterface.cc code/gazebo/trunk/player/SimulationInterface.hh code/gazebo/trunk/server/World.cc Modified: code/gazebo/trunk/examples/libgazebo/simiface/simiface.cc =================================================================== --- code/gazebo/trunk/examples/libgazebo/simiface/simiface.cc 2008-07-21 16:12:15 UTC (rev 6895) +++ code/gazebo/trunk/examples/libgazebo/simiface/simiface.cc 2008-07-21 18:16:50 UTC (rev 6896) @@ -34,19 +34,19 @@ // Example of how to move a model (box1_model) uint8_t name[512] = "pioneer2dx_model1"; - uint8_t cmd[32] = "set_pose3d"; for (int i=0; i< 10; i++) { simIface->Lock(1); - memcpy(simIface->data->model_name, name, 512); - memcpy(simIface->data->model_req,cmd, 32); + gazebo::SimulationRequestData *request = &(simIface->data->requests[simIface->data->requestCount++]); - simIface->data->model_pose.pos.x = i+.1; - simIface->data->model_pose.pos.y = 0; - simIface->data->model_pose.pos.z = 0.2; + request->type = gazebo::SimulationRequestData::SET_POSE2D; + memcpy(request->modelName, name, 512); + request->modelPose.pos.x = i+.1; + request->modelPose.pos.y = 0; + simIface->Unlock(); usleep(100000); Modified: code/gazebo/trunk/libgazebo/gazebo.h =================================================================== --- code/gazebo/trunk/libgazebo/gazebo.h 2008-07-21 16:12:15 UTC (rev 6895) +++ code/gazebo/trunk/libgazebo/gazebo.h 2008-07-21 18:16:50 UTC (rev 6896) @@ -366,7 +366,25 @@ \{ */ +#define GAZEBO_SIMULATION_MAX_REQUESTS 128 +class SimulationRequestData +{ + public: enum Type { PAUSE, + RESET, + SAVE, + GET_POSE3D, + GET_POSE2D, + SET_POSE3D, + SET_POSE2D, + }; + + public: Type type; + public: char modelName[512]; + public: Pose modelPose; + +}; + /// \brief Simulation interface data class SimulationData { @@ -383,29 +401,14 @@ /// state of the simulation : 0 paused, 1 running -1 not_started/exiting public: int state; - - /// Pause simulation (set by client) should check the state - /// Changes the state of the simulation from pause to play and viceversa. - public: bool pause; - /// Reset simulation (set by client) - public: int reset; + /// Array of requests to the simulator + public: SimulationRequestData requests[GAZEBO_SIMULATION_MAX_REQUESTS]; + public: unsigned int requestCount; - /// Save current poses to world file (set by client) - public: int save; - - /// Name of the model to get/set data - public: char model_name[512]; - - /// Type of request - /// - "get_pose" Sets model_pose to the pose of model_name - /// - "set_pose3d" Set the model_name to model_pose - /// - "set_pose2d" Set the model_name to model_pose - public: char model_req[32]; - - /// Pose of the model. - /// \sa model_req - public: Pose model_pose; + /// Array of request results from the simulator + public: SimulationRequestData results[GAZEBO_SIMULATION_MAX_REQUESTS]; + public: unsigned int resultCount; }; /// \brief Common simulation interface Modified: code/gazebo/trunk/player/SimulationInterface.cc =================================================================== --- code/gazebo/trunk/player/SimulationInterface.cc 2008-07-21 16:12:15 UTC (rev 6895) +++ code/gazebo/trunk/player/SimulationInterface.cc 2008-07-21 18:16:50 UTC (rev 6896) @@ -65,130 +65,112 @@ int SimulationInterface::ProcessMessage(QueuePointer &respQueue, player_msghdr_t *hdr, void *data) { + if (this->responseQueue) + delete this->responseQueue; - timespec sleepTime = {0, 10000000}; + this->responseQueue = new QueuePointer(respQueue); + /// Set a 3D pose if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_SIMULATION_REQ_SET_POSE3D, this->device_addr)) { + gazebo::SimulationRequestData *gzReq = NULL; player_simulation_pose3d_req_t *req = (player_simulation_pose3d_req_t*)(data); this->iface->Lock(1); - strcpy((char*)this->iface->data->model_name,req->name); - strcpy((char*)this->iface->data->model_req,"set_pose3d"); + gzReq = &(this->iface->data->requests[ this->iface->data->requestCount++ ]); - this->iface->data->model_pose.pos.x = req->pose.px; - this->iface->data->model_pose.pos.y = req->pose.py; - this->iface->data->model_pose.pos.z = req->pose.pz; + gzReq->type = gazebo::SimulationRequestData::SET_POSE3D; + strcpy((char*)gzReq->modelName, req->name); - this->iface->data->model_pose.roll = req->pose.proll; - this->iface->data->model_pose.pitch = req->pose.ppitch; - this->iface->data->model_pose.yaw = req->pose.pyaw; + gzReq->modelPose.pos.x = req->pose.px; + gzReq->modelPose.pos.y = req->pose.py; + gzReq->modelPose.pos.z = req->pose.pz; + + gzReq->modelPose.roll = req->pose.proll; + gzReq->modelPose.pitch = req->pose.ppitch; + gzReq->modelPose.yaw = req->pose.pyaw; + this->iface->Unlock(); this->driver->Publish(this->device_addr, respQueue, - PLAYER_MSGTYPE_RESP_ACK, PLAYER_SIMULATION_REQ_SET_POSE3D); + PLAYER_MSGTYPE_RESP_ACK, + PLAYER_SIMULATION_REQ_SET_POSE3D); } /// Set a 2D pose else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, - PLAYER_SIMULATION_REQ_SET_POSE2D, this->device_addr)) + PLAYER_SIMULATION_REQ_SET_POSE2D, + this->device_addr)) { + gazebo::SimulationRequestData *gzReq = NULL; + player_simulation_pose2d_req_t *req = (player_simulation_pose2d_req_t*)(data); this->iface->Lock(1); - strcpy((char*)this->iface->data->model_name,req->name); - strcpy((char*)this->iface->data->model_req,"set_pose2d"); + gzReq = &(this->iface->data->requests[ this->iface->data->requestCount++]); - this->iface->data->model_pose.pos.x = req->pose.px; - this->iface->data->model_pose.pos.y = req->pose.py; - this->iface->data->model_pose.yaw = req->pose.pa; + gzReq->type = gazebo::SimulationRequestData::SET_POSE2D; + + strcpy((char*)gzReq->modelName, req->name); + + gzReq->modelPose.pos.x = req->pose.px; + gzReq->modelPose.pos.y = req->pose.py; + gzReq->modelPose.yaw = req->pose.pa; + this->iface->Unlock(); this->driver->Publish(this->device_addr, respQueue, - PLAYER_MSGTYPE_RESP_ACK, PLAYER_SIMULATION_REQ_SET_POSE2D); + PLAYER_MSGTYPE_RESP_ACK, + PLAYER_SIMULATION_REQ_SET_POSE2D); } /// Get a 3d pose else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, - PLAYER_SIMULATION_REQ_GET_POSE3D, this->device_addr)) + PLAYER_SIMULATION_REQ_GET_POSE3D, + this->device_addr)) { - bool response = false; + gazebo::SimulationRequestData *gzReq = NULL; player_simulation_pose3d_req_t *req = (player_simulation_pose3d_req_t*)(data); this->iface->Lock(1); - strcpy((char*)this->iface->data->model_name, req->name); - strcpy((char*)this->iface->data->model_req,"get_pose"); + gzReq = &(this->iface->data->requests[this->iface->data->requestCount++]); - this->iface->Unlock(); + gzReq->type = gazebo::SimulationRequestData::GET_POSE3D; - // Wait for response from gazebo - while (!response) - { - this->iface->Lock(1); - response = strcmp((char*)this->iface->data->model_name,"") == 0; - this->iface->Unlock(); - nanosleep(&sleepTime, NULL); - } + strcpy((char*)gzReq->modelName, req->name); - this->iface->Lock(1); - req->pose.px = this->iface->data->model_pose.pos.x; - req->pose.py = this->iface->data->model_pose.pos.y; - req->pose.pz = this->iface->data->model_pose.pos.z; - - req->pose.proll = this->iface->data->model_pose.roll; - req->pose.ppitch = this->iface->data->model_pose.pitch; - req->pose.pyaw = this->iface->data->model_pose.yaw; this->iface->Unlock(); - - this->driver->Publish(this->device_addr, respQueue, - PLAYER_MSGTYPE_RESP_ACK, PLAYER_SIMULATION_REQ_GET_POSE3D, - req, sizeof(*req), NULL); } /// Get a 2D pose else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_SIMULATION_REQ_GET_POSE2D, this->device_addr)) { - bool response = false; + gazebo::SimulationRequestData *gzReq = NULL; player_simulation_pose2d_req_t *req = (player_simulation_pose2d_req_t*)(data); this->iface->Lock(1); - strcpy((char*)this->iface->data->model_name, req->name); - strcpy((char*)this->iface->data->model_req,"get_pose"); + gzReq = &(this->iface->data->requests[this->iface->data->requestCount++]); - this->iface->Unlock(); + gzReq->type = gazebo::SimulationRequestData::GET_POSE2D; - // Wait for response from gazebo - while (!response) - { - this->iface->Lock(1); - response = strcmp((char*)this->iface->data->model_name,"") == 0; - this->iface->Unlock(); - nanosleep(&sleepTime, NULL); - } + strcpy((char*)gzReq->modelName, req->name); - this->iface->Lock(1); - req->pose.px = this->iface->data->model_pose.pos.x; - req->pose.py = this->iface->data->model_pose.pos.y; - req->pose.pa = this->iface->data->model_pose.yaw; this->iface->Unlock(); - - this->driver->Publish(this->device_addr, respQueue, - PLAYER_MSGTYPE_RESP_ACK, PLAYER_SIMULATION_REQ_GET_POSE2D, - req, sizeof(*req), NULL); } else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, - PLAYER_SIMULATION_REQ_GET_PROPERTY, this->device_addr)) + PLAYER_SIMULATION_REQ_GET_PROPERTY, + this->device_addr)) { player_simulation_property_req_t *req = (player_simulation_property_req_t*)(data); @@ -241,6 +223,65 @@ // called from GazeboDriver::Update void SimulationInterface::Update() { + gazebo::SimulationRequestData *result = NULL; + this->iface->Lock(1); + + for (unsigned int i=0; i < this->iface->data->resultCount; i++) + { + result = &(this->iface->data->results[i]); + + switch (result->type) + { + case gazebo::SimulationRequestData::PAUSE: + case gazebo::SimulationRequestData::RESET: + case gazebo::SimulationRequestData::SAVE: + case gazebo::SimulationRequestData::SET_POSE2D: + case gazebo::SimulationRequestData::SET_POSE3D: + break; + + case gazebo::SimulationRequestData::GET_POSE3D: + { + player_simulation_pose3d_req_t req; + + strcpy(req.name, result->modelName); + req.name_count = strlen(req.name); + + req.pose.px = result->modelPose.pos.x; + req.pose.py = result->modelPose.pos.y; + req.pose.pz = result->modelPose.pos.z; + + req.pose.proll = result->modelPose.roll; + req.pose.ppitch = result->modelPose.pitch; + req.pose.pyaw = result->modelPose.yaw; + + this->driver->Publish(this->device_addr, *(this->responseQueue), + PLAYER_MSGTYPE_RESP_ACK, PLAYER_SIMULATION_REQ_GET_POSE3D, + &req, sizeof(req), NULL); + + break; + } + case gazebo::SimulationRequestData::GET_POSE2D: + { + player_simulation_pose2d_req_t req; + + strcpy(req.name, result->modelName); + req.name_count = strlen(req.name); + + req.pose.px = result->modelPose.pos.x; + req.pose.py = result->modelPose.pos.y; + req.pose.pa = result->modelPose.yaw; + + this->driver->Publish(this->device_addr, *(this->responseQueue), + PLAYER_MSGTYPE_RESP_ACK, PLAYER_SIMULATION_REQ_GET_POSE2D, + &req, sizeof(req), NULL); + + break; + } + } + } + + this->iface->Unlock(); + return; } Modified: code/gazebo/trunk/player/SimulationInterface.hh =================================================================== --- code/gazebo/trunk/player/SimulationInterface.hh 2008-07-21 16:12:15 UTC (rev 6895) +++ code/gazebo/trunk/player/SimulationInterface.hh 2008-07-21 18:16:50 UTC (rev 6896) @@ -81,6 +81,8 @@ /// \brief Pointer to the Simulation Interface public: SimulationIface *iface; + + private: QueuePointer *responseQueue; }; /// \} Modified: code/gazebo/trunk/server/World.cc =================================================================== --- code/gazebo/trunk/server/World.cc 2008-07-21 16:12:15 UTC (rev 6895) +++ code/gazebo/trunk/server/World.cc 2008-07-21 18:16:50 UTC (rev 6896) @@ -150,11 +150,6 @@ (*miter)->Init(); } - // Set initial simulator state - this->simIface->Lock(1); - this->simIface->data->pause = Simulator::Instance()->IsPaused(); - this->simIface->Unlock(); - this->physicsEngine->Init(); this->toAddModels.clear(); @@ -495,91 +490,139 @@ // Update the simulation interface void World::UpdateSimulationIface() { + SimulationRequestData *results = NULL; + //TODO: Move this method to simulator? Hard because of the models this->simIface->Lock(1); - if (this->simIface->GetOpenCount() > 0) + if (this->simIface->GetOpenCount() <= 0) { - this->simIface->data->simTime = Simulator::Instance()->GetSimTime(); - this->simIface->data->pauseTime = Simulator::Instance()->GetPauseTime(); - this->simIface->data->realTime = Simulator::Instance()->GetRealTime(); - this->simIface->data->state = !Simulator::Instance()->GetUserPause(); + this->simIface->Unlock(); + return; + } - if (this->simIface->data->reset) - { - this->Reset(); - this->simIface->data->reset = 0; - } + results = this->simIface->data->results; + this->simIface->data->resultCount = 0; - if (this->simIface->data->pause) - { - Simulator::Instance()->SetUserPause(!Simulator::Instance()->GetUserPause()); - this->simIface->data->pause = 0; - } + this->simIface->data->simTime = Simulator::Instance()->GetSimTime(); + this->simIface->data->pauseTime = Simulator::Instance()->GetPauseTime(); + this->simIface->data->realTime = Simulator::Instance()->GetRealTime(); + this->simIface->data->state = !Simulator::Instance()->GetUserPause(); - //TODO: save as , load - if (this->simIface->data->save) - { - Simulator::Instance()->Save(); - this->simIface->data->save=0; - } + unsigned int requestCount = this->simIface->data->requestCount; - // If the model_name is set, then a request has been received - if (strcmp((char*)this->simIface->data->model_name,"")!=0) + // Max sure the request count is valid + if (this->simIface->data->requestCount > GAZEBO_SIMULATION_MAX_REQUESTS) + { + gzerr(0) << "Request count[" << this->simIface->data->requestCount << "] greater than max allowable[" << GAZEBO_SIMULATION_MAX_REQUESTS << "]\n"; + + requestCount = GAZEBO_SIMULATION_MAX_REQUESTS; + } + + // Process all the requests + for (unsigned int i=0; i < requestCount; i++) + { + SimulationRequestData *req = &(this->simIface->data->requests[i]); + + switch (req->type) { - /// Get the model requested - Model *model = this->GetModelByName((char*)this->simIface->data->model_name); - if (model) - { - std::string req = (char*)this->simIface->data->model_req; - if (req == "get_pose") - { - Pose3d pose = model->GetPose(); - Vector3 rot = pose.rot.GetAsEuler(); + case SimulationRequestData::PAUSE: + Simulator::Instance()->SetUserPause( + !Simulator::Instance()->GetUserPause()); + break; - this->simIface->data->model_pose.pos.x = pose.pos.x; - this->simIface->data->model_pose.pos.y = pose.pos.y; - this->simIface->data->model_pose.pos.z = pose.pos.z; + case SimulationRequestData::RESET: + this->Reset(); + break; + case SimulationRequestData::SAVE: + Simulator::Instance()->Save(); + break; - this->simIface->data->model_pose.roll = rot.x; - this->simIface->data->model_pose.pitch = rot.y; - this->simIface->data->model_pose.yaw = rot.z; - } - else if (req == "set_pose3d") + case SimulationRequestData::SET_POSE3D: { Pose3d pose; + Model *model = this->GetModelByName((char*)req->modelName); + if (model) + { + pose.pos.x = req->modelPose.pos.x; + pose.pos.y = req->modelPose.pos.y; + pose.pos.z = req->modelPose.pos.z; - pose.pos.x = this->simIface->data->model_pose.pos.x; - pose.pos.y = this->simIface->data->model_pose.pos.y; - pose.pos.z = this->simIface->data->model_pose.pos.z; - pose.rot.SetFromEuler(Vector3(this->simIface->data->model_pose.roll, - this->simIface->data->model_pose.pitch, - this->simIface->data->model_pose.yaw)); - model->SetPose(pose); + pose.rot.SetFromEuler( + Vector3(req->modelPose.roll, + req->modelPose.pitch, + req->modelPose.yaw)); + model->SetPose(pose); + } + else + { + gzerr(0) << "Invalid model name[" << req->modelName << "] in simulation interface Set Pose 3d Request.\n"; + } + + break; } - else if (req == "set_pose2d") + + case SimulationRequestData::GET_POSE3D: { - Pose3d pose = model->GetPose(); - Vector3 rot = pose.rot.GetAsEuler(); + Model *model = this->GetModelByName((char*)req->modelName); + if (model) + { + Pose3d pose = model->GetPose(); + Vector3 rot = pose.rot.GetAsEuler(); - pose.pos.x = this->simIface->data->model_pose.pos.x; - pose.pos.y = this->simIface->data->model_pose.pos.y; + results->type = req->type; - pose.rot.SetFromEuler(Vector3(rot.x, rot.y, - this->simIface->data->model_pose.yaw)); - model->SetPose(pose); + strcpy( results->modelName, req->modelName); + results->modelPose.pos.x = pose.pos.x; + results->modelPose.pos.y = pose.pos.y; + results->modelPose.pos.z = pose.pos.z; + + results->modelPose.roll = rot.x; + results->modelPose.pitch = rot.y; + results->modelPose.yaw = rot.z; + + results++; + this->simIface->data->resultCount++; + } + else + { + gzerr(0) << "Invalid model name[" << req->modelName << "] in simulation interface Get Pose 3d Request.\n"; + } + + break; } - } - else - { - gzmsg(-1) << "Simulation Iface: Model[" << this->simIface->data->model_name << "] does not exist\n"; - } + case SimulationRequestData::SET_POSE2D: + { + Model *model = this->GetModelByName((char*)req->modelName); + if (model) + { + Pose3d pose = model->GetPose(); + Vector3 rot = pose.rot.GetAsEuler(); - strcpy((char*)this->simIface->data->model_name, ""); + pose.pos.x = req->modelPose.pos.x; + pose.pos.y = req->modelPose.pos.y; + + pose.rot.SetFromEuler(Vector3(rot.x, rot.y, + req->modelPose.yaw)); + model->SetPose(pose); + } + else + { + gzerr(0) << "Invalid model name[" << req->modelName << "] in simulation interface Set Pose 2d Request.\n"; + } + break; + } + + default: + gzerr(0) << "Unknown simulation iface request[" << req->type << "]\n"; + break; } + + this->simIface->data->requestCount = 0; } + this->simIface->Unlock(); } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-07-21 11:27:24
|
Revision: 6897 http://playerstage.svn.sourceforge.net/playerstage/?rev=6897&view=rev Author: natepak Date: 2008-07-21 18:27:33 +0000 (Mon, 21 Jul 2008) Log Message: ----------- Renamed result to response in the simulation interface Modified Paths: -------------- code/gazebo/trunk/libgazebo/gazebo.h code/gazebo/trunk/player/SimulationInterface.cc code/gazebo/trunk/server/World.cc Modified: code/gazebo/trunk/libgazebo/gazebo.h =================================================================== --- code/gazebo/trunk/libgazebo/gazebo.h 2008-07-21 18:16:50 UTC (rev 6896) +++ code/gazebo/trunk/libgazebo/gazebo.h 2008-07-21 18:27:33 UTC (rev 6897) @@ -406,9 +406,9 @@ public: SimulationRequestData requests[GAZEBO_SIMULATION_MAX_REQUESTS]; public: unsigned int requestCount; - /// Array of request results from the simulator - public: SimulationRequestData results[GAZEBO_SIMULATION_MAX_REQUESTS]; - public: unsigned int resultCount; + /// Array of request responses from the simulator + public: SimulationRequestData responses[GAZEBO_SIMULATION_MAX_REQUESTS]; + public: unsigned int responseCount; }; /// \brief Common simulation interface Modified: code/gazebo/trunk/player/SimulationInterface.cc =================================================================== --- code/gazebo/trunk/player/SimulationInterface.cc 2008-07-21 18:16:50 UTC (rev 6896) +++ code/gazebo/trunk/player/SimulationInterface.cc 2008-07-21 18:27:33 UTC (rev 6897) @@ -223,14 +223,14 @@ // called from GazeboDriver::Update void SimulationInterface::Update() { - gazebo::SimulationRequestData *result = NULL; + gazebo::SimulationRequestData *response = NULL; this->iface->Lock(1); - for (unsigned int i=0; i < this->iface->data->resultCount; i++) + for (unsigned int i=0; i < this->iface->data->responseCount; i++) { - result = &(this->iface->data->results[i]); + response = &(this->iface->data->responses[i]); - switch (result->type) + switch (response->type) { case gazebo::SimulationRequestData::PAUSE: case gazebo::SimulationRequestData::RESET: @@ -243,16 +243,16 @@ { player_simulation_pose3d_req_t req; - strcpy(req.name, result->modelName); + strcpy(req.name, response->modelName); req.name_count = strlen(req.name); - req.pose.px = result->modelPose.pos.x; - req.pose.py = result->modelPose.pos.y; - req.pose.pz = result->modelPose.pos.z; + req.pose.px = response->modelPose.pos.x; + req.pose.py = response->modelPose.pos.y; + req.pose.pz = response->modelPose.pos.z; - req.pose.proll = result->modelPose.roll; - req.pose.ppitch = result->modelPose.pitch; - req.pose.pyaw = result->modelPose.yaw; + req.pose.proll = response->modelPose.roll; + req.pose.ppitch = response->modelPose.pitch; + req.pose.pyaw = response->modelPose.yaw; this->driver->Publish(this->device_addr, *(this->responseQueue), PLAYER_MSGTYPE_RESP_ACK, PLAYER_SIMULATION_REQ_GET_POSE3D, @@ -264,12 +264,12 @@ { player_simulation_pose2d_req_t req; - strcpy(req.name, result->modelName); + strcpy(req.name, response->modelName); req.name_count = strlen(req.name); - req.pose.px = result->modelPose.pos.x; - req.pose.py = result->modelPose.pos.y; - req.pose.pa = result->modelPose.yaw; + req.pose.px = response->modelPose.pos.x; + req.pose.py = response->modelPose.pos.y; + req.pose.pa = response->modelPose.yaw; this->driver->Publish(this->device_addr, *(this->responseQueue), PLAYER_MSGTYPE_RESP_ACK, PLAYER_SIMULATION_REQ_GET_POSE2D, @@ -280,6 +280,8 @@ } } + this->iface->data->responseCount = 0; + this->iface->Unlock(); return; Modified: code/gazebo/trunk/server/World.cc =================================================================== --- code/gazebo/trunk/server/World.cc 2008-07-21 18:16:50 UTC (rev 6896) +++ code/gazebo/trunk/server/World.cc 2008-07-21 18:27:33 UTC (rev 6897) @@ -490,7 +490,7 @@ // Update the simulation interface void World::UpdateSimulationIface() { - SimulationRequestData *results = NULL; + SimulationRequestData *response = NULL; //TODO: Move this method to simulator? Hard because of the models this->simIface->Lock(1); @@ -501,8 +501,8 @@ return; } - results = this->simIface->data->results; - this->simIface->data->resultCount = 0; + response = this->simIface->data->responses; + this->simIface->data->responseCount = 0; this->simIface->data->simTime = Simulator::Instance()->GetSimTime(); this->simIface->data->pauseTime = Simulator::Instance()->GetPauseTime(); @@ -571,19 +571,19 @@ Pose3d pose = model->GetPose(); Vector3 rot = pose.rot.GetAsEuler(); - results->type = req->type; + response->type = req->type; - strcpy( results->modelName, req->modelName); - results->modelPose.pos.x = pose.pos.x; - results->modelPose.pos.y = pose.pos.y; - results->modelPose.pos.z = pose.pos.z; + strcpy( response->modelName, req->modelName); + response->modelPose.pos.x = pose.pos.x; + response->modelPose.pos.y = pose.pos.y; + response->modelPose.pos.z = pose.pos.z; - results->modelPose.roll = rot.x; - results->modelPose.pitch = rot.y; - results->modelPose.yaw = rot.z; + response->modelPose.roll = rot.x; + response->modelPose.pitch = rot.y; + response->modelPose.yaw = rot.z; - results++; - this->simIface->data->resultCount++; + response++; + this->simIface->data->responseCount++; } else { This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-07-22 09:14:53
|
Revision: 6900 http://playerstage.svn.sourceforge.net/playerstage/?rev=6900&view=rev Author: natepak Date: 2008-07-22 16:14:59 +0000 (Tue, 22 Jul 2008) Log Message: ----------- Added mouse picking. Modified Paths: -------------- code/gazebo/trunk/server/Entity.cc code/gazebo/trunk/server/Model.cc code/gazebo/trunk/server/Model.hh code/gazebo/trunk/server/Simulator.cc code/gazebo/trunk/server/Simulator.hh code/gazebo/trunk/server/gui/GLWindow.cc code/gazebo/trunk/server/gui/GLWindow.hh code/gazebo/trunk/server/physics/Body.cc code/gazebo/trunk/server/physics/Body.hh code/gazebo/trunk/server/physics/Geom.cc code/gazebo/trunk/server/physics/ode/ODEPhysics.cc code/gazebo/trunk/server/physics/ode/ODEPhysics.hh code/gazebo/trunk/server/rendering/OgreAdaptor.cc code/gazebo/trunk/server/rendering/OgreAdaptor.hh code/gazebo/trunk/server/rendering/OgreVisual.cc code/gazebo/trunk/server/rendering/OgreVisual.hh code/gazebo/trunk/server/rendering/UserCamera.cc code/gazebo/trunk/worlds/simpleshapes.world Modified: code/gazebo/trunk/server/Entity.cc =================================================================== --- code/gazebo/trunk/server/Entity.cc 2008-07-21 23:39:22 UTC (rev 6899) +++ code/gazebo/trunk/server/Entity.cc 2008-07-22 16:14:59 UTC (rev 6900) @@ -24,6 +24,8 @@ * Date: 03 Apr 2007 * SVN: $Id$ */ + +#include "Body.hh" #include "GazeboError.hh" #include "Global.hh" #include "OgreVisual.hh" @@ -134,7 +136,19 @@ // Set whether this entity is static: immovable void Entity::SetStatic(bool s) { + std::vector< Entity *>::iterator iter; + Body *body = NULL; + this->isStatic = s; + + for (iter = this->children.begin(); iter != this->children.end(); iter++) + { + (*iter)->SetStatic(s); + body = dynamic_cast<Body*>(*iter); + if (body) + body->SetEnabled(!s); + } + } //////////////////////////////////////////////////////////////////////////////// Modified: code/gazebo/trunk/server/Model.cc =================================================================== --- code/gazebo/trunk/server/Model.cc 2008-07-21 23:39:22 UTC (rev 6899) +++ code/gazebo/trunk/server/Model.cc 2008-07-22 16:14:59 UTC (rev 6900) @@ -174,11 +174,6 @@ } -void Model::Test() -{ - std::cout << "this is a test\n"; -} - void Model::Save() { /*std::map<std::string, Body* >::iterator bodyIter; @@ -290,11 +285,9 @@ boost::python::call<void>(this->pFuncUpdate, this); }*/ - Body *b = NULL; if (!this->canonicalBodyName.empty()) { this->pose = this->bodies[this->canonicalBodyName]->GetPose(); - b = this->bodies[this->canonicalBodyName]; } return this->UpdateChild(); Modified: code/gazebo/trunk/server/Model.hh =================================================================== --- code/gazebo/trunk/server/Model.hh 2008-07-21 23:39:22 UTC (rev 6899) +++ code/gazebo/trunk/server/Model.hh 2008-07-22 16:14:59 UTC (rev 6900) @@ -66,7 +66,6 @@ /// \brief Save the model public: void Save(); - public: void Test(); /// \brief Initialize the model public: int Init(); Modified: code/gazebo/trunk/server/Simulator.cc =================================================================== --- code/gazebo/trunk/server/Simulator.cc 2008-07-21 23:39:22 UTC (rev 6899) +++ code/gazebo/trunk/server/Simulator.cc 2008-07-22 16:14:59 UTC (rev 6900) @@ -28,6 +28,9 @@ #include <fstream> #include <sys/time.h> +#include "Model.hh" +#include "Entity.hh" +#include "OgreVisual.hh" #include "World.hh" #include "Gui.hh" #include "XMLConfig.hh" @@ -466,3 +469,33 @@ { return this->physicsEnabled; } + +//////////////////////////////////////////////////////////////////////////////// +/// Set the selected entity +void Simulator::SetSelectedEntity( Entity *ent ) +{ + if (this->selectedEntity) + { + this->selectedEntity->GetVisualNode()->ShowSelectionBox(false); + this->selectedEntity->SetStatic(false); + } + + if (this->selectedEntity != ent) + { + this->selectedEntity = ent; + this->selectedEntity->GetVisualNode()->ShowSelectionBox(true); + this->selectedEntity->SetStatic(true); + } + else + this->selectedEntity = NULL; + +} + +//////////////////////////////////////////////////////////////////////////////// +/// Get the selected entity +Entity *Simulator::GetSelectedEntity() const +{ + return this->selectedEntity; +} + + Modified: code/gazebo/trunk/server/Simulator.hh =================================================================== --- code/gazebo/trunk/server/Simulator.hh 2008-07-21 23:39:22 UTC (rev 6899) +++ code/gazebo/trunk/server/Simulator.hh 2008-07-22 16:14:59 UTC (rev 6900) @@ -44,6 +44,7 @@ class XMLConfigNode; class GazeboConfig; class OgreAdaptor; + class Entity; /// \brief The World /* @@ -157,6 +158,12 @@ /// \brief Get the physics enabled/disabled public: bool GetPhysicsEnabled() const; + /// \brief Set the selected entity + public: void SetSelectedEntity( Entity *ent ); + + /// \brief Get the selected entity + public: Entity *GetSelectedEntity() const; + ///pointer to the XML Data private: XMLConfig *xmlFile; @@ -213,6 +220,9 @@ /// Length of time the simulation should run private: double timeout; + /// The entity currently selected by the user + private: Entity *selectedEntity; + //Singleton implementation private: friend class DestroyerT<Simulator>; private: friend class SingletonT<Simulator>; Modified: code/gazebo/trunk/server/gui/GLWindow.cc =================================================================== --- code/gazebo/trunk/server/gui/GLWindow.cc 2008-07-21 23:39:22 UTC (rev 6899) +++ code/gazebo/trunk/server/gui/GLWindow.cc 2008-07-22 16:14:59 UTC (rev 6900) @@ -24,6 +24,7 @@ * SVN: $Id:$ */ +#include "Model.hh" #include <X11/keysym.h> #include <X11/Xlib.h> #include <X11/Xutil.h> @@ -32,6 +33,7 @@ #include <GL/glx.h> +#include "Entity.hh" #include "OgreCamera.hh" #include "OgreCreator.hh" #include "Simulator.hh" @@ -195,7 +197,11 @@ if (!this->mouseDrag) { - //Entity *ent = World::Instance()->GetEntityAt(this->mousePos); + Entity *ent = OgreAdaptor::Instance()->GetEntityAt(this->activeCamera, this->mousePos); + if (ent) + { + Simulator::Instance()->SetSelectedEntity( ent ); + } } this->mouseDrag = false; @@ -207,18 +213,44 @@ { if (this->activeCamera && this->activeCamera->GetUserMovable()) { + Vector2<int> d = this->mousePos - this->prevMousePos; if (this->leftMousePressed) { - Vector2<int> d = this->mousePos - this->prevMousePos; this->activeCamera->RotateYaw(DTOR(-d.x * this->rotateAmount)); this->activeCamera->RotatePitch(DTOR(d.y * this->rotateAmount)); } + else if (this->rightMousePressed) + { + Model *model = dynamic_cast<Model*>(Simulator::Instance()->GetSelectedEntity()); + if (model) + { + Pose3d pose = model->GetPose(); + pose.pos.y -= d.x * 0.05; + pose.pos.x -= d.y * 0.05; + model->SetPose(pose); + } + } + } this->mouseDrag = true; } //////////////////////////////////////////////////////////////////////////////// +// Handle mouse wheel movement +void GLWindow::HandleMouseWheel(int dx, int dy) +{ + Model *model = dynamic_cast<Model*>(Simulator::Instance()->GetSelectedEntity()); + if (model) + { + Pose3d pose = model->GetPose(); + pose.pos.z += dy * 0.05; + model->SetPose(pose); + } + +} + +//////////////////////////////////////////////////////////////////////////////// /// Handle a key press void GLWindow::HandleKeyPress(int keyNum) { @@ -380,6 +412,11 @@ handled = true; break; + case FL_MOUSEWHEEL: + this->HandleMouseWheel(Fl::event_dx(), Fl::event_dy()); + handled = true; + break; + default: break; } Modified: code/gazebo/trunk/server/gui/GLWindow.hh =================================================================== --- code/gazebo/trunk/server/gui/GLWindow.hh 2008-07-21 23:39:22 UTC (rev 6899) +++ code/gazebo/trunk/server/gui/GLWindow.hh 2008-07-22 16:14:59 UTC (rev 6900) @@ -113,6 +113,8 @@ /// \brief Handle a key release private: void HandleKeyRelease(int keyNum); + /// \brief Handle mouse wheel movement + private: void HandleMouseWheel(int dx, int dy); /// ID of the window private: Window windowId; Modified: code/gazebo/trunk/server/physics/Body.cc =================================================================== --- code/gazebo/trunk/server/physics/Body.cc 2008-07-21 23:39:22 UTC (rev 6899) +++ code/gazebo/trunk/server/physics/Body.cc 2008-07-22 16:14:59 UTC (rev 6900) @@ -51,16 +51,11 @@ Body::Body(Entity *parent, dWorldID worldId) : Entity(parent) { - if (!this->IsStatic()) - { - this->bodyId = dBodyCreate(worldId); + this->bodyId = dBodyCreate(worldId); - dMassSetZero( &this->mass ); - } - else - { - this->bodyId = NULL; - } + dMassSetZero( &this->mass ); + + this->SetEnabled(!this->IsStatic()); } @@ -163,8 +158,7 @@ // Set whether gravity affects this body void Body::SetGravityMode(bool mode) { - if (this->bodyId) - dBodySetGravityMode(this->bodyId, mode); + dBodySetGravityMode(this->bodyId, mode); } //////////////////////////////////////////////////////////////////////////////// @@ -191,11 +185,16 @@ if (!this->IsStatic()) { + //this->SetEnabled(true); Pose3d pose = this->GetPose(); // Set the pose of the scene node this->visualNode->SetPose(pose); } + else + { + this->SetEnabled(false); + } for (geomIter=this->geoms.begin(); geomIter!=this->geoms.end(); geomIter++) @@ -214,7 +213,7 @@ // Attach a geom to this body void Body::AttachGeom( Geom *geom ) { - if (this->bodyId) + //if (this->bodyId) { if (geom->IsPlaceable()) { @@ -232,73 +231,35 @@ // Set the pose of the body void Body::SetPose(const Pose3d &pose) { - if (this->IsStatic()) - { - std::vector< Geom* >::iterator giter; - PlaneGeom *plane = NULL; - this->staticPose = pose; + Pose3d localPose; - this->SetPosition(this->staticPose.pos); - this->SetRotation(this->staticPose.rot); + // Compute pose of CoM + localPose = this->comPose + pose; - // Hack to fix the altitude of the ODE plane - for (giter = this->geoms.begin(); giter != this->geoms.end(); giter++) - { - if ((*giter)->GetGeomClass() == dPlaneClass) - { - plane = dynamic_cast<PlaneGeom*>(*giter); - plane->SetAltitude(pose.pos); - } - else - (*giter)->SetImmovableBasePose(this->staticPose); - } - } - else - { - Pose3d localPose; - - // Compute pose of CoM - localPose =this->comPose + pose; - - this->SetPosition(localPose.pos); - this->SetRotation(localPose.rot); - } + this->SetPosition(localPose.pos); + this->SetRotation(localPose.rot); } //////////////////////////////////////////////////////////////////////////////// // Return the pose of the body Pose3d Body::GetPose() const { - if (this->IsStatic()) - return this->staticPose; - else - { - Pose3d pose; + Pose3d pose; - pose.pos = this->GetPosition(); - pose.rot = this->GetRotation(); + pose.pos = this->GetPosition(); + pose.rot = this->GetRotation(); - pose = this->comPose.CoordPoseSolve(pose); + pose = this->comPose.CoordPoseSolve(pose); - return pose; - } + return pose; } //////////////////////////////////////////////////////////////////////////////// // Set the position of the body void Body::SetPosition(const Vector3 &pos) { + dBodySetPosition(this->bodyId, pos.x, pos.y, pos.z); - if (!this->IsStatic()) - { - // Set the position of the ODE body - dBodySetPosition(this->bodyId, pos.x, pos.y, pos.z); - } - else - { - this->staticPose.pos = pos; - } - // Set the position of the scene node this->visualNode->SetPosition(pos); } @@ -307,23 +268,15 @@ // Set the rotation of the body void Body::SetRotation(const Quatern &rot) { - if (!this->IsStatic()) - { - dQuaternion q; - q[0] = rot.u; - q[1] = rot.x; - q[2] = rot.y; - q[3] = rot.z; + dQuaternion q; + q[0] = rot.u; + q[1] = rot.x; + q[2] = rot.y; + q[3] = rot.z; - // Set the rotation of the ODE body - dBodySetQuaternion(this->bodyId, q); + // Set the rotation of the ODE body + dBodySetQuaternion(this->bodyId, q); - } - else - { - this->staticPose.rot = rot; - } - // Set the orientation of the scene node this->visualNode->SetRotation(rot); } @@ -332,22 +285,16 @@ // Return the position of the body. in global CS Vector3 Body::GetPosition() const { + Vector3 pos; + const dReal *p; - if (!this->IsStatic()) - { - Vector3 pos; - const dReal *p; + p = dBodyGetPosition(this->bodyId); - p = dBodyGetPosition(this->bodyId); + pos.x = p[0]; + pos.y = p[1]; + pos.z = p[2]; - pos.x = p[0]; - pos.y = p[1]; - pos.z = p[2]; - - return pos; - } - else - return this->staticPose.pos; + return pos; } @@ -355,23 +302,17 @@ // Return the rotation Quatern Body::GetRotation() const { - if (!this->IsStatic()) - { - Quatern rot; - const dReal *r; + Quatern rot; + const dReal *r; - r = dBodyGetQuaternion(this->bodyId); + r = dBodyGetQuaternion(this->bodyId); - rot.u = r[0]; - rot.x = r[1]; - rot.y = r[2]; - rot.z = r[3]; + rot.u = r[0]; + rot.x = r[1]; + rot.y = r[2]; + rot.z = r[3]; - return rot; - } - else - return this->staticPose.rot; - + return rot; } //////////////////////////////////////////////////////////////////////////////// @@ -477,10 +418,6 @@ Pose3d oldPose, newPose, pose; std::vector< Geom* >::iterator giter; - // Dummy bodies dont have mass - if (!this->bodyId) - return; - // Construct the mass matrix by combining all the geoms dMassSetZero( &this->mass ); @@ -552,12 +489,7 @@ /// Set the velocity of the body void Body::SetLinearVel(const Vector3 &vel) { - if (!this->bodyId) - { - gzmsg(0) << "Invalid ODE body\n"; - } - else - dBodySetLinearVel(this->bodyId, vel.x, vel.y, vel.z); + dBodySetLinearVel(this->bodyId, vel.x, vel.y, vel.z); } //////////////////////////////////////////////////////////////////////////////// @@ -567,12 +499,6 @@ Vector3 vel; const dReal *dvel; - if (!this->bodyId) - { - gzmsg(0) << "Invalid ODE body\n"; - return vel; - } - dvel = dBodyGetLinearVel(this->bodyId); vel.x = dvel[0]; @@ -586,12 +512,7 @@ /// Set the velocity of the body void Body::SetAngularVel(const Vector3 &vel) { - if (!this->bodyId) - { - gzmsg(0) << "Invalid ODE body\n"; - } - else - dBodySetAngularVel(this->bodyId, vel.x, vel.y, vel.z); + dBodySetAngularVel(this->bodyId, vel.x, vel.y, vel.z); } //////////////////////////////////////////////////////////////////////////////// @@ -601,12 +522,6 @@ Vector3 vel; const dReal *dvel; - if (!this->bodyId) - { - gzmsg(0) << "Invalid ODE body\n"; - return vel; - } - dvel = dBodyGetAngularVel(this->bodyId); vel.x = dvel[0]; @@ -619,12 +534,7 @@ /// \brief Set the force applied to the body void Body::SetForce(const Vector3 &force) { - if (!this->bodyId) - { - gzmsg(0) << "Invalid ODE body\n"; - } - else - dBodySetForce(this->bodyId, force.x, force.y, force.z); + dBodySetForce(this->bodyId, force.x, force.y, force.z); } //////////////////////////////////////////////////////////////////////////////// @@ -634,12 +544,6 @@ Vector3 force; const dReal *dforce; - if (!this->bodyId) - { - gzmsg(0) << "Invalid ODE body\n"; - return force; - } - dforce = dBodyGetForce(this->bodyId); force.x = dforce[0]; @@ -653,12 +557,7 @@ /// \brief Set the torque applied to the body void Body::SetTorque(const Vector3 &torque) { - if (!this->bodyId) - { - gzmsg(0) << "Invalid ODE body\n"; - } - else - dBodySetTorque(this->bodyId, torque.x, torque.y, torque.z); + dBodySetTorque(this->bodyId, torque.x, torque.y, torque.z); } //////////////////////////////////////////////////////////////////////////////// @@ -668,13 +567,6 @@ Vector3 torque; const dReal *dtorque; - if (!this->bodyId) - { - gzmsg(0) << "Invalid ODE body\n"; - return torque; - } - - dtorque = dBodyGetTorque(this->bodyId); torque.x = dtorque[0]; Modified: code/gazebo/trunk/server/physics/Body.hh =================================================================== --- code/gazebo/trunk/server/physics/Body.hh 2008-07-21 23:39:22 UTC (rev 6899) +++ code/gazebo/trunk/server/physics/Body.hh 2008-07-22 16:14:59 UTC (rev 6900) @@ -168,7 +168,6 @@ private: bool isStatic; private: Pose3d comPose; - private: Pose3d staticPose; }; /// \} Modified: code/gazebo/trunk/server/physics/Geom.cc =================================================================== --- code/gazebo/trunk/server/physics/Geom.cc 2008-07-21 23:39:22 UTC (rev 6899) +++ code/gazebo/trunk/server/physics/Geom.cc 2008-07-22 16:14:59 UTC (rev 6900) @@ -122,7 +122,7 @@ childNode = node->GetChild("visual"); while (childNode) { - OgreVisual *visual = new OgreVisual(this->visualNode); + OgreVisual *visual = new OgreVisual(this->visualNode, this); visual->Load(childNode); this->visuals.push_back(visual); childNode = childNode->GetNext("visual"); Modified: code/gazebo/trunk/server/physics/ode/ODEPhysics.cc =================================================================== --- code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2008-07-21 23:39:22 UTC (rev 6899) +++ code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2008-07-22 16:14:59 UTC (rev 6900) @@ -165,15 +165,12 @@ { entity->spaceId = entity->GetParent()->spaceId; } - - this->entities[entity->GetId()] = entity; } //////////////////////////////////////////////////////////////////////////////// // Remove an entity from the physics engine void ODEPhysics::RemoveEntity(Entity *entity) { - this->entities.erase(entity->GetId()); } //////////////////////////////////////////////////////////////////////////////// @@ -262,25 +259,24 @@ double h, kp, kd; contact.geom = contactGeoms[i]; - contact.surface.mode = 0; + contact.surface.mode = dContactSoftERP | dContactSoftCFM | + dContactBounce | dContactMu2; + // Compute the CFM and ERP by assuming the two bodies form a // spring-damper system. h = self->stepTime; kp = 1 / (1 / geom1->contact->kp + 1 / geom2->contact->kp); kd = geom1->contact->kd + geom2->contact->kd; - contact.surface.mode |= dContactSoftERP | dContactSoftCFM; contact.surface.soft_erp = h * kp / (h * kp + kd); contact.surface.soft_cfm = 1 / (h * kp + kd); - //contacts[i].surface.mode |= dContactBounce | dContactSoftCFM; - contact.surface.mode |= dContactApprox1; contact.surface.mu = MIN(geom1->contact->mu1, geom2->contact->mu1); - contact.surface.mu2 = 0; + contact.surface.mu2 = MIN(geom1->contact->mu2, geom2->contact->mu2); contact.surface.bounce = 0.1; contact.surface.bounce_vel = 0.1; - //contacts[i].surface.soft_cfm = 0.01; + contact.surface.soft_cfm = 0.01; dJointID c = dJointCreateContact (self->worldId, self->contactGroup, &contact); Modified: code/gazebo/trunk/server/physics/ode/ODEPhysics.hh =================================================================== --- code/gazebo/trunk/server/physics/ode/ODEPhysics.hh 2008-07-21 23:39:22 UTC (rev 6899) +++ code/gazebo/trunk/server/physics/ode/ODEPhysics.hh 2008-07-22 16:14:59 UTC (rev 6900) @@ -28,11 +28,9 @@ #define ODEPHYSICS_HH #include <ode/ode.h> -#include <map> #include "PhysicsEngine.hh" - namespace gazebo { class SphereGeom; @@ -131,9 +129,6 @@ /// \brief Collision attributes private: dJointGroupID contactGroup; - /// List of all the entities - private: std::map<int, Entity* > entities; - private: double globalCFM; private: double globalERP; }; Modified: code/gazebo/trunk/server/rendering/OgreAdaptor.cc =================================================================== --- code/gazebo/trunk/server/rendering/OgreAdaptor.cc 2008-07-21 23:39:22 UTC (rev 6899) +++ code/gazebo/trunk/server/rendering/OgreAdaptor.cc 2008-07-22 16:14:59 UTC (rev 6900) @@ -34,6 +34,8 @@ #include <iostream> #include <string.h> +#include "Model.hh" +#include "OgreVisual.hh" #include "UserCamera.hh" #include "MovableText.hh" #include "OgreHUD.hh" @@ -273,6 +275,8 @@ this->updateRate = node->GetDouble("maxUpdateRate",0,0); this->raySceneQuery = this->sceneMgr->createRayQuery( Ogre::Ray() ); + this->raySceneQuery->setSortByDistance(true); + this->raySceneQuery->setQueryMask(Ogre::SceneManager::ENTITY_TYPE_MASK); } //////////////////////////////////////////////////////////////////////////////// @@ -466,30 +470,50 @@ //////////////////////////////////////////////////////////////////////////////// /// Get an entity at a pixel location using a camera. Used for mouse picking. -Entity *OgreAdaptor::GetEntityAt(OgreCamera *camera, int x, int y) +Entity *OgreAdaptor::GetEntityAt(OgreCamera *camera, Vector2<int> mousePos) { - /* - Ogre::Vector3 camPos = camera->getPosition(); + Entity *entity = NULL; + Ogre::Camera *ogreCam = camera->GetOgreCamera(); + Ogre::Vector3 camPos = ogreCam->getPosition(); - Ogre::Ray mouseRay = camera->getCameraToViewportRay( - x/camera->GetViewportWidth(), y/camera->GetViewportHeight()); + Ogre::Ray mouseRay = ogreCam->getCameraToViewportRay( + (float)mousePos.x / ogreCam->getViewport()->getActualWidth(), + (float)mousePos.y / ogreCam->getViewport()->getActualHeight() ); this->raySceneQuery->setRay( mouseRay ); // Perform the scene query Ogre::RaySceneQueryResult &result = this->raySceneQuery->execute(); Ogre::RaySceneQueryResult::iterator iter = result.begin(); - - // Get the results, set the camera height - if (iter != result.end() && iter->worldFragment) + + for (iter = result.begin(); iter != result.end(); iter++) { - Ogre::Real terrainHeight = iter->worldFragment->singleIntersection.y; - if ((terrainHeight + 10.0f) > camPos.y) - camera->setPosition( camPos.x, terrainHeight + 10.0f, camPos.z); + if (iter->movable) + { + + OgreVisual *vis = dynamic_cast<OgreVisual*>(iter->movable->getUserObject()); + if (vis && vis->GetEntity()) + { + entity = vis->GetEntity(); + entity->GetVisualNode()->ShowSelectionBox(true); + Model *model = NULL; + + do + { + model = dynamic_cast<Model*>(entity); + entity = entity->GetParent(); + } while (model == NULL); + + return model; + } + } + } - */ + + return NULL; } + //////////////////////////////////////////////////////////////////////////////// /// Get the desired update rate double OgreAdaptor::GetUpdateRate() Modified: code/gazebo/trunk/server/rendering/OgreAdaptor.hh =================================================================== --- code/gazebo/trunk/server/rendering/OgreAdaptor.hh 2008-07-21 23:39:22 UTC (rev 6899) +++ code/gazebo/trunk/server/rendering/OgreAdaptor.hh 2008-07-22 16:14:59 UTC (rev 6900) @@ -31,6 +31,7 @@ #include <X11/Xutil.h> #include <GL/glx.h> +#include "Vector2.hh" #include "SingletonT.hh" namespace Ogre @@ -97,7 +98,7 @@ /// \brief Get an entity at a pixel location using a camera. Used for /// mouse picking. - public: Entity *GetEntityAt(OgreCamera *camera, int x, int y); + public: Entity *GetEntityAt(OgreCamera *camera, Vector2<int> mousePos); private: void LoadPlugins(); private: void SetupResources(); Modified: code/gazebo/trunk/server/rendering/OgreVisual.cc =================================================================== --- code/gazebo/trunk/server/rendering/OgreVisual.cc 2008-07-21 23:39:22 UTC (rev 6899) +++ code/gazebo/trunk/server/rendering/OgreVisual.cc 2008-07-22 16:14:59 UTC (rev 6900) @@ -24,6 +24,7 @@ * SVN: $Id:$ */ #include <Ogre.h> +#include "Entity.hh" #include "Global.hh" #include "GazeboMessage.hh" #include "GazeboError.hh" @@ -37,7 +38,7 @@ //////////////////////////////////////////////////////////////////////////////// // Constructor -OgreVisual::OgreVisual(OgreVisual *node) +OgreVisual::OgreVisual(OgreVisual *node, Entity *owner) { std::ostringstream stream; @@ -52,10 +53,10 @@ //FIXME: what if we add the capability to delete and add new children? stream << this->parentNode->getName() << "_VISUAL_" << visualCounter++; - // Create the scene node this->sceneNode = this->parentNode->createChildSceneNode( stream.str() ); + this->entity = owner; this->staticGeometry = NULL; this->boundingBoxNode = NULL; @@ -100,7 +101,7 @@ try { // Create the entity - stream << this->sceneNode->getName() << "_ENTITY"; + stream << "ENTITY_" << this->sceneNode->getName(); obj = (Ogre::MovableObject*)this->sceneNode->getCreator()->createEntity(stream.str(), meshName); } catch (Ogre::Exception e) @@ -171,6 +172,7 @@ void OgreVisual::AttachObject( Ogre::MovableObject *obj) { this->sceneNode->attachObject(obj); + obj->setUserObject( this ); } //////////////////////////////////////////////////////////////////////////////// @@ -524,3 +526,26 @@ this->boundingBoxNode->setVisible(false); } + +//////////////////////////////////////////////////////////////////////////////// +/// Get the entity that manages this visual +Entity *OgreVisual::GetEntity() const +{ + return this->entity; +} + +//////////////////////////////////////////////////////////////////////////////// +/// Set to true to show a white bounding box, used to indicate user selection +void OgreVisual::ShowSelectionBox( bool value ) +{ + Ogre::SceneNode *node = this->sceneNode; + + while (node && node->numAttachedObjects() == 0) + { + node = dynamic_cast<Ogre::SceneNode*>(node->getChild(0)); + } + if (node) + node->showBoundingBox(value); +} + + Modified: code/gazebo/trunk/server/rendering/OgreVisual.hh =================================================================== --- code/gazebo/trunk/server/rendering/OgreVisual.hh 2008-07-21 23:39:22 UTC (rev 6899) +++ code/gazebo/trunk/server/rendering/OgreVisual.hh 2008-07-22 16:14:59 UTC (rev 6900) @@ -38,12 +38,13 @@ { class XMLConfigNode; - + class Entity; + /// \brief Ogre Visual Object - class OgreVisual + class OgreVisual : public Ogre::UserDefinedObject { /// \brief Constructor - public: OgreVisual (OgreVisual *node); + public: OgreVisual (OgreVisual *node, Entity *owner = NULL); /// \brief Destructor public: virtual ~OgreVisual(); @@ -117,6 +118,14 @@ /// \brief Make the visual objects static renderables public: void MakeStatic(); + /// \brief Get the entity that manages this visual + public: Entity *GetEntity() const; + + /// \brief Set to true to show a white bounding box, used to indicate + // user selection + public: void ShowSelectionBox( bool value ); + + private: Ogre::MaterialPtr origMaterial; private: Ogre::MaterialPtr myMaterial; private: Ogre::SceneBlendType sceneBlendType; @@ -134,6 +143,7 @@ private: static unsigned int visualCounter; + private: Entity *entity; }; } Modified: code/gazebo/trunk/server/rendering/UserCamera.cc =================================================================== --- code/gazebo/trunk/server/rendering/UserCamera.cc 2008-07-21 23:39:22 UTC (rev 6899) +++ code/gazebo/trunk/server/rendering/UserCamera.cc 2008-07-22 16:14:59 UTC (rev 6900) @@ -39,7 +39,7 @@ //////////////////////////////////////////////////////////////////////////////// /// Constructor UserCamera::UserCamera(GLWindow *parentWindow) - : OgreCamera("User") + : OgreCamera("UserCamera") { this->window = OgreCreator::CreateWindow(parentWindow, parentWindow->w(), parentWindow->h()); Modified: code/gazebo/trunk/worlds/simpleshapes.world =================================================================== --- code/gazebo/trunk/worlds/simpleshapes.world 2008-07-21 23:39:22 UTC (rev 6899) +++ code/gazebo/trunk/worlds/simpleshapes.world 2008-07-22 16:14:59 UTC (rev 6900) @@ -15,10 +15,10 @@ <verbosity>5</verbosity> <physics:ode> - <stepTime>0.03</stepTime> + <stepTime>0.01</stepTime> <gravity>0 0 -9.8</gravity> - <cfm>0.008</cfm> - <erp>0.2</erp> + <cfm>10e-5</cfm> + <erp>0.3</erp> </physics:ode> <rendering:gui> @@ -32,18 +32,21 @@ <sky> <material>Gazebo/CloudySky</material> </sky> + <grid>false</grid> </rendering:ogre> <model:physical name="sphere1_model"> - <xyz>1 0 1.5</xyz> + <xyz>1 0 0.25</xyz> <rpy>0.0 0.0 0.0</rpy> <static>false</static> <body:sphere name="sphere1_body"> <geom:sphere name="sphere1_geom"> <size>0.5</size> - <mass>1.0</mass> + <mass>0.0</mass> + <mu1>109999.0</mu1> + <visual> <scale>0.5 0.5 0.5</scale> <mesh>unit_sphere</mesh> @@ -62,7 +65,7 @@ <geom:box name="box1_geom"> <size>1 1 1</size> - <mass>1.0</mass> + <mass>0.1</mass> <visual> <mesh>unit_box</mesh> <material>Gazebo/Rocky</material> @@ -79,6 +82,8 @@ <geom:cylinder name="cylinder1_geom"> <size>0.5 1</size> <mass>1.0</mass> + <mu1>1000.0</mu1> + <visual> <mesh>unit_cylinder</mesh> <material>Gazebo/RustyBarrel</material> @@ -100,6 +105,8 @@ <segments>10 10</segments> <uvTile>100 100</uvTile> <material>Gazebo/Grey</material> + <mu1>109999.0</mu1> + <mu2>1000.0</mu2> </geom:plane> </body:plane> </model:physical> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-07-22 22:01:02
|
Revision: 6906 http://playerstage.svn.sourceforge.net/playerstage/?rev=6906&view=rev Author: natepak Date: 2008-07-23 05:01:11 +0000 (Wed, 23 Jul 2008) Log Message: ----------- Updates to the toolbar Modified Paths: -------------- code/gazebo/trunk/server/gui/Gui.cc code/gazebo/trunk/server/gui/SConscript code/gazebo/trunk/server/gui/Toolbar.cc code/gazebo/trunk/server/gui/Toolbar.hh code/gazebo/trunk/worlds/simpleshapes.world Modified: code/gazebo/trunk/server/gui/Gui.cc =================================================================== --- code/gazebo/trunk/server/gui/Gui.cc 2008-07-23 02:39:35 UTC (rev 6905) +++ code/gazebo/trunk/server/gui/Gui.cc 2008-07-23 05:01:11 UTC (rev 6906) @@ -54,14 +54,22 @@ // The order of creation matters! Menubar first, followed by FrameManager, // then statusbar { + int toolbarWidth = 200; + // Create a main menu new MainMenu(0,0,w(),30,(char *)"MainMenu"); // Create the frame mamanger - this->frameMgr = new GLFrameManager(0,30, this->w(), this->h()-60, ""); + this->frameMgr = new GLFrameManager(0, 30, + this->w()-toolbarWidth, this->h()-60, ""); + this->toolbar = new Toolbar(this->w() - toolbarWidth, 30, + toolbarWidth, this->h() - 60); + // Create the status bar - this->statusbar = new StatusBar(0, height-30, width, 30); + this->statusbar = new StatusBar(0, height-30, + width-toolbarWidth, 30); + this->statusbar->gui = this; } @@ -110,7 +118,7 @@ //////////////////////////////////////////////////////////////////////////////// void Gui::Update() { - //this->toolbar->Update(); + this->toolbar->Update(); this->statusbar->Update(); this->frameMgr->Update(); Fl::check(); Modified: code/gazebo/trunk/server/gui/SConscript =================================================================== --- code/gazebo/trunk/server/gui/SConscript 2008-07-23 02:39:35 UTC (rev 6905) +++ code/gazebo/trunk/server/gui/SConscript 2008-07-23 05:01:11 UTC (rev 6906) @@ -18,7 +18,7 @@ 'server/gui/StatusBar.hh', 'server/gui/Toolbar.hh', 'server/gui/GLFrameManager.hh', - 'server/gui/GLFrame.hh' + 'server/gui/GLFrame.hh', ] ) staticObjs.append(env.StaticObject(sources)) Modified: code/gazebo/trunk/server/gui/Toolbar.cc =================================================================== --- code/gazebo/trunk/server/gui/Toolbar.cc 2008-07-23 02:39:35 UTC (rev 6905) +++ code/gazebo/trunk/server/gui/Toolbar.cc 2008-07-23 05:01:11 UTC (rev 6906) @@ -29,6 +29,8 @@ #include <FL/Fl_Output.H> #include <FL/Fl_Button.H> +#include "Entity.hh" +#include "Simulator.hh" #include "CameraManager.hh" #include "OgreCamera.hh" #include "Toolbar.hh" @@ -39,13 +41,13 @@ Toolbar::Toolbar(int x, int y, int w, int h, const char *l) : Fl_Group(x,y,w,h,l) { -/* char *buffer = new char[256]; + char *buffer = new char[256]; this->box(FL_UP_BOX); - OgreCamera *camera = CameraManager::Instance()->GetActiveCamera(); + //OgreCamera *camera = CameraManager::Instance()->GetActiveCamera(); - if (camera) + /*if (camera) { sprintf(buffer,"%s [%d x %d]", camera->GetName().c_str(), camera->GetImageWidth(), camera->GetImageHeight()); @@ -54,18 +56,19 @@ { sprintf(buffer,"Camera"); } + */ - this->cameraInfoGrp = new Fl_Group(x+10,y+20,w-20,25*5, "Camera"); + this->entityInfoGrp = new Fl_Group(x+10,y+20,w-20,25*5, "Entity"); // Camera Info Group - this->cameraInfoGrp->box(FL_BORDER_BOX); + this->entityInfoGrp->box(FL_BORDER_BOX); - // Camera name output - x = this->cameraInfoGrp->x()+20; - y = this->cameraInfoGrp->y()+2; - this->cameraName = new Fl_Output(x,y,this->cameraInfoGrp->w()-44,20); + // Entity name output + x = this->entityInfoGrp->x()+50; + y = this->entityInfoGrp->y()+2; + this->entityName = new Fl_Output(x,y, this->entityInfoGrp->w()-55,20, "Name: "); - // Prev camera button + /*// Prev camera button x = this->cameraInfoGrp->x()+2; y = this->cameraInfoGrp->y()+2; this->prevCameraButton = new Fl_Button(x,y,16,20,"<"); @@ -118,15 +121,15 @@ y = this->outputPitch->y() + this->outputPitch->h() + 5; this->outputYaw = new Fl_Value_Output(x,y,60,20,"Y"); this->outputYaw->precision(2); + */ - this->cameraInfoGrp->end(); + this->entityInfoGrp->end(); this->end(); this->resizable(NULL); delete buffer; - */ } Toolbar::~Toolbar() @@ -137,6 +140,18 @@ /// Update the toolbar data void Toolbar::Update() { + char *buffer = new char[256]; + Entity *entity = Simulator::Instance()->GetSelectedEntity(); + + if (entity) + { + sprintf(buffer,"%s", entity->GetName().c_str()); + if (strcmp(buffer, this->entityName->value()) != 0) + { + this->entityName->value(buffer); + } + } + /*char *buffer = new char[256]; OgreCamera *camera = CameraManager::Instance()->GetActiveCamera(); @@ -167,12 +182,12 @@ */ } -void Toolbar::PrevCameraButtonCB(Fl_Widget * /*w*/, void *data) -{ +//void Toolbar::PrevCameraButtonCB(Fl_Widget * /*w*/, void *data) +//{ //CameraManager::Instance()->DecActiveCamera(); -} +//} -void Toolbar::NextCameraButtonCB(Fl_Widget * /*w*/, void *data) -{ +//void Toolbar::NextCameraButtonCB(Fl_Widget * /*w*/, void *data) +//{ //CameraManager::Instance()->IncActiveCamera(); -} +//} Modified: code/gazebo/trunk/server/gui/Toolbar.hh =================================================================== --- code/gazebo/trunk/server/gui/Toolbar.hh 2008-07-23 02:39:35 UTC (rev 6905) +++ code/gazebo/trunk/server/gui/Toolbar.hh 2008-07-23 05:01:11 UTC (rev 6906) @@ -23,6 +23,7 @@ * Date: 13 Feb 2006 * SVN: $Id:$ */ + #ifndef TOOLBAR_HH #define TOOLBAR_HH @@ -47,14 +48,15 @@ /// \brief Update the toolbar data public: void Update(); - public: static void PrevCameraButtonCB(Fl_Widget * /*w*/, void *data); - public: static void NextCameraButtonCB(Fl_Widget * /*w*/, void *data); +// public: static void PrevCameraButtonCB(Fl_Widget * /*w*/, void *data); + //public: static void NextCameraButtonCB(Fl_Widget * /*w*/, void *data); - private: Fl_Group *cameraInfoGrp; - private: Fl_Button *prevCameraButton; + private: Fl_Group *entityInfoGrp; + /*private: Fl_Button *prevCameraButton; private: Fl_Button *nextCameraButton; - private: Fl_Output *cameraName; - private: Fl_Output *cameraDimensions; + */ + private: Fl_Output *entityName; + /*private: Fl_Output *cameraDimensions; private: Fl_Value_Output *outputX; private: Fl_Value_Output *outputY; private: Fl_Value_Output *outputZ; @@ -62,6 +64,7 @@ private: Fl_Value_Output *outputRoll; private: Fl_Value_Output *outputPitch; private: Fl_Value_Output *outputYaw; + */ }; Modified: code/gazebo/trunk/worlds/simpleshapes.world =================================================================== --- code/gazebo/trunk/worlds/simpleshapes.world 2008-07-23 02:39:35 UTC (rev 6905) +++ code/gazebo/trunk/worlds/simpleshapes.world 2008-07-23 05:01:11 UTC (rev 6906) @@ -15,7 +15,7 @@ <verbosity>5</verbosity> <physics:ode> - <stepTime>0.01</stepTime> + <stepTime>0.03</stepTime> <gravity>0 0 -9.8</gravity> <cfm>10e-5</cfm> <erp>0.3</erp> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-07-23 09:52:06
|
Revision: 6915 http://playerstage.svn.sourceforge.net/playerstage/?rev=6915&view=rev Author: natepak Date: 2008-07-23 16:52:14 +0000 (Wed, 23 Jul 2008) Log Message: ----------- Output entity name, pos, and rot when selected Modified Paths: -------------- code/gazebo/trunk/server/Simulator.cc code/gazebo/trunk/server/Simulator.hh code/gazebo/trunk/server/gui/GLWindow.cc code/gazebo/trunk/server/gui/Gui.cc code/gazebo/trunk/server/gui/StatusBar.cc code/gazebo/trunk/server/gui/StatusBar.hh code/gazebo/trunk/server/gui/Toolbar.cc code/gazebo/trunk/server/gui/Toolbar.hh code/gazebo/trunk/worlds/pioneer2dx.world code/gazebo/trunk/worlds/trimesh.world Modified: code/gazebo/trunk/server/Simulator.cc =================================================================== --- code/gazebo/trunk/server/Simulator.cc 2008-07-23 07:20:46 UTC (rev 6914) +++ code/gazebo/trunk/server/Simulator.cc 2008-07-23 16:52:14 UTC (rev 6915) @@ -65,7 +65,6 @@ checkpoint(0.0), renderUpdates(0), userPause(false), - userStep(false), userStepInc(false), userQuit(false), guiEnabled(true), @@ -260,8 +259,8 @@ this->simTime += step; // Update the physics engine - if (!this->GetUserPause() && !this->GetUserStep() || - (this->GetUserStep() && this->GetUserStepInc())) + if (!this->GetUserPause() || + (this->GetUserPause() && this->GetUserStepInc())) { this->iterations++; this->pause=false; @@ -386,37 +385,25 @@ //////////////////////////////////////////////////////////////////////////////// bool Simulator::GetUserPause() const { - return userPause; + return this->userPause; } //////////////////////////////////////////////////////////////////////////////// void Simulator::SetUserPause(bool pause) { - userPause = pause; + this->userPause = pause; } //////////////////////////////////////////////////////////////////////////////// -bool Simulator::GetUserStep() const -{ - return userStep; -} - -//////////////////////////////////////////////////////////////////////////////// -void Simulator::SetUserStep( bool step ) -{ - userStep = step; -} - -//////////////////////////////////////////////////////////////////////////////// bool Simulator::GetUserStepInc() const { - return userStepInc; + return this->userStepInc; } //////////////////////////////////////////////////////////////////////////////// void Simulator::SetUserStepInc(bool step) { - userStepInc = step; + this->userStepInc = step; } Modified: code/gazebo/trunk/server/Simulator.hh =================================================================== --- code/gazebo/trunk/server/Simulator.hh 2008-07-23 07:20:46 UTC (rev 6914) +++ code/gazebo/trunk/server/Simulator.hh 2008-07-23 16:52:14 UTC (rev 6915) @@ -131,12 +131,6 @@ /// \brief Set whether the user has paused public: void SetUserPause(bool pause); - /// \brief Return true if the user has stepped the simulation - public: bool GetUserStep() const; - - /// \brief Set whether the user has stepped the simulation - public: void SetUserStep( bool step ); - /// \brief Return true if the step has incremented public: bool GetUserStepInc() const; @@ -201,11 +195,8 @@ /// \brief Set to true to pause the simulation private: bool userPause; - /// Set to true to step through the simulation - private: bool userStep; - /// Set to true to increment the simulation once. This is only - /// valid when userStep is true. + /// valid when paused. private: bool userStepInc; //The user has somewhat signaled the end of the program Modified: code/gazebo/trunk/server/gui/GLWindow.cc =================================================================== --- code/gazebo/trunk/server/gui/GLWindow.cc 2008-07-23 07:20:46 UTC (rev 6914) +++ code/gazebo/trunk/server/gui/GLWindow.cc 2008-07-23 16:52:14 UTC (rev 6915) @@ -321,23 +321,6 @@ // Handle all toggle keys switch (keyNum) { - case 't': - if (sim->GetUserPause()) - sim->SetUserPause(false); - sim->SetUserStep( !sim->GetUserStep() ); - sim->SetUserStepInc( false ); - break; - - case ' ': - - if (sim->GetUserStep()) - { - sim->SetUserStepInc( true ); - } - else - sim->SetUserPause( !sim->GetUserPause() ); - break; - case FL_Escape: Simulator::Instance()->SetUserQuit(); break; Modified: code/gazebo/trunk/server/gui/Gui.cc =================================================================== --- code/gazebo/trunk/server/gui/Gui.cc 2008-07-23 07:20:46 UTC (rev 6914) +++ code/gazebo/trunk/server/gui/Gui.cc 2008-07-23 16:52:14 UTC (rev 6915) @@ -68,7 +68,7 @@ // Create the status bar this->statusbar = new StatusBar(0, height-30, - width-toolbarWidth, 30); + width, 30); this->statusbar->gui = this; } Modified: code/gazebo/trunk/server/gui/StatusBar.cc =================================================================== --- code/gazebo/trunk/server/gui/StatusBar.cc 2008-07-23 07:20:46 UTC (rev 6914) +++ code/gazebo/trunk/server/gui/StatusBar.cc 2008-07-23 16:52:14 UTC (rev 6915) @@ -27,6 +27,7 @@ #include <stdio.h> #include <FL/Fl_Value_Output.H> #include <FL/Fl_Output.H> +#include <FL/Fl_Button.H> #include "Gui.hh" #include "Simulator.hh" @@ -57,14 +58,14 @@ this->pauseTime = new Fl_Value_Output(x,y,55,20,"Pause Time"); this->pauseTime->precision(2); - //x = this->pauseTime->x() + this->pauseTime->w() + 80; - //this->iterations = new Fl_Value_Output(x,y,65,20,"Iterations"); - //this->iterations->precision(0); + x = this->pauseTime->x() + this->pauseTime->w() + 30; + this->playButton = new Fl_Button(x, y, 30, 20, "@||"); + this->playButton->callback( &gazebo::StatusBar::PlayPauseButtonCB, this ); - x = this->w() - 80; - this->statusString = new Fl_Output(x,y,80,20,""); - this->statusString->value("Running"); - this->statusString->color(FL_GREEN); + x = this->playButton->x() + this->playButton->w() + 15; + this->stepButton = new Fl_Button(x, y, 30, 20, "@>|"); + this->stepButton->callback( &gazebo::StatusBar::StepButtonCB, this ); + this->stepButton->deactivate(); this->resizable(NULL); this->end(); @@ -91,24 +92,37 @@ { this->realTime->value(Simulator::Instance()->GetRealTime()); } + this->simTime->value(Simulator::Instance()->GetSimTime()); this->pauseTime->value(Simulator::Instance()->GetPauseTime()); +} - if (Simulator::Instance()->GetUserPause()) +//////////////////////////////////////////////////////////////////////////////// +// Play pause button callback +void StatusBar::PlayPauseButtonCB( Fl_Widget *w, void *data ) +{ + StatusBar *sb = (StatusBar*)(data); + + if (strcmp(w->label(), "@||") == 0) { - this->statusString->value("PAUSED"); - this->statusString->color(FL_RED); + Simulator::Instance()->SetUserPause(true); + + sb->stepButton->activate(); + w->label("@>"); } - else if (Simulator::Instance()->GetUserStep()) - { - this->statusString->value("STEP"); - this->statusString->color(FL_RED); - } else { - this->statusString->value("RUNNING"); - this->statusString->color(FL_GREEN); + Simulator::Instance()->SetUserPause(false); + sb->stepButton->deactivate(); + w->label("@||"); } - //this->redraw(); + w->clear_visible_focus(); } + +//////////////////////////////////////////////////////////////////////////////// +/// Set button callback +void StatusBar::StepButtonCB( Fl_Widget * /*w*/, void * /*data*/ ) +{ + Simulator::Instance()->SetUserStepInc( true ); +} Modified: code/gazebo/trunk/server/gui/StatusBar.hh =================================================================== --- code/gazebo/trunk/server/gui/StatusBar.hh 2008-07-23 07:20:46 UTC (rev 6914) +++ code/gazebo/trunk/server/gui/StatusBar.hh 2008-07-23 16:52:14 UTC (rev 6915) @@ -30,6 +30,7 @@ class Fl_Value_Output; class Fl_Output; +class Fl_Button; namespace gazebo { @@ -45,14 +46,21 @@ /// \brief Update the toolbar data public: void Update(); - + + /// \brief Play Pause button callback + public: static void PlayPauseButtonCB( Fl_Widget *w, void *data ); + + /// \brief Set button callback + public: static void StepButtonCB( Fl_Widget *w, void *data ); + private: Fl_Value_Output *iterations; private: Fl_Value_Output *fps; private: Fl_Value_Output *realTime; private: Fl_Value_Output *pauseTime; private: Fl_Value_Output *simTime; - private: Fl_Output *statusString; + private: Fl_Button *playButton; + private: Fl_Button *stepButton; public: Gui *gui; }; Modified: code/gazebo/trunk/server/gui/Toolbar.cc =================================================================== --- code/gazebo/trunk/server/gui/Toolbar.cc 2008-07-23 07:20:46 UTC (rev 6914) +++ code/gazebo/trunk/server/gui/Toolbar.cc 2008-07-23 16:52:14 UTC (rev 6915) @@ -30,6 +30,7 @@ #include <FL/Fl_Button.H> #include "Entity.hh" +#include "Model.hh" #include "Simulator.hh" #include "CameraManager.hh" #include "OgreCamera.hh" @@ -45,21 +46,8 @@ this->box(FL_UP_BOX); - //OgreCamera *camera = CameraManager::Instance()->GetActiveCamera(); + this->entityInfoGrp = new Fl_Group(x+10,y+20,w-20,25*3, "Entity"); - /*if (camera) - { - sprintf(buffer,"%s [%d x %d]", camera->GetName().c_str(), camera->GetImageWidth(), camera->GetImageHeight()); - - } - else - { - sprintf(buffer,"Camera"); - } - */ - - this->entityInfoGrp = new Fl_Group(x+10,y+20,w-20,25*5, "Entity"); - // Camera Info Group this->entityInfoGrp->box(FL_BORDER_BOX); @@ -68,61 +56,12 @@ y = this->entityInfoGrp->y()+2; this->entityName = new Fl_Output(x,y, this->entityInfoGrp->w()-55,20, "Name: "); - /*// Prev camera button - x = this->cameraInfoGrp->x()+2; - y = this->cameraInfoGrp->y()+2; - this->prevCameraButton = new Fl_Button(x,y,16,20,"<"); - this->prevCameraButton->callback( &gazebo::Toolbar::PrevCameraButtonCB, this ); + y = this->entityName->y() + this->entityName->h() + 5; + this->entityPos = new Fl_Output(x,y, this->entityInfoGrp->w()-55,20, "XYZ: "); - // Next camera button - x = this->cameraInfoGrp->x() + this->cameraInfoGrp->w()-22; - y = this->cameraInfoGrp->y()+2; - this->nextCameraButton = new Fl_Button(x,y,16,20,">"); - this->nextCameraButton->callback( &gazebo::Toolbar::NextCameraButtonCB, this ); + y = this->entityPos->y() + this->entityPos->h() + 5; + this->entityRot = new Fl_Output(x,y, this->entityInfoGrp->w()-55,20, "RPY: "); - // Camera dimensions - x = this->cameraInfoGrp->x() + 40; - y = this->cameraName->y()+this->cameraName->h()+5; - this->cameraDimensions = new Fl_Output(x,y,this->cameraName->w()-20,20,"WxH"); - - - // Camera X output - x = this->cameraInfoGrp->x() + 20; - y = this->cameraDimensions->y() + this->cameraDimensions->h()+5; - this->outputX = new Fl_Value_Output(x,y,60,20,"X"); - this->outputX->precision(2); - - // Camera Y output - x = this->outputX->x(); - y = this->outputX->y() + this->outputX->h()+5; - this->outputY = new Fl_Value_Output(x,y,60,20,"Y"); - this->outputY->precision(2); - - // Camera Z output - x = this->outputY->x(); - y = this->outputY->y() + this->outputX->h()+5; - this->outputZ = new Fl_Value_Output(x,y,60,20,"Z"); - this->outputZ->precision(2); - - // Camera ROLL output - x = this->outputX->x() + this->outputX->w()+20; - y = this->outputX->y(); - this->outputRoll = new Fl_Value_Output(x,y,60,20,"R"); - this->outputRoll->precision(2); - - // Camera Pitch output - x = this->outputRoll->x(); - y = this->outputRoll->y() + this->outputRoll->h() + 5; - this->outputPitch = new Fl_Value_Output(x,y,60,20,"P"); - this->outputPitch->precision(2); - - // Camera Yaw output - x = this->outputPitch->x(); - y = this->outputPitch->y() + this->outputPitch->h() + 5; - this->outputYaw = new Fl_Value_Output(x,y,60,20,"Y"); - this->outputYaw->precision(2); - */ - this->entityInfoGrp->end(); this->end(); @@ -145,49 +84,31 @@ if (entity) { + Model *model = dynamic_cast<Model *>(entity); + sprintf(buffer,"%s", entity->GetName().c_str()); if (strcmp(buffer, this->entityName->value()) != 0) - { this->entityName->value(buffer); - } - } - /*char *buffer = new char[256]; - OgreCamera *camera = CameraManager::Instance()->GetActiveCamera(); - - if (camera != NULL) - { - sprintf(buffer,"%s", camera->GetName().c_str()); - if (strcmp(buffer,this->cameraName->value()) != 0) + if (model) { - this->cameraName->value(buffer); - } + Pose3d pose = model->GetPose(); + Vector3 rpy = pose.rot.GetAsEuler(); - sprintf(buffer,"%d x %d", camera->GetImageWidth(), camera->GetImageHeight()); - if (strcmp(buffer,this->cameraDimensions->value()) != 0) - { - this->cameraDimensions->value(buffer); - } + sprintf(buffer,"%4.2f, %4.2f, %4.2f",pose.pos.x, pose.pos.y, pose.pos.z); + if (strcmp( buffer, this->entityPos->value()) != 0) + this->entityPos->value(buffer); + sprintf(buffer,"%4.2f, %4.2f, %4.2f",rpy.x, rpy.y, rpy.z); + if (strcmp( buffer, this->entityRot->value()) != 0) + this->entityRot->value(buffer); - Pose3d pose = camera->GetWorldPose(); - - this->outputX->value(pose.pos.x); - this->outputY->value(pose.pos.y); - this->outputZ->value(pose.pos.z); - this->outputRoll->value(RTOD(pose.rot.GetRoll())); - this->outputPitch->value(RTOD(pose.rot.GetPitch())); - this->outputYaw->value(RTOD(pose.rot.GetYaw())); + } } - */ + else + { + this->entityName->value(""); + this->entityPos->value(""); + this->entityRot->value(""); + } } - -//void Toolbar::PrevCameraButtonCB(Fl_Widget * /*w*/, void *data) -//{ - //CameraManager::Instance()->DecActiveCamera(); -//} - -//void Toolbar::NextCameraButtonCB(Fl_Widget * /*w*/, void *data) -//{ - //CameraManager::Instance()->IncActiveCamera(); -//} Modified: code/gazebo/trunk/server/gui/Toolbar.hh =================================================================== --- code/gazebo/trunk/server/gui/Toolbar.hh 2008-07-23 07:20:46 UTC (rev 6914) +++ code/gazebo/trunk/server/gui/Toolbar.hh 2008-07-23 16:52:14 UTC (rev 6915) @@ -48,24 +48,12 @@ /// \brief Update the toolbar data public: void Update(); -// public: static void PrevCameraButtonCB(Fl_Widget * /*w*/, void *data); - //public: static void NextCameraButtonCB(Fl_Widget * /*w*/, void *data); - private: Fl_Group *entityInfoGrp; - /*private: Fl_Button *prevCameraButton; - private: Fl_Button *nextCameraButton; - */ + private: Fl_Output *entityName; - /*private: Fl_Output *cameraDimensions; - private: Fl_Value_Output *outputX; - private: Fl_Value_Output *outputY; - private: Fl_Value_Output *outputZ; + private: Fl_Output *entityPos; + private: Fl_Output *entityRot; - private: Fl_Value_Output *outputRoll; - private: Fl_Value_Output *outputPitch; - private: Fl_Value_Output *outputYaw; - */ - }; } Modified: code/gazebo/trunk/worlds/pioneer2dx.world =================================================================== --- code/gazebo/trunk/worlds/pioneer2dx.world 2008-07-23 07:20:46 UTC (rev 6914) +++ code/gazebo/trunk/worlds/pioneer2dx.world 2008-07-23 16:52:14 UTC (rev 6915) @@ -22,8 +22,7 @@ <stepTime>0.03</stepTime> <gravity>0 0 -9.8</gravity> <cfm>10e-5</cfm> - <erp>0.8</erp> - <maxUpdateRate>0</maxUpdateRate> + <erp>0.3</erp> </physics:ode> <rendering:gui> @@ -67,7 +66,7 @@ <model:physical name="sphere1_model"> <xyz>2.15 -1.68 .7</xyz> <rpy>0.0 0.0 0.0</rpy> - <static>true</static> + <static>false</static> <body:sphere name="sphere1_body"> <geom:sphere name="sphere1_geom"> Modified: code/gazebo/trunk/worlds/trimesh.world =================================================================== --- code/gazebo/trunk/worlds/trimesh.world 2008-07-23 07:20:46 UTC (rev 6914) +++ code/gazebo/trunk/worlds/trimesh.world 2008-07-23 16:52:14 UTC (rev 6915) @@ -58,14 +58,14 @@ <static>false</static> <body:trimesh name="pallet_body"> <geom:trimesh name="pallet_geom"> - <mesh>WoodPallet.mesh</mesh> + <mesh>Mesh.mesh</mesh> <scale>.2 .2 .2</scale> <mass>0.1</mass> <visual> <scale>.2 .2 .2</scale> <rpy>0 0 0</rpy> - <mesh>WoodPallet.mesh</mesh> + <mesh>Mesh.mesh</mesh> <material>Gazebo/WoodPallet</material> </visual> </geom:trimesh> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-07-24 08:27:46
|
Revision: 6920 http://playerstage.svn.sourceforge.net/playerstage/?rev=6920&view=rev Author: natepak Date: 2008-07-24 15:27:54 +0000 (Thu, 24 Jul 2008) Log Message: ----------- Updates to geom and body. Modified Paths: -------------- code/gazebo/trunk/server/Entity.cc code/gazebo/trunk/server/Entity.hh code/gazebo/trunk/server/Simulator.cc code/gazebo/trunk/server/physics/Body.cc code/gazebo/trunk/server/physics/Body.hh code/gazebo/trunk/server/physics/ContactParams.cc code/gazebo/trunk/server/physics/Geom.cc code/gazebo/trunk/server/physics/Geom.hh code/gazebo/trunk/server/physics/MapGeom.cc code/gazebo/trunk/server/physics/MapGeom.hh code/gazebo/trunk/server/physics/ode/ODEPhysics.cc code/gazebo/trunk/worlds/map.world code/gazebo/trunk/worlds/pioneer2dx.world code/gazebo/trunk/worlds/simpleshapes.world Modified: code/gazebo/trunk/server/Entity.cc =================================================================== --- code/gazebo/trunk/server/Entity.cc 2008-07-24 00:03:46 UTC (rev 6919) +++ code/gazebo/trunk/server/Entity.cc 2008-07-24 15:27:54 UTC (rev 6920) @@ -44,6 +44,8 @@ visualNode(0) { + this->selected = false; + if (this->parent) { this->parent->AddChild(this); @@ -159,6 +161,31 @@ } //////////////////////////////////////////////////////////////////////////////// +/// Set whether this entity has been selected by the user through the gui +bool Entity::SetSelected( bool s ) +{ + std::vector< Entity *>::iterator iter; + Body *body = NULL; + + this->selected = s; + + for (iter = this->children.begin(); iter != this->children.end(); iter++) + { + (*iter)->SetSelected(s); + body = dynamic_cast<Body*>(*iter); + if (body) + body->SetEnabled(!s); + } +} + +//////////////////////////////////////////////////////////////////////////////// +/// True if the entity is selected by the user +bool Entity::IsSelected() const +{ + return this->selected; +} + +//////////////////////////////////////////////////////////////////////////////// /// Returns true if the entities are the same. Checks only the name bool Entity::operator==(const Entity &ent) const { Modified: code/gazebo/trunk/server/Entity.hh =================================================================== --- code/gazebo/trunk/server/Entity.hh 2008-07-24 00:03:46 UTC (rev 6919) +++ code/gazebo/trunk/server/Entity.hh 2008-07-24 15:27:54 UTC (rev 6920) @@ -101,6 +101,13 @@ /// \return bool True = static public: bool IsStatic() const; + /// \brief Set whether this entity has been selected by the user through + // the gui + public: bool SetSelected( bool s ); + + /// \brief True if the entity is selected by the user + public: bool IsSelected() const; + /// \brief Returns true if the entities are the same. Checks only the name public: bool operator==(const Entity &ent) const; @@ -128,6 +135,8 @@ /// \brief Name of the entity private: std::string name; + private: bool selected; + }; /// \} Modified: code/gazebo/trunk/server/Simulator.cc =================================================================== --- code/gazebo/trunk/server/Simulator.cc 2008-07-24 00:03:46 UTC (rev 6919) +++ code/gazebo/trunk/server/Simulator.cc 2008-07-24 15:27:54 UTC (rev 6920) @@ -464,14 +464,14 @@ if (this->selectedEntity) { this->selectedEntity->GetVisualNode()->ShowSelectionBox(false); - this->selectedEntity->SetStatic(false); + this->selectedEntity->SetSelected(false); } if (this->selectedEntity != ent) { this->selectedEntity = ent; this->selectedEntity->GetVisualNode()->ShowSelectionBox(true); - this->selectedEntity->SetStatic(true); + this->selectedEntity->SetSelected(true); } else this->selectedEntity = NULL; Modified: code/gazebo/trunk/server/physics/Body.cc =================================================================== --- code/gazebo/trunk/server/physics/Body.cc 2008-07-24 00:03:46 UTC (rev 6919) +++ code/gazebo/trunk/server/physics/Body.cc 2008-07-24 15:27:54 UTC (rev 6920) @@ -51,11 +51,17 @@ Body::Body(Entity *parent, dWorldID worldId) : Entity(parent) { - this->bodyId = dBodyCreate(worldId); - dMassSetZero( &this->mass ); + if ( !this->IsStatic() ) + { + this->bodyId = dBodyCreate(worldId); - this->SetEnabled(!this->IsStatic()); + dMassSetZero( &this->mass ); + } + else + { + this->bodyId = NULL; + } } @@ -158,7 +164,8 @@ // Set whether gravity affects this body void Body::SetGravityMode(bool mode) { - dBodySetGravityMode(this->bodyId, mode); + if (this->bodyId) + dBodySetGravityMode(this->bodyId, mode); } //////////////////////////////////////////////////////////////////////////////// @@ -185,21 +192,20 @@ if (!this->IsStatic()) { - //this->SetEnabled(true); Pose3d pose = this->GetPose(); // Set the pose of the scene node this->visualNode->SetPose(pose); } - else - { - this->SetEnabled(false); - } for (geomIter=this->geoms.begin(); geomIter!=this->geoms.end(); geomIter++) { (*geomIter)->Update(); + if ((*geomIter)->GetName() == "sphere1_geom") + { + std::cout << "Geom pose[" << (*geomIter)->GetPose() << "]\n"; + } } for (sensorIter=this->sensors.begin(); @@ -213,7 +219,7 @@ // Attach a geom to this body void Body::AttachGeom( Geom *geom ) { - //if (this->bodyId) + if ( this->bodyId ) { if (geom->IsPlaceable()) { @@ -233,11 +239,38 @@ { Pose3d localPose; - // Compute pose of CoM - localPose = this->comPose + pose; + if (this->IsStatic()) + { + Pose3d oldPose = this->staticPose; + Pose3d newPose; + this->staticPose = pose; - this->SetPosition(localPose.pos); - this->SetRotation(localPose.rot); + std::vector<Geom*>::iterator iter; + + //this->SetPosition(this->staticPose.pos); + //this->SetRotation(this->staticPose.rot); + + if (this->GetName() == "map_body") + { + std::cout << "Set new pose[" << pose << "]\n"; + std::cout << "Old Pose[ " << oldPose << "]\n"; + } + + for (iter = this->geoms.begin(); iter != this->geoms.end(); iter++) + { + newPose = (*iter)->GetPose() - oldPose; + newPose += this->staticPose; + (*iter)->SetPose(newPose); + } + } + else + { + // Compute pose of CoM + localPose = this->comPose + pose; + + this->SetPosition(localPose.pos); + this->SetRotation(localPose.rot); + } } //////////////////////////////////////////////////////////////////////////////// @@ -246,10 +279,17 @@ { Pose3d pose; - pose.pos = this->GetPosition(); - pose.rot = this->GetRotation(); + if (this->IsStatic()) + { + pose = this->staticPose; + } + else + { + pose.pos = this->GetPosition(); + pose.rot = this->GetRotation(); - pose = this->comPose.CoordPoseSolve(pose); + pose = this->comPose.CoordPoseSolve(pose); + } return pose; } @@ -258,7 +298,8 @@ // Set the position of the body void Body::SetPosition(const Vector3 &pos) { - dBodySetPosition(this->bodyId, pos.x, pos.y, pos.z); + if (this->bodyId) + dBodySetPosition(this->bodyId, pos.x, pos.y, pos.z); // Set the position of the scene node this->visualNode->SetPosition(pos); @@ -268,15 +309,19 @@ // Set the rotation of the body void Body::SetRotation(const Quatern &rot) { - dQuaternion q; - q[0] = rot.u; - q[1] = rot.x; - q[2] = rot.y; - q[3] = rot.z; - // Set the rotation of the ODE body - dBodySetQuaternion(this->bodyId, q); + if (this->bodyId) + { + dQuaternion q; + q[0] = rot.u; + q[1] = rot.x; + q[2] = rot.y; + q[3] = rot.z; + // Set the rotation of the ODE body + dBodySetQuaternion(this->bodyId, q); + } + // Set the orientation of the scene node this->visualNode->SetRotation(rot); } @@ -286,14 +331,22 @@ Vector3 Body::GetPosition() const { Vector3 pos; - const dReal *p; - p = dBodyGetPosition(this->bodyId); + if (this->bodyId) + { + const dReal *p; - pos.x = p[0]; - pos.y = p[1]; - pos.z = p[2]; + p = dBodyGetPosition(this->bodyId); + pos.x = p[0]; + pos.y = p[1]; + pos.z = p[2]; + } + else + { + pos = this->staticPose.pos; + } + return pos; } @@ -303,15 +356,23 @@ Quatern Body::GetRotation() const { Quatern rot; - const dReal *r; - r = dBodyGetQuaternion(this->bodyId); + if (this->bodyId) + { + const dReal *r; - rot.u = r[0]; - rot.x = r[1]; - rot.y = r[2]; - rot.z = r[3]; + r = dBodyGetQuaternion(this->bodyId); + rot.u = r[0]; + rot.x = r[1]; + rot.y = r[2]; + rot.z = r[3]; + } + else + { + rot = this->staticPose.rot; + } + return rot; } @@ -327,6 +388,9 @@ // Set whether this body is enabled void Body::SetEnabled(bool enable) const { + if (!this->bodyId) + return; + if (enable) dBodyEnable(this->bodyId); else @@ -356,7 +420,7 @@ } else if (node->GetName() == "map") { - this->SetStatic(true); + //this->SetStatic(true); geom = new MapGeom(this); } else @@ -418,6 +482,9 @@ Pose3d oldPose, newPose, pose; std::vector< Geom* >::iterator giter; + if (!this->bodyId) + return; + // Construct the mass matrix by combining all the geoms dMassSetZero( &this->mass ); @@ -489,7 +556,8 @@ /// Set the velocity of the body void Body::SetLinearVel(const Vector3 &vel) { - dBodySetLinearVel(this->bodyId, vel.x, vel.y, vel.z); + if (this->bodyId) + dBodySetLinearVel(this->bodyId, vel.x, vel.y, vel.z); } //////////////////////////////////////////////////////////////////////////////// @@ -497,14 +565,18 @@ Vector3 Body::GetLinearVel() const { Vector3 vel; - const dReal *dvel; - dvel = dBodyGetLinearVel(this->bodyId); + if (this->bodyId) + { + const dReal *dvel; - vel.x = dvel[0]; - vel.y = dvel[1]; - vel.z = dvel[2]; + dvel = dBodyGetLinearVel(this->bodyId); + vel.x = dvel[0]; + vel.y = dvel[1]; + vel.z = dvel[2]; + } + return vel; } @@ -512,7 +584,8 @@ /// Set the velocity of the body void Body::SetAngularVel(const Vector3 &vel) { - dBodySetAngularVel(this->bodyId, vel.x, vel.y, vel.z); + if (this->bodyId) + dBodySetAngularVel(this->bodyId, vel.x, vel.y, vel.z); } //////////////////////////////////////////////////////////////////////////////// @@ -520,21 +593,26 @@ Vector3 Body::GetAngularVel() const { Vector3 vel; - const dReal *dvel; - dvel = dBodyGetAngularVel(this->bodyId); + if (this->bodyId) + { + const dReal *dvel; - vel.x = dvel[0]; - vel.y = dvel[1]; - vel.z = dvel[2]; + dvel = dBodyGetAngularVel(this->bodyId); + vel.x = dvel[0]; + vel.y = dvel[1]; + vel.z = dvel[2]; + } + return vel; } //////////////////////////////////////////////////////////////////////////////// /// \brief Set the force applied to the body void Body::SetForce(const Vector3 &force) { - dBodySetForce(this->bodyId, force.x, force.y, force.z); + if (this->bodyId) + dBodySetForce(this->bodyId, force.x, force.y, force.z); } //////////////////////////////////////////////////////////////////////////////// @@ -542,14 +620,18 @@ Vector3 Body::GetForce() const { Vector3 force; - const dReal *dforce; - dforce = dBodyGetForce(this->bodyId); + if (this->bodyId) + { + const dReal *dforce; - force.x = dforce[0]; - force.y = dforce[1]; - force.z = dforce[2]; + dforce = dBodyGetForce(this->bodyId); + force.x = dforce[0]; + force.y = dforce[1]; + force.z = dforce[2]; + } + return force; } @@ -557,7 +639,8 @@ /// \brief Set the torque applied to the body void Body::SetTorque(const Vector3 &torque) { - dBodySetTorque(this->bodyId, torque.x, torque.y, torque.z); + if (this->bodyId) + dBodySetTorque(this->bodyId, torque.x, torque.y, torque.z); } //////////////////////////////////////////////////////////////////////////////// @@ -565,13 +648,16 @@ Vector3 Body::GetTorque() const { Vector3 torque; - const dReal *dtorque; + if (this->bodyId) + { + const dReal *dtorque; - dtorque = dBodyGetTorque(this->bodyId); + dtorque = dBodyGetTorque(this->bodyId); - torque.x = dtorque[0]; - torque.y = dtorque[1]; - torque.z = dtorque[2]; + torque.x = dtorque[0]; + torque.y = dtorque[1]; + torque.z = dtorque[2]; + } return torque; } Modified: code/gazebo/trunk/server/physics/Body.hh =================================================================== --- code/gazebo/trunk/server/physics/Body.hh 2008-07-24 00:03:46 UTC (rev 6919) +++ code/gazebo/trunk/server/physics/Body.hh 2008-07-24 15:27:54 UTC (rev 6920) @@ -168,6 +168,7 @@ private: bool isStatic; private: Pose3d comPose; + private: Pose3d staticPose; }; /// \} Modified: code/gazebo/trunk/server/physics/ContactParams.cc =================================================================== --- code/gazebo/trunk/server/physics/ContactParams.cc 2008-07-24 00:03:46 UTC (rev 6919) +++ code/gazebo/trunk/server/physics/ContactParams.cc 2008-07-24 15:27:54 UTC (rev 6920) @@ -42,7 +42,7 @@ this->softCfm = 0.01; this->mu1 = dInfinity; - this->mu2 = 0.0; + this->mu2 = dInfinity; this->slip1 = 0.01; this->slip2 = 0.01; } Modified: code/gazebo/trunk/server/physics/Geom.cc =================================================================== --- code/gazebo/trunk/server/physics/Geom.cc 2008-07-24 00:03:46 UTC (rev 6919) +++ code/gazebo/trunk/server/physics/Geom.cc 2008-07-24 15:27:54 UTC (rev 6920) @@ -215,6 +215,8 @@ //this->SetName(stream.str()); } +//////////////////////////////////////////////////////////////////////////////// +// Update void Geom::Update() { this->UpdateChild(); @@ -251,34 +253,6 @@ return this->placeable; } -void Geom::PlaceImmovable() -{ - assert(IsStatic()); - if (this->geomId == 0) return; - - dQuaternion q; - Pose3d finalPose = immovableRelativePose + immovableBasePose; - q[0] = finalPose.rot.u; - q[1] = finalPose.rot.x; - q[2] = finalPose.rot.y; - q[3] = finalPose.rot.z; - - // Set the pose of the encapsulated geom - dGeomSetPosition( this->geomId, finalPose.pos.x, finalPose.pos.y, finalPose.pos.z ); - dGeomSetQuaternion( this->geomId, q); - - return; -} - -////////////////////////////////////////////////////////////////////////////// -// Set base pose when body is immovable (static) -void Geom::SetImmovableBasePose(const Pose3d & pose) -{ - immovableBasePose = pose; - this->PlaceImmovable(); - return; -} - //////////////////////////////////////////////////////////////////////////////// // Set the pose relative to the body void Geom::SetPose(const Pose3d &pose, bool updateCoM) @@ -299,18 +273,16 @@ this->visualNode->SetPose(pose); - if (this->IsStatic()) - { - immovableRelativePose = pose; - this->PlaceImmovable(); - return; - } - // Set the pose of the encapsulated geom; this is always relative // to the CoM dGeomSetPosition(this->geomId, localPose.pos.x, localPose.pos.y, localPose.pos.z); dGeomSetQuaternion(this->geomId, q); + if (this->GetName() == "sphere1_geom") + { + std::cout << "Geom set pose[" << this->GetPose() << "]\n"; + } + if (updateCoM) { this->body->UpdateCoM(); Modified: code/gazebo/trunk/server/physics/Geom.hh =================================================================== --- code/gazebo/trunk/server/physics/Geom.hh 2008-07-24 00:03:46 UTC (rev 6919) +++ code/gazebo/trunk/server/physics/Geom.hh 2008-07-24 15:27:54 UTC (rev 6920) @@ -136,12 +136,6 @@ /// \brief Set the visibility of the physical entity of this geom public: void ShowPhysics(bool); - /// @brief Place Geom when body is immovable (static) - private: void PlaceImmovable( ); - - /// @brief Set base pose when body is immovable (static) - public: virtual void SetImmovableBasePose( const Pose3d & pose ); - /// Contact parameters public: ContactParams *contact; @@ -181,9 +175,6 @@ ///our XML DATA private: XMLConfigNode *xmlNode; - private: Pose3d immovableBasePose; - private: Pose3d immovableRelativePose; - }; /// \} Modified: code/gazebo/trunk/server/physics/MapGeom.cc =================================================================== --- code/gazebo/trunk/server/physics/MapGeom.cc 2008-07-24 00:03:46 UTC (rev 6919) +++ code/gazebo/trunk/server/physics/MapGeom.cc 2008-07-24 15:27:54 UTC (rev 6920) @@ -110,11 +110,50 @@ this->ReduceTree(this->root); } - this->CreateBoxes(this->root); + //this->CreateBoxes(this->root); + this->CreateBox(); this->visualNode->MakeStatic(); } +void MapGeom::CreateBox() +{ + std::ostringstream stream; + + // Create the box geometry + BoxGeom* newBox = new BoxGeom( this->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 = 2; + float y = 2; + float z = 0.5; + float xSize = 1.0; + float ySize = 1.0; + float zSize = 1.0; + + stream << "<geom:box name='map_geom_box'>"; + stream << " <mass>0.0</mass>"; + stream << " <xyz>" << x << " " << y << " " << z << "</xyz>"; + stream << " <rpy>0 0 0</rpy>"; + stream << " <size>" << xSize << " " << ySize << " " << zSize << "</size>"; + stream << " <mass>0.01</mass>"; + stream << " <visual>"; + stream << " <mesh>unit_box</mesh>"; + stream << " <material>" << this->material << "</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; +} + void MapGeom::CreateBoxes(QuadNode *node) { if (node->leaf) @@ -125,7 +164,7 @@ std::ostringstream stream; // Create the box geometry - BoxGeom* newBox = new BoxGeom( body ); + BoxGeom* newBox = new BoxGeom( this->body ); XMLConfig *boxConfig = new XMLConfig(); Modified: code/gazebo/trunk/server/physics/MapGeom.hh =================================================================== --- code/gazebo/trunk/server/physics/MapGeom.hh 2008-07-24 00:03:46 UTC (rev 6919) +++ code/gazebo/trunk/server/physics/MapGeom.hh 2008-07-24 15:27:54 UTC (rev 6920) @@ -115,6 +115,8 @@ /// \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); Modified: code/gazebo/trunk/server/physics/ode/ODEPhysics.cc =================================================================== --- code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2008-07-24 00:03:46 UTC (rev 6919) +++ code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2008-07-24 15:27:54 UTC (rev 6920) @@ -259,8 +259,7 @@ double h, kp, kd; contact.geom = contactGeoms[i]; - contact.surface.mode = dContactSoftERP | dContactSoftCFM | - dContactBounce | dContactMu2; + contact.surface.mode = dContactSoftERP | dContactBounce | dContactMu2; // Compute the CFM and ERP by assuming the two bodies form a @@ -276,7 +275,7 @@ contact.surface.mu2 = MIN(geom1->contact->mu2, geom2->contact->mu2); contact.surface.bounce = 0.1; contact.surface.bounce_vel = 0.1; - contact.surface.soft_cfm = 0.01; + //contact.surface.soft_cfm = 0.01; dJointID c = dJointCreateContact (self->worldId, self->contactGroup, &contact); Modified: code/gazebo/trunk/worlds/map.world =================================================================== --- code/gazebo/trunk/worlds/map.world 2008-07-24 00:03:46 UTC (rev 6919) +++ code/gazebo/trunk/worlds/map.world 2008-07-24 15:27:54 UTC (rev 6920) @@ -22,7 +22,7 @@ <stepTime>0.03</stepTime> <gravity>0 0 -9.8</gravity> <cfm>10e-5</cfm> - <erp>0.8</erp> + <erp>0.3</erp> <maxUpdateRate>0</maxUpdateRate> </physics:ode> @@ -63,6 +63,7 @@ <model:physical name="map"> + <static>true</static> <body:map name="map_body"> <geom:map name="map_geom"> <image>willowMap.png</image> @@ -85,5 +86,36 @@ </light> </model:renderable> + <model:physical name="pioneer2dx_model1"> + <xyz>0 0 0.145</xyz> + <rpy>0.0 0.0 0.0</rpy> + <controller:differential_position2d name="controller1"> + <leftJoint>left_wheel_hinge</leftJoint> + <rightJoint>right_wheel_hinge</rightJoint> + <wheelSeparation>0.39</wheelSeparation> + <wheelDiameter>0.15</wheelDiameter> + <torque>5</torque> + <interface:position name="position_iface_0"/> + </controller:differential_position2d> + + <model:physical name="laser"> + <xyz>0.15 0 0.18</xyz> + + <attach> + <parentBody>chassis_body</parentBody> + <myBody>laser_body</myBody> + </attach> + + <include embedded="true"> + <xi:include href="models/sicklms200.model" /> + </include> + </model:physical> + + <include embedded="true"> + <xi:include href="models/pioneer2dx.model" /> + </include> + </model:physical> + + </gazebo:world> Modified: code/gazebo/trunk/worlds/pioneer2dx.world =================================================================== --- code/gazebo/trunk/worlds/pioneer2dx.world 2008-07-24 00:03:46 UTC (rev 6919) +++ code/gazebo/trunk/worlds/pioneer2dx.world 2008-07-24 15:27:54 UTC (rev 6920) @@ -64,9 +64,9 @@ </model:physical> <model:physical name="sphere1_model"> - <xyz>2.15 -1.68 .7</xyz> + <xyz>2.15 -1.68 .3</xyz> <rpy>0.0 0.0 0.0</rpy> - <static>false</static> + <static>true</static> <body:sphere name="sphere1_body"> <geom:sphere name="sphere1_geom"> @@ -117,10 +117,6 @@ </include> </model:physical> - <!-- - The include should be last within a model. All previous statements - will override those in the included file - --> <include embedded="true"> <xi:include href="models/pioneer2dx.model" /> </include> Modified: code/gazebo/trunk/worlds/simpleshapes.world =================================================================== --- code/gazebo/trunk/worlds/simpleshapes.world 2008-07-24 00:03:46 UTC (rev 6919) +++ code/gazebo/trunk/worlds/simpleshapes.world 2008-07-24 15:27:54 UTC (rev 6920) @@ -38,7 +38,7 @@ <model:physical name="sphere1_model"> <xyz>1 0 0.25</xyz> <rpy>0.0 0.0 0.0</rpy> - <static>false</static> + <static>true</static> <body:sphere name="sphere1_body"> <geom:sphere name="sphere1_geom"> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-07-24 09:22:20
|
Revision: 6921 http://playerstage.svn.sourceforge.net/playerstage/?rev=6921&view=rev Author: natepak Date: 2008-07-24 16:22:28 +0000 (Thu, 24 Jul 2008) Log Message: ----------- Fixed placement of static geometries Modified Paths: -------------- code/gazebo/trunk/server/physics/Body.cc code/gazebo/trunk/server/physics/Geom.cc code/gazebo/trunk/server/physics/MapGeom.cc code/gazebo/trunk/server/physics/SphereGeom.cc code/gazebo/trunk/worlds/map.world Modified: code/gazebo/trunk/server/physics/Body.cc =================================================================== --- code/gazebo/trunk/server/physics/Body.cc 2008-07-24 15:27:54 UTC (rev 6920) +++ code/gazebo/trunk/server/physics/Body.cc 2008-07-24 16:22:28 UTC (rev 6921) @@ -202,10 +202,6 @@ geomIter!=this->geoms.end(); geomIter++) { (*geomIter)->Update(); - if ((*geomIter)->GetName() == "sphere1_geom") - { - std::cout << "Geom pose[" << (*geomIter)->GetPose() << "]\n"; - } } for (sensorIter=this->sensors.begin(); @@ -250,12 +246,6 @@ //this->SetPosition(this->staticPose.pos); //this->SetRotation(this->staticPose.rot); - if (this->GetName() == "map_body") - { - std::cout << "Set new pose[" << pose << "]\n"; - std::cout << "Old Pose[ " << oldPose << "]\n"; - } - for (iter = this->geoms.begin(); iter != this->geoms.end(); iter++) { newPose = (*iter)->GetPose() - oldPose; Modified: code/gazebo/trunk/server/physics/Geom.cc =================================================================== --- code/gazebo/trunk/server/physics/Geom.cc 2008-07-24 15:27:54 UTC (rev 6920) +++ code/gazebo/trunk/server/physics/Geom.cc 2008-07-24 16:22:28 UTC (rev 6921) @@ -188,7 +188,7 @@ this->geomId = geomId; this->transId = NULL; - if (this->placeable) + if (this->placeable && !this->IsStatic()) { if (dGeomGetClass(geomId) != dTriMeshClass) { @@ -198,8 +198,11 @@ assert(dGeomGetSpace(this->geomId) == 0); } } - else + else if ( dGeomGetSpace(this->geomId) == 0 ) + { + dSpaceAdd(this->spaceId, this->geomId); assert(dGeomGetSpace(this->geomId) != 0); + } dGeomSetData(this->geomId, this); @@ -271,17 +274,13 @@ q[2] = localPose.rot.y; q[3] = localPose.rot.z; - this->visualNode->SetPose(pose); // Set the pose of the encapsulated geom; this is always relative // to the CoM dGeomSetPosition(this->geomId, localPose.pos.x, localPose.pos.y, localPose.pos.z); dGeomSetQuaternion(this->geomId, q); - if (this->GetName() == "sphere1_geom") - { - std::cout << "Geom set pose[" << this->GetPose() << "]\n"; - } + this->visualNode->SetPose(localPose); if (updateCoM) { Modified: code/gazebo/trunk/server/physics/MapGeom.cc =================================================================== --- code/gazebo/trunk/server/physics/MapGeom.cc 2008-07-24 15:27:54 UTC (rev 6920) +++ code/gazebo/trunk/server/physics/MapGeom.cc 2008-07-24 16:22:28 UTC (rev 6921) @@ -110,50 +110,14 @@ this->ReduceTree(this->root); } - //this->CreateBoxes(this->root); - this->CreateBox(); + this->CreateBoxes(this->root); this->visualNode->MakeStatic(); } -void MapGeom::CreateBox() -{ - std::ostringstream stream; - // Create the box geometry - BoxGeom* newBox = new BoxGeom( this->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 = 2; - float y = 2; - float z = 0.5; - float xSize = 1.0; - float ySize = 1.0; - float zSize = 1.0; - - stream << "<geom:box name='map_geom_box'>"; - stream << " <mass>0.0</mass>"; - stream << " <xyz>" << x << " " << y << " " << z << "</xyz>"; - stream << " <rpy>0 0 0</rpy>"; - stream << " <size>" << xSize << " " << ySize << " " << zSize << "</size>"; - stream << " <mass>0.01</mass>"; - stream << " <visual>"; - stream << " <mesh>unit_box</mesh>"; - stream << " <material>" << this->material << "</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; -} - +////////////////////////////////////////////////////////////////////////////// +// Create the ODE boxes void MapGeom::CreateBoxes(QuadNode *node) { if (node->leaf) @@ -205,6 +169,8 @@ } } +////////////////////////////////////////////////////////////////////////////// +// Reduce the size of the quad tree void MapGeom::ReduceTree(QuadNode *node) { std::deque<QuadNode*>::iterator iter; @@ -262,6 +228,8 @@ } } +////////////////////////////////////////////////////////////////////////////// +// Merege quad tree cells void MapGeom::Merge(QuadNode *nodeA, QuadNode *nodeB) { std::deque<QuadNode*>::iterator iter; @@ -310,6 +278,8 @@ } +////////////////////////////////////////////////////////////////////////////// +// Construct the quad tree void MapGeom::BuildTree(QuadNode *node) { QuadNode *newNode = NULL; @@ -382,6 +352,8 @@ } +////////////////////////////////////////////////////////////////////////////// +// 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, Modified: code/gazebo/trunk/server/physics/SphereGeom.cc =================================================================== --- code/gazebo/trunk/server/physics/SphereGeom.cc 2008-07-24 15:27:54 UTC (rev 6920) +++ code/gazebo/trunk/server/physics/SphereGeom.cc 2008-07-24 16:22:28 UTC (rev 6921) @@ -56,7 +56,7 @@ dMassSetSphereTotal(&this->mass, this->dblMass, radius); // Create the sphere geometry - this->SetGeom(dCreateSphere(0, radius ), true); + this->SetGeom( dCreateSphere(0, radius ), true); //to be able to show physics /* this->visualNode->AttachMesh("unit_sphere"); // unit_sphere radius=1 diameter=2 Modified: code/gazebo/trunk/worlds/map.world =================================================================== --- code/gazebo/trunk/worlds/map.world 2008-07-24 15:27:54 UTC (rev 6920) +++ code/gazebo/trunk/worlds/map.world 2008-07-24 16:22:28 UTC (rev 6921) @@ -19,7 +19,7 @@ <verbosity>5</verbosity> <physics:ode> - <stepTime>0.03</stepTime> + <stepTime>0.3</stepTime> <gravity>0 0 -9.8</gravity> <cfm>10e-5</cfm> <erp>0.3</erp> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-08-10 14:12:24
|
Revision: 6948 http://playerstage.svn.sourceforge.net/playerstage/?rev=6948&view=rev Author: natepak Date: 2008-08-10 21:12:29 +0000 (Sun, 10 Aug 2008) Log Message: ----------- Fixed simulation interface updating Modified Paths: -------------- code/gazebo/trunk/libgazebo/gazebo.h code/gazebo/trunk/player/SimulationInterface.cc code/gazebo/trunk/server/World.cc code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc code/gazebo/trunk/worlds/simpleshapes.world Modified: code/gazebo/trunk/libgazebo/gazebo.h =================================================================== --- code/gazebo/trunk/libgazebo/gazebo.h 2008-08-10 18:26:25 UTC (rev 6947) +++ code/gazebo/trunk/libgazebo/gazebo.h 2008-08-10 21:12:29 UTC (rev 6948) @@ -1368,18 +1368,6 @@ /// Vertical field of view of the camera in radians public: double vfov; - // Left image size - public: unsigned int left_rgb_size; - - /// left image (R8G8B8) - public: unsigned char left_rgb[GAZEBO_STEREO_CAMERA_MAX_RGB_SIZE]; - - /// Right image size - public: unsigned int right_rgb_size; - - /// Right image (R8G8B8) - public: unsigned char right_rgb[GAZEBO_STEREO_CAMERA_MAX_RGB_SIZE]; - /// Left depth map size public: unsigned int left_depth_size; Modified: code/gazebo/trunk/player/SimulationInterface.cc =================================================================== --- code/gazebo/trunk/player/SimulationInterface.cc 2008-08-10 18:26:25 UTC (rev 6947) +++ code/gazebo/trunk/player/SimulationInterface.cc 2008-08-10 21:12:29 UTC (rev 6948) @@ -51,6 +51,8 @@ GazeboClient::Init(serverId, ""); this->iface = new SimulationIface(); + + this->responseQueue = NULL; } /////////////////////////////////////////////////////////////////////////////// @@ -58,6 +60,12 @@ SimulationInterface::~SimulationInterface() { delete this->iface; + + if (this->responseQueue) + { + delete this->responseQueue; + this->responseQueue = NULL; + } } /////////////////////////////////////////////////////////////////////////////// @@ -205,8 +213,7 @@ this->iface->Unlock(); this->driver->Publish(this->device_addr, respQueue, - PLAYER_MSGTYPE_RESP_ACK, PLAYER_SIMULATION_REQ_GET_PROPERTY, - req, sizeof(*req), NULL); + PLAYER_MSGTYPE_RESP_ACK, PLAYER_SIMULATION_REQ_GET_PROPERTY, req, sizeof(*req), NULL); if (req->value) { Modified: code/gazebo/trunk/server/World.cc =================================================================== --- code/gazebo/trunk/server/World.cc 2008-08-10 18:26:25 UTC (rev 6947) +++ code/gazebo/trunk/server/World.cc 2008-08-10 21:12:29 UTC (rev 6948) @@ -502,7 +502,6 @@ } response = this->simIface->data->responses; - this->simIface->data->responseCount = 0; this->simIface->data->simTime = Simulator::Instance()->GetSimTime(); this->simIface->data->pauseTime = Simulator::Instance()->GetPauseTime(); @@ -583,7 +582,7 @@ response->modelPose.yaw = rot.z; response++; - this->simIface->data->responseCount++; + this->simIface->data->responseCount += 1; } else { Modified: code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc =================================================================== --- code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc 2008-08-10 18:26:25 UTC (rev 6947) +++ code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc 2008-08-10 21:12:29 UTC (rev 6948) @@ -151,30 +151,12 @@ stereo_data->hfov = this->myParent->GetHFOV(); stereo_data->vfov = this->myParent->GetVFOV(); - //stereo_data->right_rgb_size = stereo_data->width * stereo_data->height * 3; - //stereo_data->left_rgb_size = stereo_data->width * stereo_data->height * 3; - stereo_data->right_depth_size = stereo_data->width * stereo_data->height * sizeof(float); stereo_data->left_depth_size = stereo_data->width * stereo_data->height * sizeof(float); - // Make sure there is room to store the image - //assert (stereo_data->right_rgb_size <= sizeof(stereo_data->right_rgb)); - //assert (stereo_data->left_rgb_size <= sizeof(stereo_data->left_rgb)); - assert (stereo_data->right_depth_size <= sizeof(stereo_data->right_depth)); assert (stereo_data->left_depth_size <= sizeof(stereo_data->left_depth)); - // Copy the left pixel data to the interface - /*rgb_src = this->myParent->GetImageData(0); - rgb_dst = stereo_data->left_rgb; - memcpy(rgb_dst, rgb_src, stereo_data->left_rgb_size); - - // Copy the right pixel data to the interface - rgb_src = this->myParent->GetImageData(1); - rgb_dst = stereo_data->right_rgb; - memcpy(rgb_dst, rgb_src, stereo_data->right_rgb_size); - */ - // Copy the left depth data to the interface disp_src = this->myParent->GetDepthData(0); disp_dst = stereo_data->left_depth; Modified: code/gazebo/trunk/worlds/simpleshapes.world =================================================================== --- code/gazebo/trunk/worlds/simpleshapes.world 2008-08-10 18:26:25 UTC (rev 6947) +++ code/gazebo/trunk/worlds/simpleshapes.world 2008-08-10 21:12:29 UTC (rev 6948) @@ -36,9 +36,9 @@ </rendering:ogre> <model:physical name="sphere1_model"> - <xyz>1 0 0.25</xyz> + <xyz>1 0 0.5</xyz> <rpy>0.0 0.0 0.0</rpy> - <static>true</static> + <static>false</static> <body:sphere name="sphere1_body"> <geom:sphere name="sphere1_geom"> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-08-14 08:03:01
|
Revision: 6952 http://playerstage.svn.sourceforge.net/playerstage/?rev=6952&view=rev Author: natepak Date: 2008-08-14 15:03:10 +0000 (Thu, 14 Aug 2008) Log Message: ----------- Added error checking for interfaces with the same name Modified Paths: -------------- code/gazebo/trunk/libgazebo/Iface.cc code/gazebo/trunk/server/Model.cc code/gazebo/trunk/server/Simulator.cc code/gazebo/trunk/server/World.cc code/gazebo/trunk/server/controllers/Controller.cc code/gazebo/trunk/worlds/test.world Modified: code/gazebo/trunk/libgazebo/Iface.cc =================================================================== --- code/gazebo/trunk/libgazebo/Iface.cc 2008-08-12 21:56:03 UTC (rev 6951) +++ code/gazebo/trunk/libgazebo/Iface.cc 2008-08-14 15:03:10 UTC (rev 6952) @@ -78,7 +78,16 @@ Iface::~Iface() { if (this->mmapFd && this->creator) - this->Destroy(); + { + try + { + this->Destroy(); + } + catch (std::string e) + { + std::cerr << "Error: " << e << "\n"; + } + } } @@ -132,6 +141,13 @@ // Work out the filename this->Filename(this->id); + int testFD = open(this->filename.c_str(), O_RDONLY); + if (testFD >= 0) + { + stream << "error: interface[" << this->filename << " already exists."; + throw(stream.str()); + } + // Create and open the file this->mmapFd = open(this->filename.c_str(), O_RDWR | O_CREAT | O_TRUNC, S_IREAD | S_IWRITE); Modified: code/gazebo/trunk/server/Model.cc =================================================================== --- code/gazebo/trunk/server/Model.cc 2008-08-12 21:56:03 UTC (rev 6951) +++ code/gazebo/trunk/server/Model.cc 2008-08-14 15:03:10 UTC (rev 6952) @@ -688,17 +688,7 @@ while (childNode) { - try - { - this->LoadBody(childNode); - } - catch (GazeboError e) - { - std::cerr << "Error Loading body[" << childNode->GetString("name",std::string(), 0) << "]\n"; - std::cerr << e << std::endl; - childNode = childNode->GetNextByNSPrefix("body"); - continue; - } + this->LoadBody(childNode); childNode = childNode->GetNextByNSPrefix("body"); } Modified: code/gazebo/trunk/server/Simulator.cc =================================================================== --- code/gazebo/trunk/server/Simulator.cc 2008-08-12 21:56:03 UTC (rev 6951) +++ code/gazebo/trunk/server/Simulator.cc 2008-08-14 15:03:10 UTC (rev 6952) @@ -191,7 +191,7 @@ } catch (GazeboError e) { - gzthrow("Failed to load the GUI\n" << e); + gzthrow("Failed to load the World\n" << e); } this->loaded=true; Modified: code/gazebo/trunk/server/World.cc =================================================================== --- code/gazebo/trunk/server/World.cc 2008-08-12 21:56:03 UTC (rev 6951) +++ code/gazebo/trunk/server/World.cc 2008-08-14 15:03:10 UTC (rev 6952) @@ -85,7 +85,15 @@ GZ_DELETE (this->physicsEngine) GZ_DELETE (this->server) - GZ_DELETE (this->simIface) + + try + { + GZ_DELETE (this->simIface) + } + catch (std::string e) + { + gzthrow(e); + } } //////////////////////////////////////////////////////////////////////////////// @@ -107,8 +115,15 @@ // Create the simulator interface - this->simIface = new SimulationIface(); - this->simIface->Create(this->server, "default" ); + try + { + this->simIface = new SimulationIface(); + this->simIface->Create(this->server, "default" ); + } + catch (std::string err) + { + gzthrow(err); + } this->physicsEngine = new ODEPhysics(); //TODO: use exceptions here @@ -226,7 +241,14 @@ gzmsg(-1) << "Problem destroying simIface[" << e << "]\n"; } - this->server->Fini(); + try + { + this->server->Fini(); + } + catch (std::string e) + { + gzthrow(e); + } return 0; } Modified: code/gazebo/trunk/server/controllers/Controller.cc =================================================================== --- code/gazebo/trunk/server/controllers/Controller.cc 2008-08-12 21:56:03 UTC (rev 6951) +++ code/gazebo/trunk/server/controllers/Controller.cc 2008-08-14 15:03:10 UTC (rev 6952) @@ -96,7 +96,14 @@ } // Create the iface - iface->Create(World::Instance()->GetGzServer(), ifaceName); + try + { + iface->Create(World::Instance()->GetGzServer(), ifaceName); + } + catch (std::string e) + { + gzthrow(e); + } this->ifaces.push_back(iface); Modified: code/gazebo/trunk/worlds/test.world =================================================================== --- code/gazebo/trunk/worlds/test.world 2008-08-12 21:56:03 UTC (rev 6951) +++ code/gazebo/trunk/worlds/test.world 2008-08-14 15:03:10 UTC (rev 6952) @@ -137,6 +137,21 @@ </include> </model:physical> + <model:physical name="laser"> + <xyz>-0.15 0 0.18</xyz> + <rpy>0 0 -180</rpy> + + <attach> + <parentBody>chassis_body</parentBody> + <myBody>laser_body</myBody> + </attach> + + <include embedded="true"> + <xi:include href="models/sicklms200.model" /> + </include> + </model:physical> + + <!-- The include should be last within a model. All previous statements will override those in the included file This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-08-20 14:08:33
|
Revision: 6962 http://playerstage.svn.sourceforge.net/playerstage/?rev=6962&view=rev Author: natepak Date: 2008-08-20 21:08:32 +0000 (Wed, 20 Aug 2008) Log Message: ----------- Added in patch 2003317 from John Hsu. Added ability to save world files. Modified Paths: -------------- code/gazebo/trunk/server/Entity.cc code/gazebo/trunk/server/Entity.hh code/gazebo/trunk/server/GazeboMessage.cc code/gazebo/trunk/server/GazeboMessage.hh code/gazebo/trunk/server/Global.hh code/gazebo/trunk/server/Model.cc code/gazebo/trunk/server/Model.hh code/gazebo/trunk/server/Quatern.hh code/gazebo/trunk/server/SConscript code/gazebo/trunk/server/Simulator.cc code/gazebo/trunk/server/Simulator.hh code/gazebo/trunk/server/Vector2.hh code/gazebo/trunk/server/Vector3.hh code/gazebo/trunk/server/World.cc code/gazebo/trunk/server/World.hh code/gazebo/trunk/server/XMLConfig.cc code/gazebo/trunk/server/controllers/Controller.cc code/gazebo/trunk/server/controllers/Controller.hh code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.cc code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.hh code/gazebo/trunk/server/controllers/camera/generic/Generic_Camera.cc code/gazebo/trunk/server/controllers/camera/generic/Generic_Camera.hh code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.hh code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.cc code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.hh code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.cc code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.cc code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.hh code/gazebo/trunk/server/controllers/ptz/generic/Generic_PTZ.cc code/gazebo/trunk/server/controllers/ptz/generic/Generic_PTZ.hh code/gazebo/trunk/server/gui/GLFrame.cc code/gazebo/trunk/server/gui/GLFrameManager.cc code/gazebo/trunk/server/gui/GLFrameManager.hh code/gazebo/trunk/server/gui/GLWindow.cc code/gazebo/trunk/server/gui/Gui.cc code/gazebo/trunk/server/gui/Gui.hh code/gazebo/trunk/server/gui/MainMenu.cc code/gazebo/trunk/server/gui/MainMenu.hh code/gazebo/trunk/server/physics/BallJoint.cc code/gazebo/trunk/server/physics/BallJoint.hh code/gazebo/trunk/server/physics/Body.cc code/gazebo/trunk/server/physics/Body.hh code/gazebo/trunk/server/physics/BoxGeom.cc code/gazebo/trunk/server/physics/BoxGeom.hh code/gazebo/trunk/server/physics/CylinderGeom.cc code/gazebo/trunk/server/physics/CylinderGeom.hh code/gazebo/trunk/server/physics/Geom.cc code/gazebo/trunk/server/physics/Geom.hh code/gazebo/trunk/server/physics/HeightmapGeom.cc code/gazebo/trunk/server/physics/HeightmapGeom.hh code/gazebo/trunk/server/physics/Hinge2Joint.cc code/gazebo/trunk/server/physics/Hinge2Joint.hh code/gazebo/trunk/server/physics/HingeJoint.cc code/gazebo/trunk/server/physics/HingeJoint.hh code/gazebo/trunk/server/physics/Joint.cc code/gazebo/trunk/server/physics/Joint.hh code/gazebo/trunk/server/physics/MapGeom.cc code/gazebo/trunk/server/physics/MapGeom.hh code/gazebo/trunk/server/physics/PhysicsEngine.cc code/gazebo/trunk/server/physics/PhysicsEngine.hh code/gazebo/trunk/server/physics/PlaneGeom.cc code/gazebo/trunk/server/physics/PlaneGeom.hh code/gazebo/trunk/server/physics/RayGeom.hh code/gazebo/trunk/server/physics/SliderJoint.cc code/gazebo/trunk/server/physics/SliderJoint.hh code/gazebo/trunk/server/physics/SphereGeom.cc code/gazebo/trunk/server/physics/SphereGeom.hh code/gazebo/trunk/server/physics/TrimeshGeom.cc code/gazebo/trunk/server/physics/TrimeshGeom.hh code/gazebo/trunk/server/physics/UniversalJoint.cc code/gazebo/trunk/server/physics/UniversalJoint.hh code/gazebo/trunk/server/physics/ode/ODEPhysics.cc code/gazebo/trunk/server/physics/ode/ODEPhysics.hh code/gazebo/trunk/server/rendering/OgreAdaptor.cc code/gazebo/trunk/server/rendering/OgreAdaptor.hh code/gazebo/trunk/server/rendering/OgreCamera.cc code/gazebo/trunk/server/rendering/OgreCamera.hh code/gazebo/trunk/server/rendering/OgreCreator.cc code/gazebo/trunk/server/rendering/OgreCreator.hh code/gazebo/trunk/server/rendering/OgreVisual.cc code/gazebo/trunk/server/rendering/OgreVisual.hh code/gazebo/trunk/server/sensors/Sensor.cc code/gazebo/trunk/server/sensors/Sensor.hh code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.cc code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.hh code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh code/gazebo/trunk/server/sensors/ray/RaySensor.cc code/gazebo/trunk/server/sensors/ray/RaySensor.hh code/gazebo/trunk/worlds/models/pioneer2dx.model code/gazebo/trunk/worlds/simpleshapes.world code/gazebo/trunk/worlds/test.world Modified: code/gazebo/trunk/server/Entity.cc =================================================================== --- code/gazebo/trunk/server/Entity.cc 2008-08-18 19:48:42 UTC (rev 6961) +++ code/gazebo/trunk/server/Entity.cc 2008-08-20 21:08:32 UTC (rev 6962) @@ -40,9 +40,10 @@ Entity::Entity(Entity *parent) : parent(parent), id(++idCounter), - isStatic(false), visualNode(0) { + this->nameP = new Param<std::string>("name","",1); + this->staticP = new Param<bool>("static",false,0); this->selected = false; @@ -63,6 +64,9 @@ Entity::~Entity() { + delete this->nameP; + delete this->staticP; + GZ_DELETE(this->visualNode); World::Instance()->GetPhysicsEngine()->RemoveEntity(this); } @@ -123,14 +127,14 @@ // Set the name of the body void Entity::SetName(const std::string &name) { - this->name = name; + this->nameP->SetValue( name ); } //////////////////////////////////////////////////////////////////////////////// // Return the name of the body std::string Entity::GetName() const { - return this->name; + return this->nameP->GetValue(); } @@ -141,7 +145,7 @@ std::vector< Entity *>::iterator iter; Body *body = NULL; - this->isStatic = s; + this->staticP->SetValue( s ); for (iter = this->children.begin(); iter != this->children.end(); iter++) { @@ -157,7 +161,7 @@ // Return whether this entity is static bool Entity::IsStatic() const { - return this->isStatic; + return this->staticP->GetValue(); } //////////////////////////////////////////////////////////////////////////////// Modified: code/gazebo/trunk/server/Entity.hh =================================================================== --- code/gazebo/trunk/server/Entity.hh 2008-08-18 19:48:42 UTC (rev 6961) +++ code/gazebo/trunk/server/Entity.hh 2008-08-20 21:08:32 UTC (rev 6962) @@ -32,114 +32,116 @@ #include <string> #include <ode/ode.h> +#include "Param.hh" + namespace gazebo { - -class OgreVisual; -/// \addtogroup gazebo_server -/// \{ - - -/// Base class for all objects in Gazebo -/* - * Facilitates meshing of physics engine with rendering engine - */ -class Entity -{ - /// \brief Constructor - /// \param parent Parent of the entity. - public: Entity(Entity *parent = NULL); - - /// \brief Destructor - public: virtual ~Entity(); - - /// \brief Return the ID of this entity. This id is unique - /// \return Integer ID - public: int GetId() const; - - /// \brief Return the ID of the parent - /// \return Integer ID - public: int GetParentId() const; - - /// \brief Set the parent - /// \param parent Parent entity - public: void SetParent(Entity *parent); - - /// \brief Get the parent - /// \return Pointer to the parent entity - public: Entity *GetParent() const; - - /// \brief Add a child to this entity - /// \param child Child entity - public: void AddChild(Entity *child); - - /// \brief Get all children - /// \return Vector of children entities - public: std::vector< Entity* > &GetChildren(); - - /// \brief Return this entity's sceneNode - /// \return Ogre scene node - public: OgreVisual *GetVisualNode() const; - - /// \brief Set the scene node - /// \param sceneNode Ogre scene node - public: void SetVisualNode(OgreVisual *visualNode); - - /// \brief Set the name of the entity - /// \param name Body name - public: void SetName(const std::string &name); - - /// \brief Return the name of the entity - /// \return Name of the entity - public: std::string GetName() const; - - /// \brief Set whether this entity is static: immovable - /// \param s Bool, true = static - public: void SetStatic(bool s); - - /// \brief Return whether this entity is static - /// \return bool True = static - public: bool IsStatic() const; - - /// \brief Set whether this entity has been selected by the user through - // the gui - public: bool SetSelected( bool s ); - - /// \brief True if the entity is selected by the user - public: bool IsSelected() const; - - /// \brief Returns true if the entities are the same. Checks only the name - public: bool operator==(const Entity &ent) const; - - /// \brief Parent of this entity - protected: Entity *parent; - - /// \brief Children of this entity - protected: std::vector< Entity* > children; - - /// \brief This entities ID - private: unsigned int id; - - /// \brief Used to automaticaly chose a unique ID on creation - private: static unsigned int idCounter; - - // is this an static entity - private: bool isStatic; - - /// \brief Visual stuff - protected: OgreVisual *visualNode; - - /// \brief ODE Stuff (should be go somewhere else) - public: dSpaceID spaceId; - - /// \brief Name of the entity - private: std::string name; - - private: bool selected; - -}; - -/// \} + + class OgreVisual; + /// \addtogroup gazebo_server + /// \{ + + + /// Base class for all objects in Gazebo + /* + * Facilitates meshing of physics engine with rendering engine + */ + class Entity + { + /// \brief Constructor + /// \param parent Parent of the entity. + public: Entity(Entity *parent = NULL); + + /// \brief Destructor + public: virtual ~Entity(); + + /// \brief Return the ID of this entity. This id is unique + /// \return Integer ID + public: int GetId() const; + + /// \brief Return the ID of the parent + /// \return Integer ID + public: int GetParentId() const; + + /// \brief Set the parent + /// \param parent Parent entity + public: void SetParent(Entity *parent); + + /// \brief Get the parent + /// \return Pointer to the parent entity + public: Entity *GetParent() const; + + /// \brief Add a child to this entity + /// \param child Child entity + public: void AddChild(Entity *child); + + /// \brief Get all children + /// \return Vector of children entities + public: std::vector< Entity* > &GetChildren(); + + /// \brief Return this entity's sceneNode + /// \return Ogre scene node + public: OgreVisual *GetVisualNode() const; + + /// \brief Set the scene node + /// \param sceneNode Ogre scene node + public: void SetVisualNode(OgreVisual *visualNode); + + /// \brief Set the name of the entity + /// \param name Body name + public: void SetName(const std::string &name); + + /// \brief Return the name of the entity + /// \return Name of the entity + public: std::string GetName() const; + + /// \brief Set whether this entity is static: immovable + /// \param s Bool, true = static + public: void SetStatic(bool s); + + /// \brief Return whether this entity is static + /// \return bool True = static + public: bool IsStatic() const; + + /// \brief Set whether this entity has been selected by the user through + // the gui + public: bool SetSelected( bool s ); + + /// \brief True if the entity is selected by the user + public: bool IsSelected() const; + + /// \brief Returns true if the entities are the same. Checks only the name + public: bool operator==(const Entity &ent) const; + + /// \brief Parent of this entity + protected: Entity *parent; + + /// \brief Children of this entity + protected: std::vector< Entity* > children; + + /// \brief This entities ID + private: unsigned int id; + + /// \brief Used to automaticaly chose a unique ID on creation + private: static unsigned int idCounter; + + // is this an static entity + protected: Param<bool> *staticP; + + /// \brief Visual stuff + protected: OgreVisual *visualNode; + + /// \brief ODE Stuff (should be go somewhere else) + public: dSpaceID spaceId; + + /// \brief Name of the entity + protected: Param<std::string> *nameP; + + private: bool selected; + + }; + + /// \} } #endif Modified: code/gazebo/trunk/server/GazeboMessage.cc =================================================================== --- code/gazebo/trunk/server/GazeboMessage.cc 2008-08-18 19:48:42 UTC (rev 6961) +++ code/gazebo/trunk/server/GazeboMessage.cc 2008-08-20 21:08:32 UTC (rev 6962) @@ -27,6 +27,7 @@ #include <time.h> #include <string.h> + #include "XMLConfig.hh" #include "GazeboError.hh" #include "GazeboMessage.hh" @@ -43,12 +44,17 @@ this->errStream = &std::cerr; this->level = 0; + + this->verbosityP = new Param<int>("verbosity",0,0); + this->logDataP = new Param<bool>("logData",false,0); } //////////////////////////////////////////////////////////////////////////////// /// Destructor GazeboMessage::~GazeboMessage() { + delete this->verbosityP; + delete this->logDataP; } //////////////////////////////////////////////////////////////////////////////// @@ -61,6 +67,8 @@ return myself; } +//////////////////////////////////////////////////////////////////////////////// +// Load the Message parameters void GazeboMessage::Load(XMLConfigNode *node) { char logFilename[50]; @@ -70,10 +78,12 @@ gzthrow("Null XMLConfig node"); } - this->SetVerbose(node->GetInt("verbosity",0,0)); - this->logData = node->GetBool("logData",false); + this->verbosityP->Load(node); + this->logDataP->Load(node); - if (this->logData) + this->SetVerbose(**(this->verbosityP)); + + if (**(this->logDataP)) { time_t t; struct tm *localTime; @@ -95,20 +105,12 @@ this->logStream.open(logFilename, std::ios::out); } -void GazeboMessage::Save(XMLConfigNode *node) +//////////////////////////////////////////////////////////////////////////////// +// Save in xml format +void GazeboMessage::Save(std::string &prefix, std::ostream &stream) { - /*node->SetValue("verbosity", this->level); - node->SetValue("logData", this->logData); - - //node->NewElement("verbosity", String(this->level)); //std::ostringstream << this->level); - - //node->NewElement("logData", gazebo::String(this->logData)); - - //if (this->logData) - // node->NewElement("logData", std::ostringstream << "true"); - //else - // node->NewElement("logData", std::ostringstream << "true"); - */ + stream << prefix << *(this->verbosityP) << "\n"; + stream << prefix << *(this->logDataP) << "\n"; } //////////////////////////////////////////////////////////////////////////////// Modified: code/gazebo/trunk/server/GazeboMessage.hh =================================================================== --- code/gazebo/trunk/server/GazeboMessage.hh 2008-08-18 19:48:42 UTC (rev 6961) +++ code/gazebo/trunk/server/GazeboMessage.hh 2008-08-20 21:08:32 UTC (rev 6962) @@ -33,9 +33,11 @@ #include <string> #include <sstream> +#include "Param.hh" + namespace gazebo { - + /// \addtogroup gazebo_server /// \brief Gazebo message class /// \{ @@ -78,7 +80,7 @@ public: void Load(XMLConfigNode *node); /// \brief Saves the message parameters - public: void Save(XMLConfigNode *node); + public: void Save(std::string &prefix, std::ostream &stream); /// \brief Set the verbosity /// \param level Level of the verbosity @@ -105,7 +107,10 @@ private: std::ostream *msgStream; private: std::ostream *errStream; private: std::ofstream logStream; - + + private: Param<int> *verbosityP; + private: Param<bool> *logDataP; + /// Pointer to myself private: static GazeboMessage *myself; }; Modified: code/gazebo/trunk/server/Global.hh =================================================================== --- code/gazebo/trunk/server/Global.hh 2008-08-18 19:48:42 UTC (rev 6961) +++ code/gazebo/trunk/server/Global.hh 2008-08-20 21:08:32 UTC (rev 6962) @@ -88,5 +88,4 @@ #define GZ_DELETE(p) { if(p) { delete (p); (p)=NULL; } } - #endif Modified: code/gazebo/trunk/server/Model.cc =================================================================== --- code/gazebo/trunk/server/Model.cc 2008-08-18 19:48:42 UTC (rev 6961) +++ code/gazebo/trunk/server/Model.cc 2008-08-20 21:08:32 UTC (rev 6962) @@ -58,6 +58,16 @@ { this->type = ""; this->joint = NULL; + + this->canonicalBodyNameP = new Param<std::string>("canonicalBody", + std::string(),0); + this->xyzP = new Param<Vector3>("xyz", Vector3(0,0,0), 0); + this->rpyP = new Param<Quatern>("rpy", Quatern(1,0,0,0), 0); + + this->parameters.push_back( this->canonicalBodyNameP ); + + this->parentBodyNameP = NULL; + this->myBodyNameP = NULL; } //////////////////////////////////////////////////////////////////////////////// @@ -86,6 +96,9 @@ GZ_DELETE( citer->second ); } this->controllers.clear(); + + GZ_DELETE(this->parentBodyNameP); + GZ_DELETE(this->myBodyNameP); } //////////////////////////////////////////////////////////////////////////////// @@ -95,11 +108,18 @@ XMLConfigNode *childNode; Pose3d pose; + this->nameP->Load(node); + this->staticP->Load(node); + + this->canonicalBodyNameP->Load(node); + this->xyzP->Load(node); + this->rpyP->Load(node); + this->xmlNode = node; this->type=node->GetName(); - this->SetName(node->GetString("name",std::string(),1)); - this->SetStatic(node->GetBool("static",false,0)); + this->SetStatic(this->staticP->GetValue()); + if (this->type == "physical") this->LoadPhysical(node); else if (this->type == "renderable") @@ -131,26 +151,25 @@ // Store the pointer to this body this->bodies[body->GetName()] = body; - this->canonicalBodyName = bodyName.str(); + this->canonicalBodyNameP->SetValue( bodyName.str() ); } - if (this->canonicalBodyName.empty()) + if (this->canonicalBodyNameP->GetValue().empty()) { - this->canonicalBodyName = this->bodies.begin()->first; + this->canonicalBodyNameP->SetValue( this->bodies.begin()->first ); } // Get the position and orientation of the model (relative to parent) pose.Reset(); - pose.pos = node->GetVector3( "xyz", pose.pos ); - pose.rot = node->GetRotation( "rpy", pose.rot ); + pose.pos = this->xyzP->GetValue(); + pose.rot = this->rpyP->GetValue(); // Record the model's initial pose (for reseting) this->SetInitPose(pose); + return 0; - return this->LoadChild(node); - // Get the name of the python module /*this->pName.reset(PyString_FromString(node->GetString("python","",0).c_str())); //this->pName.reset(PyString_FromString("pioneer2dx")); @@ -170,57 +189,92 @@ this->pFuncUpdate = NULL; } */ - - } -void Model::Save() +//////////////////////////////////////////////////////////////////////////////// +// Save the model in XML format +void Model::Save(std::string &prefix, std::ostream &stream) { - /*std::map<std::string, Body* >::iterator bodyIter; + std::string p = prefix + " "; + std::string typeName; + std::map<std::string, Body* >::iterator bodyIter; std::map<std::string, Controller* >::iterator contIter; std::map<std::string, Joint* >::iterator jointIter; + Vector3 pos = this->pose.pos; - this->xmlNode->SetValue("name", this->GetName()); - this->xmlNode->SetValue("xyz", this->pose.pos); - this->xmlNode->SetValue("rpy", this->pose.rot); - this->xmlNode->SetValue("static", this->IsStatic()); - //TODO: Attach tag - if (this->type=="renderable") + if (this->parent) { - // TODO: lights + Model *pmodel = dynamic_cast<Model*>(this->parent); + pos = this->pose.pos - pmodel->GetPose().pos; } + + this->xyzP->SetValue( pos ); + this->rpyP->SetValue( this->pose.rot ); + + if (this->type=="renderable") + typeName = "renderable"; else if (this->type=="physical") + typeName = "physical"; + + stream << prefix << "<model:" << typeName; + stream << " name=\"" << this->nameP->GetValue() << "\">\n"; + stream << prefix << " " << *(this->xyzP) << "\n"; + stream << prefix << " " << *(this->rpyP) << "\n"; + + if (this->type == "physical") { - this->xmlNode->SetValue("canonicalBody",this->canonicalBodyName); + stream << prefix << " " << *(this->staticP) << "\n"; + // Save all the bodies for (bodyIter=this->bodies.begin(); bodyIter!=this->bodies.end(); bodyIter++) { + stream << "\n"; if (bodyIter->second) - { - bodyIter->second->Save(); + bodyIter->second->Save(p, stream); + } - } + // Save all the joints + for (jointIter = this->joints.begin(); jointIter != this->joints.end(); + jointIter++) + { + if (jointIter->second) + jointIter->second->Save(p, stream); } - for (jointIter = this->joints.begin(); jointIter != this->joints.end(); jointIter++) + // Save all the controllers + for (contIter=this->controllers.begin(); + contIter!=this->controllers.end(); contIter++) { - //TODO:When joints can be changed with the GUI.. + if (contIter->second) + contIter->second->Save(p, stream); } - } - else // empty + else { + if (!this->lightName.empty()) + { + OgreCreator::SaveLight(p, this->lightName, stream); + } } - for (contIter=this->controllers.begin(); - contIter!=this->controllers.end(); contIter++) + if (this->parentBodyNameP && this->myBodyNameP) { - //TODO: when the controllers can be changed (maybe reload the XML is the best way) - if (contIter->second) - contIter->second->Save(); + stream << prefix << " <attach>\n"; + stream << prefix << " " << *(this->parentBodyNameP) << "\n"; + stream << prefix << " " << *(this->myBodyNameP) << "\n"; + stream << prefix << " </attach>\n"; } - */ + // Save all child models + std::vector< Entity* >::iterator eiter; + for (eiter = this->children.begin(); eiter != this->children.end(); eiter++) + { + Model *cmodel = dynamic_cast<Model*>(*eiter); + if (cmodel) + cmodel->Save(p, stream); + } + + stream << prefix << "</model:" << typeName << ">\n"; } //////////////////////////////////////////////////////////////////////////////// @@ -285,9 +339,9 @@ boost::python::call<void>(this->pFuncUpdate, this); }*/ - if (!this->canonicalBodyName.empty()) + if (!this->canonicalBodyNameP->GetValue().empty()) { - this->pose = this->bodies[this->canonicalBodyName]->GetPose(); + this->pose = this->bodies[this->canonicalBodyNameP->GetValue()]->GetPose(); } return this->UpdateChild(); @@ -475,20 +529,6 @@ Joint *joint; - Body *body1(this->bodies[node->GetString("body1",std::string(),1)]); - Body *body2(this->bodies[node->GetString("body2",std::string(),1)]); - Body *anchorBody(this->bodies[node->GetString("anchor",std::string(),0)]); - Vector3 anchorOffset = node->GetVector3("anchorOffset",Vector3(0,0,0)); - - if (!body1) - { - gzthrow("Couldn't Find Body[" + node->GetString("body1","",1)); - } - if (!body2) - { - gzthrow("Couldn't Find Body[" + node->GetString("body2","",1)); - } - // Create a Hinge Joint if (node->GetName() == "hinge") joint = this->CreateJoint(Joint::HINGE); @@ -507,23 +547,6 @@ joint->SetModel(this); - // Attach two bodies - joint->Attach(body1,body2); - - Vector3 anchorVec = anchorBody->GetPosition() + anchorOffset; - - // Set the anchor vector - if (anchorBody) - { - joint->SetAnchor(anchorVec); - //joint->SetAnchor(anchorBody->GetPosition()); - } - /*else - { - joint->SetAnchor(anchorVec); - this->bodies.erase(node->GetString("anchor",std::string(),0)); - }*/ - // Load each joint joint->Load(node); @@ -594,7 +617,7 @@ { if (this->bodies.find(name) != this->bodies.end()) { - return this->bodies[name]; + return this->bodies[name]; } else if (name == "canonical") { @@ -607,14 +630,15 @@ // Attach this model to its parent void Model::Attach(XMLConfigNode *node) { - std::string parentBodyName = "canonical"; - std::string myBodyName = "canonical"; - Model *parentModel; + Model *parentModel = NULL; + this->parentBodyNameP = new Param<std::string>("parentBody","canonical",1); + this->myBodyNameP = new Param<std::string>("myBody",this->canonicalBodyNameP->GetValue(),1); + if (node) { - parentBodyName = node->GetString("parentBody","canonical",1); - myBodyName = node->GetString("myBody",canonicalBodyName,1); + this->parentBodyNameP->Load(node); + this->myBodyNameP->Load(node); } parentModel = dynamic_cast<Model*>(this->parent); @@ -624,8 +648,8 @@ this->joint = (HingeJoint*)this->CreateJoint(Joint::HINGE); - Body *myBody = this->GetBody(myBodyName); - Body *pBody = parentModel->GetBody(parentBodyName); + Body *myBody = this->GetBody(**(this->myBodyNameP)); + Body *pBody = parentModel->GetBody(**(this->parentBodyNameP)); if (myBody == NULL) gzthrow("No canonical body set."); @@ -651,7 +675,7 @@ /// Get the canonical body. Used for connected Model heirarchies Body *Model::GetCanonicalBody() { - return this->bodies[this->canonicalBodyName]; + return this->bodies[this->canonicalBodyNameP->GetValue()]; } //////////////////////////////////////////////////////////////////////////////// @@ -672,7 +696,7 @@ if ((childNode = node->GetChild("light"))) { - OgreCreator::CreateLight(childNode, body->GetVisualNode()); + this->lightName = OgreCreator::CreateLight(childNode, body->GetVisualNode()); } } @@ -684,12 +708,19 @@ XMLConfigNode *childNode = NULL; // Load the bodies - childNode = node->GetChildByNSPrefix("body"); + if (node->GetChildByNSPrefix("body")) + childNode = node->GetChildByNSPrefix("body"); + else + childNode = node->GetChild("body"); while (childNode) { this->LoadBody(childNode); - childNode = childNode->GetNextByNSPrefix("body"); + + if (childNode->GetNextByNSPrefix("body")) + childNode = childNode->GetNextByNSPrefix("body"); + else + childNode = childNode->GetNext("body"); } // Load the joints @@ -711,5 +742,4 @@ childNode = childNode->GetNextByNSPrefix("joint"); } - this->canonicalBodyName = node->GetString("canonicalBody",std::string(),0); } Modified: code/gazebo/trunk/server/Model.hh =================================================================== --- code/gazebo/trunk/server/Model.hh 2008-08-18 19:48:42 UTC (rev 6961) +++ code/gazebo/trunk/server/Model.hh 2008-08-20 21:08:32 UTC (rev 6962) @@ -28,11 +28,12 @@ #define MODEL_HH //#include <python2.4/Python.h> +#include <boost/any.hpp> #include <map> #include <string> #include <vector> -#include "MovableText.hh" +#include "Param.hh" #include "Pose3d.hh" #include "Joint.hh" #include "Entity.hh" @@ -65,7 +66,7 @@ public: int Load(XMLConfigNode *node); /// \brief Save the model - public: void Save(); + public: void Save(std::string &prefix, std::ostream &stream); /// \brief Initialize the model public: int Init(); @@ -80,9 +81,6 @@ /// \brief Reset the model public: void Reset(); - /// \brief Load the child model - protected: virtual int LoadChild(XMLConfigNode * /*node*/) {return 0;} - /// \brief Initialize the child model protected: virtual int InitChild() {return 0;} @@ -175,17 +173,22 @@ /// \brief Map of the controllers protected: std::map<std::string, Controller* > controllers; - /// \brief Name of the canonical body - private: std::string canonicalBodyName; - /// \brief Joint used to connected models (parent->child). private: HingeJoint *joint; /// \brief Light numbering variable to give a unique name to all light entities private: static uint lightNumber; - //private: MovableText *mtext; + private: std::vector<boost::any> parameters; + private: Param<std::string> *canonicalBodyNameP; + private: Param<Vector3> *xyzP; + private: Param<Quatern> *rpyP; + private: Param<std::string> *parentBodyNameP; + private: Param<std::string> *myBodyNameP; + // Name of a light (if the model is renderable:light) + private: std::string lightName; + /* private: PyObject *pName; private: PyObject *pModule; private: PyObject *pFuncUpdate; Modified: code/gazebo/trunk/server/Quatern.hh =================================================================== --- code/gazebo/trunk/server/Quatern.hh 2008-08-18 19:48:42 UTC (rev 6961) +++ code/gazebo/trunk/server/Quatern.hh 2008-08-20 21:08:32 UTC (rev 6962) @@ -28,6 +28,9 @@ #define QUATERN_HH #include <iostream> +#include <math.h> + +#include "Angle.hh" #include "Vector3.hh" namespace gazebo @@ -133,11 +136,41 @@ /// \return The ostream public: friend std::ostream &operator<<( std::ostream &out, const gazebo::Quatern &q ) { - out << q.u << " " << q.x << " " << q.y << " " << q.z; + Vector3 v = const_cast<Quatern*>(&q)->GetAsEuler(); + v.x = v.x * 180.0 / M_PI; + v.y = v.y * 180.0 / M_PI; + v.z = v.z * 180.0 / M_PI; + if (isnan(v.x)) + v.x = 90.0; + if (isnan(v.y)) + v.y = 90.0; + if (isnan(v.z)) + v.z = 90.0; + + out << v.x << " " << v.y << " " << v.z; + return out; } + /// \brief Istream operator + /// \param in Ostream + /// \param q Quatern to read values into + /// \return The istream + public: friend std::istream &operator>>( std::istream &in, gazebo::Quatern &q ) + { + Angle r, p, y; + + // Skip white spaces + in.setf( std::ios_base::skipws ); + in >> r >> p >> y; + + q.SetFromEuler(Vector3(*r,*p,*y)); + + return in; + } + + }; /// \} Modified: code/gazebo/trunk/server/SConscript =================================================================== --- code/gazebo/trunk/server/SConscript 2008-08-18 19:48:42 UTC (rev 6961) +++ code/gazebo/trunk/server/SConscript 2008-08-20 21:08:32 UTC (rev 6962) @@ -8,6 +8,7 @@ sources = ['main.cc', 'Vector3.cc', + 'Vector4.cc', 'Quatern.cc', 'Pose3d.cc', 'World.cc', @@ -19,6 +20,7 @@ 'GazeboMessage.cc', 'Model.cc', 'Simulator.cc', + 'Angle.cc', ] headers.append( @@ -35,9 +37,11 @@ '#/server/Time.hh', '#/server/Vector2.hh', '#/server/Vector3.hh', + '#/server/Vector4.hh', '#/server/World.hh', '#/server/XMLConfig.hh', - '#/server/GazeboConfig.hh' + '#/server/GazeboConfig.hh', + '#/server/Angle.hh' ] ) staticObjs.append( env.StaticObject(sources) ) Modified: code/gazebo/trunk/server/Simulator.cc =================================================================== --- code/gazebo/trunk/server/Simulator.cc 2008-08-18 19:48:42 UTC (rev 6961) +++ code/gazebo/trunk/server/Simulator.cc 2008-08-20 21:08:32 UTC (rev 6962) @@ -206,6 +206,7 @@ //Initialize the world if (gazebo::World::Instance()->Init() != 0) return -1; + return 0; } @@ -213,18 +214,54 @@ /// Save the world configuration file void Simulator::Save(const std::string& filename) { - // Saving in the preferred order - XMLConfigNode* root=xmlFile->GetRootNode(); - GazeboMessage::Instance()->Save(root); - World::Instance()->GetPhysicsEngine()->Save(root); - this->GetRenderEngine()->Save(root); - this->SaveGui(root); - World::Instance()->Save(root); + std::fstream output; - if (xmlFile->Save(filename)<0) + output.open(filename.c_str(), std::ios::out); + + // Write out the xml header + output << "<?xml version=\"1.0\"?>\n"; + output << "<gazebo:world\n\ + xmlns:xi=\"http://www.w3.org/2001/XInclude\"\n\ + xmlns:gazebo=\"http://playerstage.sourceforge.net/gazebo/xmlschema/#gz\"\n\ + xmlns:model=\"http://playerstage.sourceforge.net/gazebo/xmlschema/#model\"\n\ + xmlns:sensor=\"http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor\"\n\ + xmlns:window=\"http://playerstage.sourceforge.net/gazebo/xmlschema/#window\"\n\ + xmlns:param=\"http://playerstage.sourceforge.net/gazebo/xmlschema/#param\"\n\ + xmlns:body=\"http://playerstage.sourceforge.net/gazebo/xmlschema/#body\"\n\ + xmlns:geom=\"http://playerstage.sourceforge.net/gazebo/xmlschema/#geom\"\n\ + xmlns:joint=\"http://playerstage.sourceforge.net/gazebo/xmlschema/#joint\"\n\ + xmlns:interface=\"http://playerstage.sourceforge.net/gazebo/xmlschema/#interface\"\n\ + xmlns:ui=\"http://playerstage.sourceforge.net/gazebo/xmlschema/#ui\"\n\ + xmlns:rendering=\"http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering\"\n\ + xmlns:controller=\"http://playerstage.sourceforge.net/gazebo/xmlschema/#controller\"\n\ + xmlns:physics=\"http://playerstage.sourceforge.net/gazebo/xmlschema/#physics\">\n\n"; + + std::string prefix = " "; + + if (output.is_open()) { - gzthrow("The XML file could not be written back to " << filename ); - } + GazeboMessage::Instance()->Save(prefix, output); + output << "\n"; + + World::Instance()->GetPhysicsEngine()->Save(prefix, output); + output << "\n"; + + this->GetRenderEngine()->Save(prefix, output); + output << "\n"; + + this->gui->Save(prefix, output); + output << "\n"; + + World::Instance()->Save(prefix, output); + output << "\n"; + + output << "</gazebo:world>\n"; + output.close(); + } + else + { + gzerr(0) << "Unable to save XML file to file[" << filename << "]\n"; + } } //////////////////////////////////////////////////////////////////////////////// @@ -407,22 +444,6 @@ this->userStepInc = step; } - -void Simulator::SaveGui(XMLConfigNode *node) -{ - /*Vector2<int> size; - XMLConfigNode* childNode = node->GetChild("gui"); - - if (childNode && this->gui) - { - size.x = this->gui->GetWidth(); - size.y = this->gui->GetHeight(); - //childNode->SetValue("size", size); - //TODO: node->SetValue("pos", Vector2<int>(x,y)); - }*/ - -} - //////////////////////////////////////////////////////////////////////////////// // True if the gui is to be used void Simulator::SetGuiEnabled( bool enabled ) Modified: code/gazebo/trunk/server/Simulator.hh =================================================================== --- code/gazebo/trunk/server/Simulator.hh 2008-08-18 19:48:42 UTC (rev 6961) +++ code/gazebo/trunk/server/Simulator.hh 2008-08-20 21:08:32 UTC (rev 6962) @@ -118,9 +118,6 @@ /// \return The wall clock time public: double GetWallTime() const; - public: void LoadGui(XMLConfigNode *rootNode); - public: void SaveGui(XMLConfigNode *node); - //User Iteractions /// \brief Simulator finished by the user public: void SetUserQuit(); Modified: code/gazebo/trunk/server/Vector2.hh =================================================================== --- code/gazebo/trunk/server/Vector2.hh 2008-08-18 19:48:42 UTC (rev 6961) +++ code/gazebo/trunk/server/Vector2.hh 2008-08-20 21:08:32 UTC (rev 6962) @@ -132,10 +132,22 @@ public: friend std::ostream &operator<<( std::ostream &out, const gazebo::Vector2<T> &pt ) { out << pt.x << " " << pt.y; - return out; } + + /// \brief Istream operator + /// \param in Ostream + /// \param pt Vector3 to read values into + /// \return The istream + public: friend std::istream &operator>>( std::istream &in, gazebo::Vector2<T> &pt ) + { + // Skip white spaces + in.setf( std::ios_base::skipws ); + in >> pt.x >> pt.y; + return in; + } + }; /// \} Modified: code/gazebo/trunk/server/Vector3.hh =================================================================== --- code/gazebo/trunk/server/Vector3.hh 2008-08-18 19:48:42 UTC (rev 6961) +++ code/gazebo/trunk/server/Vector3.hh 2008-08-20 21:08:32 UTC (rev 6962) @@ -146,6 +146,19 @@ return out; } + /// \brief Istream operator + /// \param in Ostream + /// \param pt Vector3 to read values into + /// \return The istream + public: friend std::istream &operator>>( std::istream &in, gazebo::Vector3 &pt ) + { + // Skip white spaces + in.setf( std::ios_base::skipws ); + in >> pt.x >> pt.y >> pt.z; + return in; + } + + }; /// \} Modified: code/gazebo/trunk/server/World.cc =================================================================== --- code/gazebo/trunk/server/World.cc 2008-08-18 19:48:42 UTC (rev 6961) +++ code/gazebo/trunk/server/World.cc 2008-08-20 21:08:32 UTC (rev 6962) @@ -141,13 +141,18 @@ //////////////////////////////////////////////////////////////////////////////// // Save the world -void World::Save(XMLConfigNode *node) +void World::Save(std::string &prefix, std::ostream &stream) { std::vector< Model* >::iterator miter; + // Save all the models for (miter=this->models.begin(); miter!=this->models.end(); miter++) { - (*miter)->Save(); + if ( (*miter)->GetParent() == NULL) + { + (*miter)->Save(prefix, stream); + stream << "\n"; + } } } Modified: code/gazebo/trunk/server/World.hh =================================================================== --- code/gazebo/trunk/server/World.hh 2008-08-18 19:48:42 UTC (rev 6961) +++ code/gazebo/trunk/server/World.hh 2008-08-20 21:08:32 UTC (rev 6962) @@ -27,6 +27,7 @@ #ifndef WORLD_HH #define WORLD_HH +#include <iostream> #include <vector> #include "SingletonT.hh" #include "Vector3.hh" @@ -68,8 +69,8 @@ public: void Load(XMLConfigNode *rootNode, unsigned int serverId); /// Save the world - /// \param node XMLConfig node writer pointer - public: void Save(XMLConfigNode *node); + /// \param stream Output stream + public: void Save(std::string &prefix, std::ostream &stream); /// Initialize the world /// \return 0 on success Modified: code/gazebo/trunk/server/XMLConfig.cc =================================================================== --- code/gazebo/trunk/server/XMLConfig.cc 2008-08-18 19:48:42 UTC (rev 6961) +++ code/gazebo/trunk/server/XMLConfig.cc 2008-08-20 21:08:32 UTC (rev 6962) @@ -416,6 +416,7 @@ xmlChar* XMLConfigNode::GetNodeValue( const std::string &key ) const { xmlChar *value=NULL; + xmlChar *result=NULL; // First check if the key is an attribute if (xmlHasProp( this->xmlNode, (xmlChar*) key.c_str() )) @@ -450,7 +451,17 @@ } } - return value; + if (value) + { + int j = xmlStrlen(value)-1; + int i = 0; + while (value[i] == ' ') i++; + while (value[j] == ' ') j--; + result = xmlStrndup(value+i, j-i+1); + delete value; + } + + return result; } Modified: code/gazebo/trunk/server/controllers/Controller.cc =================================================================== --- code/gazebo/trunk/server/controllers/Controller.cc 2008-08-18 19:48:42 UTC (rev 6961) +++ code/gazebo/trunk/server/controllers/Controller.cc 2008-08-20 21:08:32 UTC (rev 6962) @@ -41,6 +41,9 @@ /// Constructor Controller::Controller(Entity *entity ) { + this->nameP = new Param<std::string>("name","",1); + this->updatePeriodP = new Param<double>("updateRate", 10, 0); + if (!dynamic_cast<Model*>(entity) && !dynamic_cast<Sensor*>(entity)) { gzthrow("The parent of a controller must be a Model or a Sensor"); @@ -54,6 +57,8 @@ Controller::~Controller() { this->Fini(); + delete this->nameP; + delete this->updatePeriodP; } //////////////////////////////////////////////////////////////////////////////// @@ -65,9 +70,12 @@ if (!this->parent) gzthrow("Parent entity has not been set"); - this->name = node->GetString("name","",1); + this->typeName = node->GetName(); - this->updatePeriod = 1.0 / (node->GetDouble("updateRate", 10) + 1e-6); + this->nameP->Load(node); + this->updatePeriodP->Load(node); + this->updatePeriod = 1.0 / (this->updatePeriodP->GetValue() + 1e-6); + this->lastUpdate = -1e6; childNode = node->GetChildByNSPrefix("interface"); @@ -83,6 +91,9 @@ // Get the name of the iface std::string ifaceName = childNode->GetString("name","",1); + this->ifaceTypes.push_back(ifaceType); + this->ifaceNames.push_back(ifaceName); + try { // Use the factory to get a new iface based on the type @@ -113,12 +124,11 @@ if (this->ifaces.size() <= 0) { std::ostringstream stream; - stream << "No interface defined for " << this->name << " controller"; + stream << "No interface defined for " << this->GetName() << " controller"; gzthrow(stream.str()); } this->LoadChild(node); - this->xmlNode=node; } @@ -131,10 +141,28 @@ //////////////////////////////////////////////////////////////////////////////// /// Save the controller. -void Controller::Save() +void Controller::Save(std::string &prefix, std::ostream &stream) { - //So far the controller can not change in any way, not rewrite - this->SaveChild(this->xmlNode); + std::vector<std::string>::iterator iter1; + std::vector<std::string>::iterator iter2; + + stream << prefix << "<controller:" << this->typeName << " name=\"" << this->nameP->GetValue() << "\">\n"; + + stream << prefix << " " << *(this->updatePeriodP) << "\n"; + + // Ouptut the interfaces + for (iter1 = this->ifaceTypes.begin(), iter2=this->ifaceNames.begin(); + iter1 != this->ifaceTypes.end() && iter2 != this->ifaceNames.end(); + iter1++, iter2++) + { + stream << prefix << " <interface:" << *(iter1) << " name=\"" << *(iter2) << "\"/>\n"; + } + + std::string p = prefix + " "; + + this->SaveChild(p, stream); + + stream << prefix << "</controller:" << this->typeName << ">\n"; } //////////////////////////////////////////////////////////////////////////////// @@ -175,5 +203,5 @@ // Get the name of the controller std::string Controller::GetName() const { - return this->name; + return this->nameP->GetValue(); } Modified: code/gazebo/trunk/server/controllers/Controller.hh =================================================================== --- code/gazebo/trunk/server/controllers/Controller.hh 2008-08-18 19:48:42 UTC (rev 6961) +++ code/gazebo/trunk/server/controllers/Controller.hh 2008-08-20 21:08:32 UTC (rev 6962) @@ -30,6 +30,7 @@ #include <string> #include <vector> +#include "Param.hh" namespace gazebo { @@ -55,9 +56,9 @@ /// \param node The XMLConfig node pointer public: void Load(XMLConfigNode *node); - /// \brief Save the controller. - /// \param node The XMLConfigWriter node pointer - public: void Save(); + /// \brief Save the controller in XML format. + /// \param stream The output stream + public: void Save(std::string &prefix, std::ostream &stream); /// \brief Initialize the controller. Called once on startup. /// \return 0 on success @@ -77,7 +78,7 @@ protected: virtual void LoadChild(XMLConfigNode * /*node*/) {return;} /// \brief Save function for the child class - protected: virtual void SaveChild(XMLConfigNode * /*node*/) {return;} + protected: virtual void SaveChild(std::string &prefix, std::ostream &stream) {return;} /// \brief Init function for the child class protected: virtual void InitChild() {return;} @@ -96,21 +97,25 @@ public: std::string GetName() const; /// \brief The controller's name - protected: std::string name; + protected: Param<std::string> *nameP; /// \brief The entity that owns this controller protected: Entity *parent; /// \brief Update period protected: double updatePeriod; + protected: Param<double> *updatePeriodP; + private: std::string typeName; + /// \brief Last update time protected: double lastUpdate; /// \brief Array of all the iface for this controller protected: std::vector<Iface*> ifaces; - protected: XMLConfigNode *xmlNode; + private: std::vector< std::string > ifaceTypes; + private: std::vector< std::string > ifaceNames; }; /// \} Modified: code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.cc =================================================================== --- code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.cc 2008-08-18 19:48:42 UTC (rev 6961) +++ code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.cc 2008-08-20 21:08:32 UTC (rev 6962) @@ -55,6 +55,12 @@ // Destructor Bandit_Actarray::~Bandit_Actarray() { + for (int i=0; i<16; i++) + { + GZ_DELETE(this->jointNamesP[i]); + GZ_DELETE(this->forcesP[i]); + GZ_DELETE(this->gainsP[i]); + } } //////////////////////////////////////////////////////////////////////////////// @@ -71,24 +77,42 @@ for (i=0, jNode = node->GetChild("joint"); jNode; i++) { - std::string name = jNode->GetString("name","",1); + this->jointNamesP[i] = new Param<std::string>("name","",1); + this->jointNamesP[i]->Load(jNode); - this->joints[i] = dynamic_cast<HingeJoint*>(this->myParent->GetJoint(name)); - this->forces[i] = jNode->GetDouble("force",0.0,1); - this->gains[i] = jNode->GetDouble("gain",0.0,1); + this->forcesP[i] = new Param<float>("force",0.0,1); + this->forcesP[i]->Load(jNode); + this->gainsP[i] = new Param<float>("gain",0.0,1); + this->gainsP[i]->Load(jNode); + + this->joints[i] = dynamic_cast<HingeJoint*>(this->myParent->GetJoint(this->jointNamesP[i]->GetValue())); + jNode = jNode->GetNext("joint"); } } //////////////////////////////////////////////////////////////////////////////// +/// Save the controller. +void Bandit_Actarray::SaveChild(std::string &prefix, std::ostream &stream) +{ + for (int i=0; i<16; i++) + { + stream << prefix << "<joint name=\"" << this->jointNamesP[i]->GetValue() << "\">\n"; + stream << prefix << " " << *(this->forcesP[i]) << "\n"; + stream << prefix << " " << *(this->gainsP[i]) << "\n"; + stream << prefix << "</joint>\n"; + } +} + +//////////////////////////////////////////////////////////////////////////////// // Initialize the controller void Bandit_Actarray::InitChild() { for (int i=0; i<16; i++) { this->joints[i]->SetParam( dParamVel, 0.0); - this->joints[i]->SetParam( dParamFMax, this->forces[i] ); + this->joints[i]->SetParam( dParamFMax, **(this->forcesP[i]) ); } } @@ -126,14 +150,14 @@ if (fabs(angle) > 0.01) { - joint->SetParam( dParamVel, this->gains[i] * angle); - joint->SetParam( dParamFMax, this->forces[i] ); + joint->SetParam( dParamVel, **(this->gainsP[i]) * angle); + joint->SetParam( dParamFMax, **(this->forcesP[i]) ); } } else if (this->myIface->data->joint_mode[i] == GAZEBO_ACTARRAY_JOINT_SPEED_MODE) { joint->SetParam( dParamVel, cmdSpeed ); - joint->SetParam( dParamFMax, this->forces[i] ); + joint->SetParam( dParamFMax, **(this->forcesP[i]) ); } Modified: code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.hh =================================================================== --- code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.hh 2008-08-18 19:48:42 UTC (rev 6961) +++ code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.hh 2008-08-20 21:08:32 UTC (rev 6962) @@ -27,6 +27,7 @@ #ifndef BANDIT_ACTARRAY_HH #define BANDIT_ACTARRAY_HH +#include "Param.hh" #include "Controller.hh" #include "Entity.hh" @@ -67,6 +68,10 @@ /// \return 0 on success protected: virtual void LoadChild(XMLConfigNode *node); + /// \brief Save the controller. + /// \stream Output stream + protected: void SaveChild(std::string &prefix, std::ostream &stream); + /// Init the controller /// \return 0 on success protected: virtual void InitChild(); @@ -85,9 +90,10 @@ /// The parent Model private: Model *myParent; + private: Param<std::string> *jointNamesP[16]; private: HingeJoint *joints[16]; - private: float forces[16]; - private: float gains[16]; + private: Param<float> *forcesP[16]; + private: Param<float> *gainsP[16]; }; Modified: code/gazebo/trunk/server/controllers/camera/generic/Generic_Camera.cc =================================================================== --- code/gazebo/trunk/server/controllers/camera/generic/Generic_Camera.cc 2008-08-18 19:48:42 UTC (rev 6961) +++ code/gazebo/trunk/server/controllers/camera/generic/Generic_Camera.cc 2008-08-20 21:08:32 UTC (rev 6962) @@ -70,6 +70,13 @@ } //////////////////////////////////////////////////////////////////////////////// +/// Save the controller. +void Generic_Camera::SaveChild(std::string &prefix, std::ostream &stream) +{ + +} + +//////////////////////////////////////////////////////////////////////////////// // Initialize the controller void Generic_Camera::InitChild() { @@ -107,8 +114,8 @@ data->image_size = data->width * data->height * 3; // GetFOV() returns radians - data->hfov = this->myParent->GetHFOV(); - data->vfov = this->myParent->GetVFOV(); + data->hfov = *(this->myParent->GetHFOV()); + data->vfov = *(this->myParent->GetVFOV()); // Set the pose of the camera cameraPose = this->myParent->GetWorldPose(); Modified: code/gazebo/trunk/server/controllers/camera/generic/Generic_Camera.hh =================================================================== --- code/gazebo/trunk/server/controllers/camera/generic/Generic_Camera.hh 2008-08-18 19:48:42 UTC (rev 6961) +++ code/gazebo/trunk/server/controllers/camera/generic/Generic_Camera.hh 2008-08-20 21:08:32 UTC (rev 6962) @@ -78,6 +78,10 @@ /// \return 0 on success protected: virtual void LoadChild(XMLConfigNode *node); + /// \brief Save the controller. + /// \stream Output stream + protected: void SaveChild(std::string &prefix, std::ostream &stream); + /// \brief Init the controller /// \return 0 on success protected: virtual void InitChild(); Modified: code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc =================================================================== --- code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc 2008-08-18 19:48:42 UTC (rev 6961) +++ code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc 2008-08-20 21:08:32 UTC (rev 6962) @@ -49,6 +49,9 @@ { this->myParent = dynamic_cast<StereoCameraSensor*>(this->parent); + this->leftCameraNameP = new Param<std::string>("leftcamera","", 1); + this->rightCameraNameP = new Param<std::string>("rightcamera","", 1); + if (!this->myParent) gzthrow("Stereo_Camera controller requires a Stereo Camera Sensor as its parent"); } @@ -57,6 +60,8 @@ // Destructor Stereo_Camera::~Stereo_Camera() { + delete this->leftCameraNameP; + delete this->rightCameraNameP; } //////////////////////////////////////////////////////////////////////////////// @@ -76,14 +81,22 @@ } } - this->leftCameraName = node->GetString("leftcamera","", 1); - this->rightCameraName = node->GetString("rightcamera","", 1); + this->leftCameraNameP->Load(node); + this->rightCameraNameP->Load(node); if (!this->stereoIface) gzthrow("Stereo_Camera controller requires a StereoCameraIface"); } //////////////////////////////////////////////////////////////////////////////// +/// Save the controller. +void Stereo_Camera::SaveChild(std::string &prefix, std::ostream &stream) +{ + stream << prefix << *(this->leftCameraNameP) << "\n"; + stream << prefix << *(this->rightCameraNameP) << "\n"; +} + +//////////////////////////////////////////////////////////////////////////////// // Initialize the controller void Stereo_Camera::InitChild() { @@ -102,7 +115,7 @@ if (iter->second->data->head.openCount > 0) { - if (this->leftCameraName == iter->first) + if (**(this->leftCameraNameP) == iter->first) this->PutCameraData( iter->second->data, 0 ); else this->PutCameraData( iter->second->data, 1 ); @@ -148,8 +161,8 @@ stereo_data->farClip = this->myParent->GetFarClip(); stereo_data->nearClip = this->myParent->GetNearClip(); - stereo_data->hfov = this->myParent->GetHFOV(); - stereo_data->vfov = this->myParent->GetVFOV(); + stereo_data->hfov = *(this->myParent->GetHFOV()); + stereo_data->vfov = *(this->myParent->GetVFOV()); stereo_data->right_depth_size = stereo_data->width * stereo_data->height * sizeof(float); stereo_data->left_depth_size = stereo_data->width * stereo_data->height * sizeof(float); @@ -185,8 +198,8 @@ camera_data->image_size = camera_data->width * camera_data->height * 3; assert (camera_data->image_size <= sizeof(camera_data->image)); - camera_data->hfov = this->myParent->GetHFOV(); - camera_data->vfov = this->myParent->GetVFOV(); + camera_data->hfov = *(this->myParent->GetHFOV()); + camera_data->vfov = *(this->myParent->GetVFOV()); // Set the pose of the camera cameraPose = this->myParent->GetWorldPose(); Modified: code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.hh =================================================================== --- code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.hh 2008-08-18 19:48:42 UTC (rev 6961) +++ code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.hh 2008-08-20 21:08:32 UTC (rev 6962) @@ -30,6 +30,7 @@ #include <map> +#include "Param.hh" #include "Controller.hh" namespace gazebo @@ -79,6 +80,10 @@ /// \return 0 on success protected: virtual void LoadChild(XMLConfigNode *node); + /// \brief Save the controller. + /// \stream Output stream + protected: void SaveChild(std::string &prefix, std::ostream &stream); + /// \brief Init the controller /// \return 0 on success protected: virtual void InitChild(); @@ -101,8 +106,8 @@ private: StereoCameraIface *stereoIface; private: std::map< std::string, CameraIface*> cameraIfaces; - private: std::string leftCameraName; - private: std::string rightCameraName; + private: Param<std::string> *leftCameraNameP; + private: Param<std::string> *rightCameraNameP; /// The parent sensor private: StereoCameraSensor *myParent; Modified: code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.cc =================================================================== --- code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.cc 2008-08-18 19:48:42 UTC (rev 6961) +++ code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.cc 2008-08-20 21:08:32 UTC (rev 6962) @@ -67,11 +67,11 @@ if (!this->myIface) gzthrow("Pioneer2_Gripper controller requires a GripperIface"); - std::string leftJointName = node->GetString("leftJoint", "", 1); - std::string rightJointName = node->GetString("rightJoint", "", 1); + this->leftJointNameP = new Param<std::string>("leftJoint", "", 1); + this->rightJointNameP = new Param<std::string>("rightJoint", "", 1); - this->joints[LEFT] = dynamic_cast<SliderJoint*>(this->myParent->GetJoint(leftJointName)); - this->joints[RIGHT] = dynamic_cast<SliderJoint*>(this->myParent->GetJoint(rightJointName)); + this->joints[LEFT] = dynamic_cast<SliderJoint*>(this->myParent->GetJoint(this->leftJointNameP->GetValue())); + this->joints[RIGHT] = dynamic_cast<SliderJoint*>(this->myParent->GetJoint(this->rightJointNameP->GetValue())); if (!this->joints[LEFT]) gzthrow("couldn't get left slider joint"); @@ -82,6 +82,14 @@ } //////////////////////////////////////////////////////////////////////////////// +/// Save the controller +void Pioneer2_Gripper::SaveChild(std::string &prefix, std::ostream &stream) +{ + stream << prefix << *(this->leftJointNameP) << "\n"; + stream << prefix << *(this->rightJointNameP) << "\n"; +} + +//////////////////////////////////////////////////////////////////////////////// // Initialize the controller void Pioneer2_Gripper::InitChild() { Modified: code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.hh =================================================================== --- code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.hh 2008-08-18 19:48:42 UTC (rev 6961) +++ code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.hh 2008-08-20 21:08:32 UTC (rev 6962) @@ -28,6 +28,7 @@ #define PIONEER2_GRIPPER_HH +#include "Param.hh" #include "Controller.hh" #include "Entity.hh" @@ -70,6 +71,10 @@ /// \return 0 on success protected: virtual void LoadChild(XMLConfigNode *node); + /// \brief Save the controller. + /// \stream Output stream + protected: void SaveChild(std::string &prefix, std::ostream &stream); + /// Init the controller /// \return 0 on success protected: virtual void InitChild(); @@ -90,6 +95,8 @@ private: SliderJoint *joints[2]; + private: Param<std::string> *leftJointNameP; + private: Param<std::string> *rightJointNameP; }; /** \} */ Modified: code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.cc =================================================================== --- code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.cc 2008-08-18 19:48:42 UTC (rev 6961) +++ code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.cc 2008-08-20 21:08:32 UTC (rev 6962) @@ -139,8 +139,8 @@ double ra, rb, r, b; int v; - double maxAngle = this->myParent->GetMaxAngle(); - double minAngle = this->myParent->GetMinAngle(); + Angle maxAngle = this->myParent->GetMaxAngle(); + Angle minAngle = this->myParent->GetMinAngle(); double maxRange = this->myParent->GetMaxRange(); double minRange = this->myParent->GetMinRange(); @@ -153,9 +153,9 @@ this->laserIface->data->head.time = Simulator::Instance()->GetSimTime(); // Read out the laser range data - this->laserIface->data->min_angle = minAngle; - this->laserIface->data->max_angle = maxAngle; - this->laserIface->data->res_angle = (maxAngle - minAngle) / (rangeCount - 1); + this->laserIface->data->min_angle = minAngle.GetAsRadian(); + this->laserIface->data->max_angle = maxAngle.GetAsRadian(); + this->laserIface->data->res_angle = (maxAngle.GetAsRadian() - minAngle.GetAsRadian()) / (rangeCount - 1); this->laserIface->data->res_range = 0.1; this->laserIface->data->max_range = maxRange; this->laserIface->data->range_count = rangeCount; @@ -205,13 +205,11 @@ double r, b; double ax, ay, bx, by, cx, cy; - double maxAngle = this->myParent->GetMaxAngle(); - double minAngle = this->myParent->GetMinAngle(); + Angle maxAngle = this->myParent->GetMaxAngle(); + Angle minAngle = this->myParent->GetMinAngle(); - double maxRange = this->myParent->GetMaxRange(); double minRange = this->myParent->GetMinRange(); int rayCount = this->myParent->GetRayCount(); - int rangeCount = this->myParent->GetRangeCount(); if (this->fiducialIface->Lock(1)) { @@ -238,12 +236,12 @@ if (j - i + 1 >= 3) { r = minRange + this->myParent->GetRange(i); - b = minAngle + i * ((maxAngle-minAngle) / (rayCount - 1)); + b = minAngle.GetAsRadian() + i * ((maxAngle-minAngle).GetAsRadian() / (rayCount - 1)); ax = r * cos(b); ay = r * sin(b); r = minRange + this->myParent->GetRange(j); - b = minAngle + j * ((maxAngle-minAngle) / (rayCount - 1)); + b = minAngle.GetAsRadian() + j * ((maxAngle-minAngle).GetAsRadian() / (rayCount - 1)); bx = r * cos(b); by = r * sin(b); @@ -263,12 +261,12 @@ else { r = minRange + this->myParent->GetRange(i); - b = minAngle + i * ((maxAngle-minAngle) / (rayCount - 1)); + b = minAngle.GetAsRadian() + i * ((maxAngle-minAngle).GetAsRadian() / (rayCount - 1)); ax = r * cos(b); ay = r * sin(b); r = minRange + this->myParent->GetRange(j); - b = minAngle + j * ((maxAngle-minAngle) / (rayCount - 1)); + b = minAngle.GetAsRadian() + j * ((maxA... [truncated message content] |
From: <na...@us...> - 2008-08-20 14:53:24
|
Revision: 6965 http://playerstage.svn.sourceforge.net/playerstage/?rev=6965&view=rev Author: natepak Date: 2008-08-20 21:53:33 +0000 (Wed, 20 Aug 2008) Log Message: ----------- Added patch 2028314 by John Hsu Modified Paths: -------------- code/gazebo/trunk/libgazebo/Server.cc code/gazebo/trunk/server/physics/BallJoint.cc code/gazebo/trunk/server/physics/Hinge2Joint.cc code/gazebo/trunk/server/physics/HingeJoint.cc code/gazebo/trunk/server/physics/SliderJoint.cc code/gazebo/trunk/server/physics/UniversalJoint.cc code/gazebo/trunk/server/sensors/Sensor.cc code/gazebo/trunk/server/sensors/Sensor.hh code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.cc code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.hh code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh code/gazebo/trunk/server/sensors/ray/RaySensor.cc code/gazebo/trunk/server/sensors/ray/RaySensor.hh Modified: code/gazebo/trunk/libgazebo/Server.cc =================================================================== --- code/gazebo/trunk/libgazebo/Server.cc 2008-08-20 21:28:24 UTC (rev 6964) +++ code/gazebo/trunk/libgazebo/Server.cc 2008-08-20 21:53:33 UTC (rev 6965) @@ -95,36 +95,41 @@ // check to see if there is already a directory created. struct stat astat; - if (stat(this->filename.c_str(), &astat) == 0) { + if (stat(this->filename.c_str(), &astat) == 0) + { // directory already exists, check gazebo.pid to see if // another gazebo is already running. std::string pidfn = this->filename + "/gazebo.pid"; - + FILE *fp = fopen(pidfn.c_str(), "r"); - if(fp) { + if(fp) + { int pid; fscanf(fp, "%d", &pid); fclose(fp); std::cout << "found a pid file: pid=" << pid << "\n"; - if(kill(pid, 0) == 0) { - // a gazebo process is still alive. - errStream << "directory [" << this->filename - << "] already exists (previous crash?)\n" - << "gazebo (pid=" << pid << ") is still running."; - throw(errStream.str()); - } else { - // the gazebo process is not alive. - // remove directory. - std::cout << "The gazebo process is not alive.\n"; + if(kill(pid, 0) == 0) + { + // a gazebo process is still alive. + errStream << "directory [" << this->filename + << "] already exists (previous crash?)\n" + << "gazebo (pid=" << pid << ") is still running."; + throw(errStream.str()); + } + else + { + // the gazebo process is not alive. + // remove directory. + std::cout << "The gazebo process is not alive.\n"; - // remove the existing directory. - std::string cmd = "rm -rf '" + this->filename + "'"; - if(system(cmd.c_str()) != 0) { - errStream << "couldn't remove directory [" << this->filename << "]"; - throw(errStream.str()); - } + // remove the existing directory. + std::string cmd = "rm -rf '" + this->filename + "'"; + if(system(cmd.c_str()) != 0) { + errStream << "couldn't remove directory [" << this->filename << "]"; + throw(errStream.str()); + } } } } Modified: code/gazebo/trunk/server/physics/BallJoint.cc =================================================================== --- code/gazebo/trunk/server/physics/BallJoint.cc 2008-08-20 21:28:24 UTC (rev 6964) +++ code/gazebo/trunk/server/physics/BallJoint.cc 2008-08-20 21:53:33 UTC (rev 6965) @@ -34,7 +34,7 @@ BallJoint::BallJoint(dWorldID worldId) : Joint() { - this->type = BALL; + this->type = Joint::BALL; this->jointId = dJointCreateBall(worldId, NULL); } Modified: code/gazebo/trunk/server/physics/Hinge2Joint.cc =================================================================== --- code/gazebo/trunk/server/physics/Hinge2Joint.cc 2008-08-20 21:28:24 UTC (rev 6964) +++ code/gazebo/trunk/server/physics/Hinge2Joint.cc 2008-08-20 21:53:33 UTC (rev 6965) @@ -35,7 +35,7 @@ Hinge2Joint::Hinge2Joint( dWorldID worldId ) : Joint() { - this->type = HINGE2; + this->type = Joint::HINGE2; this->jointId = dJointCreateHinge2( worldId, NULL ); this->axis1P = new Param<Vector3>("axis1",Vector3(0,0,1), 0); Modified: code/gazebo/trunk/server/physics/HingeJoint.cc =================================================================== --- code/gazebo/trunk/server/physics/HingeJoint.cc 2008-08-20 21:28:24 UTC (rev 6964) +++ code/gazebo/trunk/server/physics/HingeJoint.cc 2008-08-20 21:53:33 UTC (rev 6965) @@ -37,7 +37,7 @@ HingeJoint::HingeJoint( dWorldID worldId ) : Joint() { - this->type = HINGE; + this->type = Joint::HINGE; this->jointId = dJointCreateHinge( worldId, NULL ); this->axisP = new Param<Vector3>("axis",Vector3(0,0,1), 0); Modified: code/gazebo/trunk/server/physics/SliderJoint.cc =================================================================== --- code/gazebo/trunk/server/physics/SliderJoint.cc 2008-08-20 21:28:24 UTC (rev 6964) +++ code/gazebo/trunk/server/physics/SliderJoint.cc 2008-08-20 21:53:33 UTC (rev 6965) @@ -34,7 +34,7 @@ SliderJoint::SliderJoint( dWorldID worldId ) : Joint() { - this->type = SLIDER; + this->type = Joint::SLIDER; this->jointId = dJointCreateSlider( worldId, NULL ); this->axisP = new Param<Vector3>("axis",Vector3(0,0,1), 0); Modified: code/gazebo/trunk/server/physics/UniversalJoint.cc =================================================================== --- code/gazebo/trunk/server/physics/UniversalJoint.cc 2008-08-20 21:28:24 UTC (rev 6964) +++ code/gazebo/trunk/server/physics/UniversalJoint.cc 2008-08-20 21:53:33 UTC (rev 6965) @@ -36,7 +36,7 @@ UniversalJoint::UniversalJoint( dWorldID worldId ) : Joint() { - this->type = UNIVERSAL; + this->type = Joint::UNIVERSAL; this->jointId = dJointCreateUniversal( worldId, NULL ); this->axis1P = new Param<Vector3>("axis1",Vector3(0,0,1),0); Modified: code/gazebo/trunk/server/sensors/Sensor.cc =================================================================== --- code/gazebo/trunk/server/sensors/Sensor.cc 2008-08-20 21:28:24 UTC (rev 6964) +++ code/gazebo/trunk/server/sensors/Sensor.cc 2008-08-20 21:53:33 UTC (rev 6965) @@ -31,6 +31,7 @@ #include "XMLConfig.hh" #include "World.hh" #include "ControllerFactory.hh" +#include "Simulator.hh" #include "Sensor.hh" using namespace gazebo; @@ -43,12 +44,15 @@ this->body = body; this->controller = NULL; this->active = true; + + this->updateRateP = new Param<double>("updateRate", 0, 0); } //////////////////////////////////////////////////////////////////////////////// // Destructor Sensor::~Sensor() { + delete this->updateRateP; } //////////////////////////////////////////////////////////////////////////////// @@ -56,6 +60,13 @@ void Sensor::Load(XMLConfigNode *node) { this->nameP->Load(node); + this->updateRateP->Load(node); + + if (**(this->updateRateP) == 0) + this->updatePeriod = 0.0; + else + this->updatePeriod = 1.0 / **(updateRateP); + this->LoadController( node->GetChildByNSPrefix("controller") ); this->LoadChild(node); } @@ -64,13 +75,26 @@ /// Save the sensor info in XML format void Sensor::Save(std::string &prefix, std::ostream &stream) { - + std::string p = prefix + " "; + + stream << prefix << "<sensor:" << this->typeName << " name=\"" << this->nameP->GetValue() << "\">\n"; + + stream << prefix << *(this->updateRateP) << "\n"; + + this->SaveChild(prefix, stream); + + if (this->controller) + this->controller->Save(p, stream); + + stream << prefix << "</sensor:" << this->typeName << ">\n"; } //////////////////////////////////////////////////////////////////////////////// /// Initialize the sensor void Sensor::Init() { + this->lastUpdate = Simulator::Instance()->GetSimTime(); + this->InitChild(); } @@ -78,7 +102,12 @@ /// Update the sensor void Sensor::Update() { - this->UpdateChild(); + if (this->lastUpdate + this->updatePeriod <= Simulator::Instance()->GetSimTime()) + { + this->UpdateChild(); + this->lastUpdate = Simulator::Instance()->GetSimTime(); + } + if (this->controller) this->controller->Update(); } Modified: code/gazebo/trunk/server/sensors/Sensor.hh =================================================================== --- code/gazebo/trunk/server/sensors/Sensor.hh 2008-08-20 21:28:24 UTC (rev 6964) +++ code/gazebo/trunk/server/sensors/Sensor.hh 2008-08-20 21:53:33 UTC (rev 6965) @@ -27,6 +27,7 @@ #ifndef SENSOR_HH #define SENSOR_HH +#include "Param.hh" #include "Entity.hh" namespace gazebo @@ -53,7 +54,10 @@ public: virtual void Load(XMLConfigNode *node); /// \brief Save the sensor info in XML format - public: virtual void Save(std::string &prefix, std::ostream &stream); + public: void Save(std::string &prefix, std::ostream &stream); + + /// \brief Child save function + protected: virtual void SaveChild(std::string &prefix,std::ostream &stream) {} /// \brief Initialize the sensor public: void Init(); @@ -91,6 +95,11 @@ /// \brief True if active protected: bool active; + + protected: Param<double> *updateRateP; + protected: double updatePeriod; + protected: double lastUpdate; + protected: std::string typeName; }; /// \} Modified: code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.cc =================================================================== --- code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.cc 2008-08-20 21:28:24 UTC (rev 6964) +++ code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.cc 2008-08-20 21:53:33 UTC (rev 6965) @@ -50,6 +50,7 @@ MonoCameraSensor::MonoCameraSensor(Body *body) : Sensor(body), OgreCamera("Mono") { + this->typeName = "monocamera"; } @@ -91,17 +92,10 @@ ////////////////////////////////////////////////////////////////////////////// /// Save the sensor info in XML format -void MonoCameraSensor::Save(std::string &prefix, std::ostream &stream) +void MonoCameraSensor::SaveChild(std::string &prefix, std::ostream &stream) { std::string p = prefix + " "; - stream << prefix << "<sensor:monocamera name=\"" << this->nameP->GetValue() << "\">\n"; - this->SaveCam(p, stream); - - if (this->controller) - this->controller->Save(p, stream); - - stream << prefix << "</sensor:monocamera>\n"; } ////////////////////////////////////////////////////////////////////////////// Modified: code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.hh =================================================================== --- code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.hh 2008-08-20 21:28:24 UTC (rev 6964) +++ code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.hh 2008-08-20 21:53:33 UTC (rev 6965) @@ -58,7 +58,7 @@ protected: virtual void LoadChild( XMLConfigNode *node ); /// \brief Save the sensor info in XML format - public: virtual void Save(std::string &prefix, std::ostream &stream); + protected: virtual void SaveChild(std::string &prefix, std::ostream &stream); /// \brief Initialize the camera protected: virtual void InitChild(); Modified: code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc =================================================================== --- code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc 2008-08-20 21:28:24 UTC (rev 6964) +++ code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc 2008-08-20 21:53:33 UTC (rev 6965) @@ -59,6 +59,8 @@ this->depthBuffer[1] = NULL; this->rgbBuffer[0] = NULL; this->rgbBuffer[1] = NULL; + + this->typeName = "stereocamera"; } @@ -87,18 +89,10 @@ ////////////////////////////////////////////////////////////////////////////// /// Save the sensor info in XML format -void StereoCameraSensor::Save(std::string &prefix, std::ostream &stream) +void StereoCameraSensor::SaveChild(std::string &prefix, std::ostream &stream) { std::string p = prefix + " "; - - stream << prefix << "<sensor:stereocamera name=\"" << this->nameP->GetValue() << "\">\n"; - this->SaveCam(p, stream); - - if (this->controller) - this->controller->Save(p, stream); - - stream << prefix << "</sensor:stereocamera>\n"; } ////////////////////////////////////////////////////////////////////////////// Modified: code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh =================================================================== --- code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh 2008-08-20 21:28:24 UTC (rev 6964) +++ code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh 2008-08-20 21:53:33 UTC (rev 6965) @@ -71,7 +71,7 @@ protected: virtual void LoadChild( XMLConfigNode *node ); /// \brief Save the sensor info in XML format - public: void Save(std::string &prefix, std::ostream &stream); + protected: virtual void SaveChild(std::string &prefix, std::ostream &stream); /// \brief Initialize the camera protected: virtual void InitChild(); Modified: code/gazebo/trunk/server/sensors/ray/RaySensor.cc =================================================================== --- code/gazebo/trunk/server/sensors/ray/RaySensor.cc 2008-08-20 21:28:24 UTC (rev 6964) +++ code/gazebo/trunk/server/sensors/ray/RaySensor.cc 2008-08-20 21:53:33 UTC (rev 6965) @@ -53,6 +53,8 @@ { this->active = false; + this->typeName = "ray"; + this->rayCountP = new Param<int>("rayCount",0,1); this->rangeCountP = new Param<int>("rangeCount",0,1); this->minAngleP = new Param<Angle>("minAngle",DTOR(-90),1); @@ -114,10 +116,8 @@ ////////////////////////////////////////////////////////////////////////////// /// Save the sensor info in XML format -void RaySensor::Save(std::string &prefix, std::ostream &stream) +void RaySensor::SaveChild(std::string &prefix, std::ostream &stream) { - std::string p = prefix + " "; - stream << prefix << "<sensor:ray name=\"" << this->nameP->GetValue() << "\">\n"; stream << prefix << " " << *(this->minAngleP) << "\n"; stream << prefix << " " << *(this->maxAngleP) << "\n"; stream << prefix << " " << *(this->minRangeP) << "\n"; @@ -126,11 +126,6 @@ stream << prefix << " " << *(this->rayCountP) << "\n"; stream << prefix << " " << *(this->rangeCountP) << "\n"; stream << prefix << " " << *(this->displayRaysP) << "\n"; - - if (this->controller) - this->controller->Save(p, stream); - - stream << prefix << "</sensor:ray>\n"; } ////////////////////////////////////////////////////////////////////////////// Modified: code/gazebo/trunk/server/sensors/ray/RaySensor.hh =================================================================== --- code/gazebo/trunk/server/sensors/ray/RaySensor.hh 2008-08-20 21:28:24 UTC (rev 6964) +++ code/gazebo/trunk/server/sensors/ray/RaySensor.hh 2008-08-20 21:53:33 UTC (rev 6965) @@ -65,7 +65,7 @@ protected: virtual void LoadChild(XMLConfigNode *node); /// \brief Save the sensor info in XML format - public: void Save(std::string &prefix, std::ostream &stream); + protected: virtual void SaveChild(std::string &prefix, std::ostream &stream); /// Initialize the ray protected: virtual void InitChild(); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-08-20 16:18:53
|
Revision: 6970 http://playerstage.svn.sourceforge.net/playerstage/?rev=6970&view=rev Author: natepak Date: 2008-08-20 23:19:02 +0000 (Wed, 20 Aug 2008) Log Message: ----------- Added Angle, Param, Vector4 classes Modified Paths: -------------- code/gazebo/trunk/worlds/bandit.world code/gazebo/trunk/worlds/models/bandit.model Added Paths: ----------- code/gazebo/trunk/server/Angle.cc code/gazebo/trunk/server/Angle.hh code/gazebo/trunk/server/Param.hh code/gazebo/trunk/server/Vector4.cc code/gazebo/trunk/server/Vector4.hh Added: code/gazebo/trunk/server/Angle.cc =================================================================== --- code/gazebo/trunk/server/Angle.cc (rev 0) +++ code/gazebo/trunk/server/Angle.cc 2008-08-20 23:19:02 UTC (rev 6970) @@ -0,0 +1,194 @@ +/* + * 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: Angle class + * Author: Nate Koenig + * Date: 18 Aug 2008 + * SVN: $Id:$ + */ + +#include <math.h> +#include "Angle.hh" + +using namespace gazebo; + +//////////////////////////////////////////////////////////////////////////////// +/// Constructor +Angle::Angle() +{ + this->value = 0; +} + +//////////////////////////////////////////////////////////////////////////////// +// Constructor +Angle::Angle(double radian) +{ + this->value = radian; +} + +//////////////////////////////////////////////////////////////////////////////// +/// Copy constructor +Angle::Angle(const Angle &angle) +{ + this->value = angle.value; +} + +//////////////////////////////////////////////////////////////////////////////// +/// Destructor +Angle::~Angle() +{ +} + +//////////////////////////////////////////////////////////////////////////////// +/// Set the value from an angle in radians +void Angle::SetFromRadian( double radian ) +{ + this->value = radian; +} + +//////////////////////////////////////////////////////////////////////////////// +/// Set the value from an angle in degrees +void Angle::SetFromDegree( double degree ) +{ + this->value = degree * M_PI / 180.0; +} + +//////////////////////////////////////////////////////////////////////////////// +/// Get the angle in radians +double Angle::GetAsRadian() const +{ + return this->value; +} + +//////////////////////////////////////////////////////////////////////////////// +/// Get the angle in degrees +double Angle::GetAsDegree() const +{ + return this->value * 180.0 / M_PI; +} + +//////////////////////////////////////////////////////////////////////////////// +// Normalize the angle +void Angle::Normalize() +{ + this->value = atan2(sin(this->value), cos(this->value)); +} + +//////////////////////////////////////////////////////////////////////////////// +/// Substraction +Angle Angle::operator-(const Angle &angle) const +{ + return Angle(this->value - angle.value); +} + +//////////////////////////////////////////////////////////////////////////////// +/// Addition +Angle Angle::operator+(const Angle &angle) const +{ + return Angle(this->value + angle.value); +} + +//////////////////////////////////////////////////////////////////////////////// +/// Multiplication +Angle Angle::operator*(const Angle &angle) const +{ + return Angle(this->value * angle.value); +} + +//////////////////////////////////////////////////////////////////////////////// +/// Division +Angle Angle::operator/(const Angle &angle) const +{ + return Angle(this->value / angle.value); +} + +//////////////////////////////////////////////////////////////////////////////// +/// Add set +Angle Angle::operator-=(const Angle &angle) +{ + this->value -= angle.value; + return *this; +} + +//////////////////////////////////////////////////////////////////////////////// +/// Sub set +Angle Angle::operator+=(const Angle &angle) +{ + this->value += angle.value; + return *this; +} + +//////////////////////////////////////////////////////////////////////////////// +/// Mul set +Angle Angle::operator*=(const Angle &angle) +{ + this->value *= angle.value; + return *this; +} + +//////////////////////////////////////////////////////////////////////////////// +/// Div set +Angle Angle::operator/=(const Angle &angle) +{ + this->value /= angle.value; + return *this; +} + +//////////////////////////////////////////////////////////////////////////////// +/// Equality +bool Angle::operator==(const Angle &angle) const +{ + return this->value == angle.value; +} + +//////////////////////////////////////////////////////////////////////////////// +/// Inequality +bool Angle::operator!=(const Angle &angle) const +{ + return !(*this == angle); +} + +//////////////////////////////////////////////////////////////////////////////// +/// Less +bool Angle::operator<(const Angle &angle) const +{ + return this->value < angle.value; +} + +//////////////////////////////////////////////////////////////////////////////// +/// Less equal +bool Angle::operator<=(const Angle &angle) const +{ + return this->value <= angle.value; +} + +//////////////////////////////////////////////////////////////////////////////// +/// Greater +bool Angle::operator>(const Angle &angle) const +{ + return this->value > angle.value; +} + +//////////////////////////////////////////////////////////////////////////////// +/// Greater equal +bool Angle::operator>=(const Angle &angle) const +{ + return this->value >= angle.value; +} Added: code/gazebo/trunk/server/Angle.hh =================================================================== --- code/gazebo/trunk/server/Angle.hh (rev 0) +++ code/gazebo/trunk/server/Angle.hh 2008-08-20 23:19:02 UTC (rev 6970) @@ -0,0 +1,136 @@ +/* + * 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: Angle class + * Author: Nate Koenig + * Date: 18 Aug 2008 + * SVN: $Id:$ + */ + +#ifndef ANGLE_HH +#define ANGLE_HH + +#include <iostream> + +namespace gazebo +{ + +/// \addtogroup gazebo_server +/// \brief Angle class +/// \{ + +/// \brief Angle class + class Angle + { + /// \brief Constructor + public: Angle(); + + /// \brief Constructor + public: Angle(double radian); + + /// \brief Copy constructor + public: Angle(const Angle &angle); + + /// \brief Destructor + public: virtual ~Angle(); + + /// \brief Set the value from an angle in radians + public: void SetFromRadian( double radian ); + + /// \brief Set the value from an angle in degrees + public: void SetFromDegree( double degree ); + + /// \brief Get the angle in radians + public: double GetAsRadian() const; + + /// \brief Get the angle in degrees + public: double GetAsDegree() const; + + /// \brief Normalize the angle + public: void Normalize(); + + /// \brief Dereference operator + public: inline double operator*() { return value; } + + /// \brief Substraction + public: Angle operator-(const Angle &angle) const; + /// \brief Addition + public: Angle operator+(const Angle &angle) const; + /// \brief Multiplication + public: Angle operator*(const Angle &angle) const; + /// \brief Division + public: Angle operator/(const Angle &angle) const; + + /// \brief Add set + public: Angle operator-=(const Angle &angle); + /// \brief Sub set + public: Angle operator+=(const Angle &angle); + /// \brief Mul set + public: Angle operator*=(const Angle &angle); + /// \brief Div set + public: Angle operator/=(const Angle &angle); + + /// \brief Equality + public: bool operator==(const Angle &angle) const; + + /// \brief Inequality + public: bool operator!=(const Angle &angle) const; + + /// \brief Less + public: bool operator<(const Angle &angle) const; + /// \brief Less equal + public: bool operator<=(const Angle &angle) const; + + /// \brief Greater + public: bool operator>(const Angle &angle) const; + /// \brief Greater equal + public: bool operator>=(const Angle &angle) const; + + /// \brief Ostream operator. Outputs in degrees + /// \param out Ostream + /// \param pt Angle to output + /// \return The Ostream + public: friend std::ostream &operator<<( std::ostream &out, const gazebo::Angle &a ) + { + out << a.GetAsDegree(); + return out; + } + + /// \brief Istream operator. Assumes input is in degrees + /// \param in Ostream + /// \param pt Angle to read value into + /// \return The istream + public: friend std::istream &operator>>( std::istream &in, gazebo::Angle &a ) + { + // Skip white spaces + in.setf( std::ios_base::skipws ); + in >> a.value; + a.value = a.value * M_PI / 180.0; + return in; + } + + /// The angle in radians + private: double value; + }; + +/// \} +} + +#endif Added: code/gazebo/trunk/server/Param.hh =================================================================== --- code/gazebo/trunk/server/Param.hh (rev 0) +++ code/gazebo/trunk/server/Param.hh 2008-08-20 23:19:02 UTC (rev 6970) @@ -0,0 +1,136 @@ +/* + * 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 parameter + * Author: Nate Koenig + * Date: 14 Aug 2008 + * SVN: $Id:$ + */ + +#ifndef PARAM_HH +#define PARAM_HH + +#include <iostream> +#include <boost/lexical_cast.hpp> +#include <string> + +#include "XMLConfig.hh" + +namespace gazebo +{ + template< typename T > + class Param + { + /// \brief Constructor + public: Param(std::string key, T defValue, int required); + + /// \brief Destructor + public: virtual ~Param(); + + /// \brief Load the param from an XML config file + public: void Load(XMLConfigNode *node); + + /// \brief Get the value + public: T GetValue() const; + + /// \brief Set the value of the parameter + public: void SetValue(const T &value); + + public: inline T operator*() const {return value;} + + public: friend std::ostream &operator<<( std::ostream &out, const Param<T> &p) + { + out << "<" << p.key << ">" << p.value << "</" << p.key << ">"; + + return out; + } + + private: T value; + + private: std::string key; + private: T defaultValue; + private: int required; + + }; + + + ////////////////////////////////////////////////////////////////////////////// + // Constructor + template< typename T> + Param<T>::Param(std::string key, T defValue, int required) + { + this->key = key; + this->defaultValue = defValue; + this->required = required; + this->value = this->defaultValue; + } + + ////////////////////////////////////////////////////////////////////////////// + // Destructor + template<typename T> + Param<T>::~Param() + { + } + + ////////////////////////////////////////////////////////////////////////////// + /// Load the param from an XML config file + template<typename T> + void Param<T>::Load(XMLConfigNode *node) + { + std::ostringstream stream; + stream << this->defaultValue; + + std::string input = node->GetString(this->key, stream.str(), + this->required); + + // "true" and "false" doesn't work properly + if (input == "true") + input = "1"; + else if (input == "false") + input = "0"; + + try + { + this->value = boost::lexical_cast<T>(input); + } + catch (boost::bad_lexical_cast &e) + { + std::cerr << "Unable to read value with key[" << this->key << "]\n"; + } + } + + ////////////////////////////////////////////////////////////////////////////// + ///Get the value + template<typename T> + T Param<T>::GetValue() const + { + return this->value; + } + + ////////////////////////////////////////////////////////////////////////////// + /// Set the value of the parameter + template<typename T> + void Param<T>::SetValue(const T &v) + { + this->value = v; + } + +} +#endif Added: code/gazebo/trunk/server/Vector4.cc =================================================================== --- code/gazebo/trunk/server/Vector4.cc (rev 0) +++ code/gazebo/trunk/server/Vector4.cc 2008-08-20 23:19:02 UTC (rev 6970) @@ -0,0 +1,273 @@ +/* + * 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: Vector 4 + * Author: Andrew Howard and Nate Koenig + * Date: 4 Apr 2007 + * SVN: $Id:$ + */ + +#include <math.h> + +#include "Vector4.hh" + +using namespace gazebo; + +//////////////////////////////////////////////////////////////////////////////// +// Constructor +Vector4::Vector4() + : x(0), y(0), z(0), w(0) +{ +} + +//////////////////////////////////////////////////////////////////////////////// +// Constructor +Vector4::Vector4( const double &x, const double &y, const double &z,const double &w ) + : x(x), y(y), z(z), w(w) +{ +} + +//////////////////////////////////////////////////////////////////////////////// +// Copy Constructor +Vector4::Vector4( const Vector4 &pt ) + : x(pt.x), y(pt.y), z(pt.z), w(pt.w) +{ +} + +//////////////////////////////////////////////////////////////////////////////// +// Destructor +Vector4::~Vector4() +{ +} + +//////////////////////////////////////////////////////////////////////////////// +// Calc distance to the given point +double Vector4::Distance(const Vector4 &pt ) const +{ + return sqrt((this->x-pt.x)*(this->x-pt.x) + + (this->y-pt.y)*(this->y-pt.y) + + (this->z-pt.z)*(this->z-pt.z) + + (this->w-pt.w)*(this->w-pt.w)); +} + +//////////////////////////////////////////////////////////////////////////////// +// Returns the length (magnitude) of the vector +double Vector4::GetLength() const +{ + return sqrt(this->x * this->x + this->y * this->y + this->z * this->z + this->w * this->w); +} + +//////////////////////////////////////////////////////////////////////////////// +// Return the square of the length (magnitude) of the vector +double Vector4::GetSquaredLength() const +{ + return this->x * this->x + this->y * this->y + this->z * this->z + this->w * this->w; +} + +//////////////////////////////////////////////////////////////////////////////// +// Normalize the vector length +void Vector4::Normalize() +{ + double d = this->GetLength(); + + this->x /= d; + this->y /= d; + this->z /= d; + this->w /= d; +} + +//////////////////////////////////////////////////////////////////////////////// +// Set the contents of the vector +void Vector4::Set(double x, double y, double z, double w) +{ + this->x = x; + this->y = y; + this->z = z; + this->w = w; +} + + +//////////////////////////////////////////////////////////////////////////////// +// Equals operator +const Vector4 &Vector4::operator=( const Vector4 &pt ) +{ + this->x = pt.x; + this->y = pt.y; + this->z = pt.z; + this->w = pt.w; + + return *this; +} + +//////////////////////////////////////////////////////////////////////////////// +/// Equal operator +const Vector4 &Vector4::operator=( double value ) +{ + this->x = value; + this->y = value; + this->z = value; + this->w = value; + + return *this; +} + + + +//////////////////////////////////////////////////////////////////////////////// +// Addition operator +Vector4 Vector4::operator+( const Vector4 &pt ) const +{ + return Vector4(this->x + pt.x, this->y + pt.y, this->z + pt.z, this->w+pt.w); +} + +const Vector4 &Vector4::operator+=( const Vector4 &pt ) +{ + this->x += pt.x; + this->y += pt.y; + this->z += pt.z; + this->w += pt.w; + + return *this; +} + +//////////////////////////////////////////////////////////////////////////////// +// Subtraction operators +Vector4 Vector4::operator-( const Vector4 &pt ) const +{ + return Vector4(this->x - pt.x, this->y - pt.y, this->z - pt.z, this->w-pt.w); +} + +const Vector4 &Vector4::operator-=( const Vector4 &pt ) +{ + this->x -= pt.x; + this->y -= pt.y; + this->z -= pt.z; + this->w -= pt.w; + + return *this; +} + + +//////////////////////////////////////////////////////////////////////////////// +// Division operators + +const Vector4 Vector4::operator/( const Vector4 &pt ) const +{ + return Vector4(this->x / pt.x, this->y / pt.y, this->z / pt.z, this->w/pt.w); +} + +const Vector4 &Vector4::operator/=( const Vector4 &pt ) +{ + this->x /= pt.x; + this->y /= pt.y; + this->z /= pt.z; + this->w /= pt.w; + + return *this; +} + +const Vector4 Vector4::operator/( double v ) const +{ + return Vector4(this->x / v, this->y / v, this->z / v, this->w / v); +} + +const Vector4 &Vector4::operator/=( double v ) +{ + this->x /= v; + this->y /= v; + this->z /= v; + this->w /= v; + + return *this; +} + + + +//////////////////////////////////////////////////////////////////////////////// +// Mulitplication operators +const Vector4 Vector4::operator*( const Vector4 &pt ) const +{ + return Vector4(this->x * pt.x, this->y * pt.y, this->z * pt.z, this->w*pt.w); +} + +const Vector4 &Vector4::operator*=( const Vector4 &pt ) +{ + this->x *= pt.x; + this->y *= pt.y; + this->z *= pt.z; + this->w *= pt.w; + + return *this; +} + +const Vector4 Vector4::operator*( double v ) const +{ + return Vector4(this->x * v, this->y * v, this->z * v, this->w*v); +} + +const Vector4 &Vector4::operator*=( double v) +{ + this->x *= v; + this->y *= v; + this->z *= v; + this->w *= v; + + return *this; +} + +//////////////////////////////////////////////////////////////////////////////// +// Equality operator +bool Vector4::operator==( const Vector4 &pt ) const +{ + return this->x == pt.x && this->y == pt.y && this->z == pt.z && this->w==pt.w; +} + +//////////////////////////////////////////////////////////////////////////////// +// Inequality operator +bool Vector4::operator!=( const Vector4 &pt ) const +{ + return !(*this == pt); +} + +//////////////////////////////////////////////////////////////////////////////// +// See if a point is finite (e.g., not nan) +bool Vector4::IsFinite() const +{ + return finite(this->x) && finite(this->y) && finite(this->z) && finite(this->w); +} + +//////////////////////////////////////////////////////////////////////////////// +/// [] operator +double Vector4::operator[](unsigned int index) const +{ + switch (index) + { + case 0: + return this->x; + case 1: + return this->y; + case 2: + return this->z; + case 3: + return this->w; + default: + return 0; + } +} Added: code/gazebo/trunk/server/Vector4.hh =================================================================== --- code/gazebo/trunk/server/Vector4.hh (rev 0) +++ code/gazebo/trunk/server/Vector4.hh 2008-08-20 23:19:02 UTC (rev 6970) @@ -0,0 +1,163 @@ +/* + * 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: 4 tuple + * Author: Nate Koenig + * Date: 19 Aug 2008 + * SVN: $Id:$ + */ + +#ifndef VECTOR4_HH +#define VECTOR4_HH + +#include <iostream> +#include <fstream> + +namespace gazebo +{ +/// \addtogroup gazebo_server +/// \brief Generic x,y,z,w vector +/// \{ + +/// \brief Generic x,y,z,w vector +class Vector4 +{ + /// \brief Constructor + public: Vector4(); + + /// \brief Constructor + public: Vector4( const double &x, const double &y, const double &z, const double &w ); + + /// \brief Constructor + public: Vector4( const Vector4 &pt ); + + /// \brief Destructor + public: virtual ~Vector4(); + + /// \brief Calc distance to the given point + public: double Distance( const Vector4 &pt ) const; + + /// \brief Returns the length (magnitude) of the vector + public: double GetLength() const; + + /// \brief Return the square of the length (magnitude) of the vector + public: double GetSquaredLength() const; + + /// \brief Normalize the vector length + public: void Normalize(); + + /// \brief Set the contents of the vector + public: void Set(double x = 0, double y =0 , double z = 0, double w=0); + + /// \brief Equal operator + public: const Vector4 &operator=( const Vector4 &pt ); + + /// \brief Equal operator + public: const Vector4 &operator=( double value ); + + /// \brief Addition operator + public: Vector4 operator+( const Vector4 &pt ) const; + + /// \brief Addition operator + public: const Vector4 &operator+=( const Vector4 &pt ); + + /// \brief Subtraction operators + public: Vector4 operator-( const Vector4 &pt ) const; + + /// \brief Subtraction operators + public: const Vector4 &operator-=( const Vector4 &pt ); + + /// \brief Division operators + public: const Vector4 operator/( const Vector4 &pt ) const; + + /// \brief Division operators + public: const Vector4 &operator/=( const Vector4 &pt ); + + /// \brief Division operators + public: const Vector4 operator/( double v ) const; + + /// \brief Division operators + public: const Vector4 &operator/=( double v ); + + /// \brief Multiplication operators + public: const Vector4 operator*( const Vector4 &pt ) const; + + /// \brief Multiplication operators + public: const Vector4 &operator*=( const Vector4 &pt ); + + /// \brief Multiplication operators + public: const Vector4 operator*( double v ) const; + + /// \brief Multiplication operators + public: const Vector4 &operator*=( double v ); + + /// \brief Equality operators + public: bool operator==( const Vector4 &pt ) const; + + /// \brief Equality operators + public: bool operator!=( const Vector4 &pt ) const; + + /// \brief See if a point is finite (e.g., not nan) + public: bool IsFinite() const; + + /// \brief [] operator + public: double operator[](unsigned int index) const; + + /// X value + public: double x; + + /// Y value + public: double y; + + /// Z value + public: double z; + + /// W value + public: double w; + + /// \brief Ostream operator + /// \param out Ostream + /// \param pt Vector4 to output + /// \return The Ostream + public: friend std::ostream &operator<<( std::ostream &out, const gazebo::Vector4 &pt ) + { + out << pt.x << " " << pt.y << " " << pt.z << " " << pt.w; + + return out; + } + + /// \brief Istream operator + /// \param in Ostream + /// \param pt Vector4 to read values into + /// \return The istream + public: friend std::istream &operator>>( std::istream &in, gazebo::Vector4 &pt ) + { + // Skip white spaces + in.setf( std::ios_base::skipws ); + in >> pt.x >> pt.y >> pt.z >> pt.w; + return in; + } + +}; + +/// \} +} + +#endif Modified: code/gazebo/trunk/worlds/bandit.world =================================================================== --- code/gazebo/trunk/worlds/bandit.world 2008-08-20 22:36:51 UTC (rev 6969) +++ code/gazebo/trunk/worlds/bandit.world 2008-08-20 23:19:02 UTC (rev 6970) @@ -16,7 +16,7 @@ <verbosity>5</verbosity> <physics:ode> - <stepTime>0.03</stepTime> + <stepTime>0.3</stepTime> <gravity>0 0 -9.8</gravity> <cfm>10e-5</cfm> <erp>0.3</erp> Modified: code/gazebo/trunk/worlds/models/bandit.model =================================================================== --- code/gazebo/trunk/worlds/models/bandit.model 2008-08-20 22:36:51 UTC (rev 6969) +++ code/gazebo/trunk/worlds/models/bandit.model 2008-08-20 23:19:02 UTC (rev 6970) @@ -100,7 +100,6 @@ <lowStop>-15</lowStop> <highStop>15</highStop> <erp>0.1</erp> - <cfm>10e-5</cfm> </joint:hinge> <body:box name="torso_body"> @@ -127,7 +126,6 @@ <lowStop>-90</lowStop> <highStop>90</highStop> <erp>0.1</erp> - <cfm>10e-5</cfm> </joint:hinge> <body:box name="right_shoulder_mounting_body"> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-08-21 05:54:10
|
Revision: 6974 http://playerstage.svn.sourceforge.net/playerstage/?rev=6974&view=rev Author: natepak Date: 2008-08-21 12:54:20 +0000 (Thu, 21 Aug 2008) Log Message: ----------- Minor updates to collision handling Modified Paths: -------------- code/gazebo/trunk/build.py code/gazebo/trunk/server/Global.hh code/gazebo/trunk/server/physics/ContactParams.cc code/gazebo/trunk/server/physics/ode/ODEPhysics.cc Modified: code/gazebo/trunk/build.py =================================================================== --- code/gazebo/trunk/build.py 2008-08-20 23:56:26 UTC (rev 6973) +++ code/gazebo/trunk/build.py 2008-08-21 12:54:20 UTC (rev 6974) @@ -5,7 +5,7 @@ import os import re -version = '0.8-pre2' +version = '0.8-pre3' # # Function used to test for ODE library, and TriMesh support Modified: code/gazebo/trunk/server/Global.hh =================================================================== --- code/gazebo/trunk/server/Global.hh 2008-08-20 23:56:26 UTC (rev 6973) +++ code/gazebo/trunk/server/Global.hh 2008-08-21 12:54:20 UTC (rev 6974) @@ -45,7 +45,7 @@ #endif // TODO: Fix the version number -#define GAZEBO_VERSION "0.8-pre2" +#define GAZEBO_VERSION "0.8-pre3" #ifndef GZ_COLLIDE_BITS Modified: code/gazebo/trunk/server/physics/ContactParams.cc =================================================================== --- code/gazebo/trunk/server/physics/ContactParams.cc 2008-08-20 23:56:26 UTC (rev 6973) +++ code/gazebo/trunk/server/physics/ContactParams.cc 2008-08-21 12:54:20 UTC (rev 6974) @@ -43,8 +43,8 @@ this->mu1 = dInfinity; this->mu2 = dInfinity; - this->slip1 = 0.01; - this->slip2 = 0.01; + this->slip1 = 0.1; + this->slip2 = 0.1; } ////////////////////////////////////////////////////////////////////////////// Modified: code/gazebo/trunk/server/physics/ode/ODEPhysics.cc =================================================================== --- code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2008-08-20 23:56:26 UTC (rev 6973) +++ code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2008-08-21 12:54:20 UTC (rev 6974) @@ -256,8 +256,14 @@ { double h, kp, kd; + // skip 0 depth contacts + if(contactGeoms[i].depth == 0) + continue; + contact.geom = contactGeoms[i]; - contact.surface.mode = dContactSoftERP | dContactBounce | dContactMu2 | dContactApprox1; + contact.surface.mode = dContactSlip1 | dContactSlip2 | + dContactSoftERP | dContactSoftCFM | + dContactBounce | dContactMu2 | dContactApprox1; // Compute the CFM and ERP by assuming the two bodies form a @@ -268,13 +274,19 @@ contact.surface.soft_erp = h * kp / (h * kp + kd); contact.surface.soft_cfm = 1 / (h * kp + kd); - contact.surface.mu = MIN(geom1->contact->mu1, geom2->contact->mu1); contact.surface.mu2 = MIN(geom1->contact->mu2, geom2->contact->mu2); - contact.surface.bounce = 0.1; - contact.surface.bounce_vel = 0.1; - //contact.surface.soft_cfm = 0.01; + contact.surface.bounce = MIN(geom1->contact->bounce, + geom2->contact->bounce); + contact.surface.bounce_vel = MIN(geom1->contact->bounceVel, + geom2->contact->bounceVel); + contact.surface.slip1 = MIN(geom1->contact->slip1, + geom2->contact->slip1); + contact.surface.slip2 = MIN(geom1->contact->slip2, + geom2->contact->slip2); + + dJointID c = dJointCreateContact (self->worldId, self->contactGroup, &contact); dJointAttach (c,b1,b2); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-08-28 08:49:12
|
Revision: 6986 http://playerstage.svn.sourceforge.net/playerstage/?rev=6986&view=rev Author: natepak Date: 2008-08-28 15:49:20 +0000 (Thu, 28 Aug 2008) Log Message: ----------- Updates to the bandit model Modified Paths: -------------- code/gazebo/trunk/examples/libgazebo/position/position.cc code/gazebo/trunk/server/controllers/Controller.cc code/gazebo/trunk/server/controllers/Controller.hh code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.cc code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.hh code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.hh code/gazebo/trunk/server/physics/Body.cc code/gazebo/trunk/server/physics/Body.hh code/gazebo/trunk/server/physics/Joint.cc code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.cc code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh code/gazebo/trunk/worlds/bandit.world code/gazebo/trunk/worlds/models/bandit.model code/gazebo/trunk/worlds/simpleshapes.world Added Paths: ----------- code/gazebo/trunk/Media/models/bandit/left_eyebrow.mesh code/gazebo/trunk/Media/models/bandit/lowerlip.mesh code/gazebo/trunk/Media/models/bandit/right_eyebrow.mesh code/gazebo/trunk/Media/models/bandit/upperlip.mesh code/gazebo/trunk/examples/libgazebo/bandit/ code/gazebo/trunk/examples/libgazebo/bandit/SConstruct code/gazebo/trunk/examples/libgazebo/bandit/bandit.cc Property changes on: code/gazebo/trunk/Media/models/bandit/left_eyebrow.mesh ___________________________________________________________________ Added: svn:mime-type + application/octet-stream Property changes on: code/gazebo/trunk/Media/models/bandit/lowerlip.mesh ___________________________________________________________________ Added: svn:mime-type + application/octet-stream Property changes on: code/gazebo/trunk/Media/models/bandit/right_eyebrow.mesh ___________________________________________________________________ Added: svn:mime-type + application/octet-stream Property changes on: code/gazebo/trunk/Media/models/bandit/upperlip.mesh ___________________________________________________________________ Added: svn:mime-type + application/octet-stream Added: code/gazebo/trunk/examples/libgazebo/bandit/SConstruct =================================================================== --- code/gazebo/trunk/examples/libgazebo/bandit/SConstruct (rev 0) +++ code/gazebo/trunk/examples/libgazebo/bandit/SConstruct 2008-08-28 15:49:20 UTC (rev 6986) @@ -0,0 +1,16 @@ + +# 3rd party packages +parseConfigs=['pkg-config --cflags --libs libgazebo', + 'pkg-config --cflags --libs libgazeboServer'] + + +env = Environment ( + CC = 'g++', + CCFLAGS = Split ('-pthread -pipe -W -Wall -O2'), +) + +# Parse all the package configurations +for cfg in parseConfigs: + env.ParseConfig(cfg) + +env.Program('bandit','bandit.cc') Added: code/gazebo/trunk/examples/libgazebo/bandit/bandit.cc =================================================================== --- code/gazebo/trunk/examples/libgazebo/bandit/bandit.cc (rev 0) +++ code/gazebo/trunk/examples/libgazebo/bandit/bandit.cc 2008-08-28 15:49:20 UTC (rev 6986) @@ -0,0 +1,59 @@ +#include <gazebo/gazebo.h> +#include <gazebo/GazeboError.hh> + +int main() +{ + gazebo::Client *client = new gazebo::Client(); + gazebo::SimulationIface *simIface = new gazebo::SimulationIface(); + gazebo::ActarrayIface *actarrayIface = new gazebo::ActarrayIface(); + + int serverId = 0; + + /// Connect to the libgazebo server + try + { + client->ConnectWait(serverId, GZ_CLIENT_ID_USER_FIRST); + } + catch (gazebo::GazeboError e) + { + std::cout << "Gazebo error: Unable to connect\n" << e << "\n"; + return -1; + } + + /// Open the Simulation Interface + try + { + simIface->Open(client, "default"); + } + catch (gazebo::GazeboError e) + { + std::cout << "Gazebo error: Unable to connect to the sim interface\n" << e << "\n"; + return -1; + } + + /// Open the Actuator Array interface + try + { + actarrayIface->Open(client, "bandit_actarray_iface"); + } + catch (std::string e) + { + std::cout << "Gazebo error: Unable to connect to the actarray interface\n" + << e << "\n"; + return -1; + } + + while (true) + { + actarrayIface->Lock(1); + actarrayIface->data->cmd_pos[16] = 0.3; + actarrayIface->data->cmd_pos[17] = -0.3; + actarrayIface->data->cmd_pos[18] = 0.3; + actarrayIface->data->cmd_pos[19] = -0.3; + actarrayIface->Unlock(); + + usleep(100000); + } + return 0; +} + Modified: code/gazebo/trunk/examples/libgazebo/position/position.cc =================================================================== --- code/gazebo/trunk/examples/libgazebo/position/position.cc 2008-08-28 01:30:51 UTC (rev 6985) +++ code/gazebo/trunk/examples/libgazebo/position/position.cc 2008-08-28 15:49:20 UTC (rev 6986) @@ -51,7 +51,7 @@ while (true) { posIface->Lock(1); - posIface->data->cmdVelocity.pos.x += 10; + posIface->data->cmdVelocity.pos.x = 0.2; posIface->Unlock(); usleep(100000); Modified: code/gazebo/trunk/server/controllers/Controller.cc =================================================================== --- code/gazebo/trunk/server/controllers/Controller.cc 2008-08-28 01:30:51 UTC (rev 6985) +++ code/gazebo/trunk/server/controllers/Controller.cc 2008-08-28 15:49:20 UTC (rev 6986) @@ -176,10 +176,13 @@ /// Update the controller. Called every cycle. void Controller::Update() { - if (lastUpdate + updatePeriod <= Simulator::Instance()->GetSimTime()) + if (this->IsConnected()) { - this->UpdateChild(); - lastUpdate = Simulator::Instance()->GetSimTime(); + if (lastUpdate + updatePeriod <= Simulator::Instance()->GetSimTime()) + { + this->UpdateChild(); + lastUpdate = Simulator::Instance()->GetSimTime(); + } } } @@ -200,6 +203,21 @@ } //////////////////////////////////////////////////////////////////////////////// +/// Return true if an interface is open +bool Controller::IsConnected() const +{ + std::vector<Iface*>::const_iterator iter; + + for (iter=this->ifaces.begin(); iter!=this->ifaces.end(); iter++) + { + if ((*iter)->GetOpenCount() > 0) + return true; + } + + return false; +} + +//////////////////////////////////////////////////////////////////////////////// // Get the name of the controller std::string Controller::GetName() const { Modified: code/gazebo/trunk/server/controllers/Controller.hh =================================================================== --- code/gazebo/trunk/server/controllers/Controller.hh 2008-08-28 01:30:51 UTC (rev 6985) +++ code/gazebo/trunk/server/controllers/Controller.hh 2008-08-28 15:49:20 UTC (rev 6986) @@ -74,6 +74,9 @@ /// \brief Finialize the controller. Called once on completion. public: void Fini(); + /// \brief Return true if an interface is open + public: bool IsConnected() const; + /// \brief Load function for the child class protected: virtual void LoadChild(XMLConfigNode * /*node*/) {return;} Modified: code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.cc =================================================================== --- code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.cc 2008-08-28 01:30:51 UTC (rev 6985) +++ code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.cc 2008-08-28 15:49:20 UTC (rev 6986) @@ -55,7 +55,7 @@ // Destructor Bandit_Actarray::~Bandit_Actarray() { - for (int i=0; i<16; i++) + for (int i=0; i<JOINTCNT; i++) { GZ_DELETE(this->jointNamesP[i]); GZ_DELETE(this->forcesP[i]); @@ -96,7 +96,7 @@ /// Save the controller. void Bandit_Actarray::SaveChild(std::string &prefix, std::ostream &stream) { - for (int i=0; i<16; i++) + for (int i=0; i<JOINTCNT; i++) { stream << prefix << "<joint name=\"" << this->jointNamesP[i]->GetValue() << "\">\n"; stream << prefix << " " << *(this->forcesP[i]) << "\n"; @@ -109,7 +109,7 @@ // Initialize the controller void Bandit_Actarray::InitChild() { - for (int i=0; i<16; i++) + for (int i=0; i<JOINTCNT; i++) { this->joints[i]->SetParam( dParamVel, 0.0); this->joints[i]->SetParam( dParamFMax, **(this->forcesP[i]) ); @@ -126,9 +126,9 @@ this->myIface->Lock(1); this->myIface->data->head.time = Simulator::Instance()->GetSimTime(); - this->myIface->data->actuators_count = 16; + this->myIface->data->actuators_count = JOINTCNT; - for (unsigned int i=0; i<16; i++) + for (unsigned int i=0; i<JOINTCNT; i++) { double cmdAngle = this->myIface->data->cmd_pos[i]; double cmdSpeed = this->myIface->data->cmd_speed[i]; @@ -153,6 +153,7 @@ joint->SetParam( dParamVel, **(this->gainsP[i]) * angle); joint->SetParam( dParamFMax, **(this->forcesP[i]) ); } + } else if (this->myIface->data->joint_mode[i] == GAZEBO_ACTARRAY_JOINT_SPEED_MODE) { Modified: code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.hh =================================================================== --- code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.hh 2008-08-28 01:30:51 UTC (rev 6985) +++ code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.hh 2008-08-28 15:49:20 UTC (rev 6986) @@ -31,6 +31,8 @@ #include "Controller.hh" #include "Entity.hh" +#define JOINTCNT 20 + namespace gazebo { class HingeJoint; @@ -90,10 +92,10 @@ /// The parent Model private: Model *myParent; - private: Param<std::string> *jointNamesP[16]; - private: HingeJoint *joints[16]; - private: Param<float> *forcesP[16]; - private: Param<float> *gainsP[16]; + private: Param<std::string> *jointNamesP[JOINTCNT]; + private: HingeJoint *joints[JOINTCNT]; + private: Param<float> *forcesP[JOINTCNT]; + private: Param<float> *gainsP[JOINTCNT]; }; Modified: code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc =================================================================== --- code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc 2008-08-28 01:30:51 UTC (rev 6985) +++ code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc 2008-08-28 15:49:20 UTC (rev 6986) @@ -103,6 +103,16 @@ } //////////////////////////////////////////////////////////////////////////////// +/// True if a stereo iface is connected +bool Stereo_Camera::StereoIfaceConnected() const +{ + if (this->stereoIface) + return this->stereoIface->GetOpenCount() > 0; + else + return false; +} + +//////////////////////////////////////////////////////////////////////////////// // Update the controller void Stereo_Camera::UpdateChild() { Modified: code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.hh =================================================================== --- code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.hh 2008-08-28 01:30:51 UTC (rev 6985) +++ code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.hh 2008-08-28 15:49:20 UTC (rev 6986) @@ -30,6 +30,7 @@ #include <map> +#include "Stereo_Camera.hh" #include "Param.hh" #include "Controller.hh" @@ -75,6 +76,9 @@ /// \brief Destructor public: virtual ~Stereo_Camera(); + /// \brief True if a stereo iface is connected + public: bool StereoIfaceConnected() const; + /// \brief Load the controller /// \param node XML config node /// \return 0 on success Modified: code/gazebo/trunk/server/physics/Body.cc =================================================================== --- code/gazebo/trunk/server/physics/Body.cc 2008-08-28 01:30:51 UTC (rev 6985) +++ code/gazebo/trunk/server/physics/Body.cc 2008-08-28 15:49:20 UTC (rev 6986) @@ -213,12 +213,12 @@ std::vector< Sensor* >::iterator sensorIter; std::vector< Geom* >::iterator geomIter; + this->UpdatePose(); + if (!this->IsStatic()) { - Pose3d pose = this->GetPose(); - // Set the pose of the scene node - this->visualNode->SetPose(pose); + this->visualNode->SetPose(this->pose); } for (geomIter=this->geoms.begin(); @@ -256,8 +256,9 @@ // Set the pose of the body void Body::SetPose(const Pose3d &pose) { - Pose3d localPose; + this->pose = pose; + if (this->IsStatic()) { Pose3d oldPose = this->staticPose; @@ -279,8 +280,10 @@ } else { + Pose3d localPose; + // Compute pose of CoM - localPose = this->comPose + pose; + localPose = this->comPose + this->pose; this->SetPosition(localPose.pos); this->SetRotation(localPose.rot); @@ -291,21 +294,20 @@ // Return the pose of the body Pose3d Body::GetPose() const { - Pose3d pose; - if (this->IsStatic()) - { - pose = this->staticPose; - } + return this->staticPose; else - { - pose.pos = this->GetPosition(); - pose.rot = this->GetRotation(); + return this->pose; +} - pose = this->comPose.CoordPoseSolve(pose); - } +//////////////////////////////////////////////////////////////////////////////// +// Update the pose of the body +void Body::UpdatePose() +{ + this->pose.pos = this->GetPosition(); + this->pose.rot = this->GetRotation(); - return pose; + this->pose = this->comPose.CoordPoseSolve(pose); } //////////////////////////////////////////////////////////////////////////////// Modified: code/gazebo/trunk/server/physics/Body.hh =================================================================== --- code/gazebo/trunk/server/physics/Body.hh 2008-08-28 01:30:51 UTC (rev 6985) +++ code/gazebo/trunk/server/physics/Body.hh 2008-08-28 15:49:20 UTC (rev 6986) @@ -150,7 +150,10 @@ /// \brief Load a renderable private: void LoadVisual(XMLConfigNode *node); - + + /// \brief Update the pose of the body + private: void UpdatePose(); + /// List of geometries attached to this body private: std::vector< Geom* > geoms; @@ -167,6 +170,7 @@ private: Pose3d comPose; private: Pose3d staticPose; + private: Pose3d pose; private: Param<Vector3> *xyzP; private: Param<Quatern> *rpyP; Modified: code/gazebo/trunk/server/physics/Joint.cc =================================================================== --- code/gazebo/trunk/server/physics/Joint.cc 2008-08-28 01:30:51 UTC (rev 6985) +++ code/gazebo/trunk/server/physics/Joint.cc 2008-08-28 15:49:20 UTC (rev 6986) @@ -42,7 +42,7 @@ this->nameP = new Param<std::string>("name","",1); this->erpP = new Param<double>("erp",0.4,0); - this->cfmP = new Param<double>("cfm",0.8,0); + this->cfmP = new Param<double>("cfm",10e-3,0); this->suspensionCfmP = new Param<double>("suspensionCfm",0.0,0); this->body1NameP = new Param<std::string>("body1",std::string(),1); this->body2NameP = new Param<std::string>("body2",std::string(),1); Modified: code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.cc =================================================================== --- code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.cc 2008-08-28 01:30:51 UTC (rev 6985) +++ code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.cc 2008-08-28 15:49:20 UTC (rev 6986) @@ -131,6 +131,12 @@ // Update the drawing void MonoCameraSensor::UpdateChild() { + // Only continue if the controller has an active interface. Or frames need + // to be saved + if ( (this->controller && !this->controller->IsConnected()) && + !this->saveFramesP->GetValue()) + return; + this->UpdateCam(); this->renderTarget->update(); Modified: code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc =================================================================== --- code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc 2008-08-28 01:30:51 UTC (rev 6985) +++ code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc 2008-08-28 15:49:20 UTC (rev 6986) @@ -140,6 +140,7 @@ this->renderTarget = this->renderTargets[D_LEFT]; this->InitCam(); + // Hack to make the camera use the right render target too. for (i=0; i<4; i++) { @@ -206,6 +207,12 @@ Ogre::SceneNode *gridNode = NULL; int i; + // Only continue if the controller has an active interface. Or frames need + // to be saved + if ( (this->controller && !this->controller->IsConnected()) && + !this->saveFramesP->GetValue()) + return; + this->UpdateCam(); try @@ -236,7 +243,6 @@ // we have to set the distance every time. this->GetOgreCamera()->setFarClipDistance( this->farClipP->GetValue() ); - Ogre::AutoParamDataSource autoParamDataSource; vp = this->renderTargets[i]->getViewport(0); Modified: code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh =================================================================== --- code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh 2008-08-28 01:30:51 UTC (rev 6985) +++ code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh 2008-08-28 15:49:20 UTC (rev 6986) @@ -120,7 +120,7 @@ private: double baseline; private: Ogre::Camera *depthCamera; - + /*private: class StereoCameraListener : public Ogre::RenderTargetListener { Modified: code/gazebo/trunk/worlds/bandit.world =================================================================== --- code/gazebo/trunk/worlds/bandit.world 2008-08-28 01:30:51 UTC (rev 6985) +++ code/gazebo/trunk/worlds/bandit.world 2008-08-28 15:49:20 UTC (rev 6986) @@ -19,17 +19,25 @@ <stepTime>0.3</stepTime> <gravity>0 0 -9.8</gravity> <cfm>10e-5</cfm> - <erp>0.3</erp> + <erp>0.1</erp> </physics:ode> <rendering:gui> <type>fltk</type> <size>800 600</size> <pos>0 0</pos> + <frames> + <row height="100%"> + <camera width="100%"> + <xyz>2.16 0.0 0.86</xyz> + <rpy>0 16.5 179.50</rpy> + </camera> + </row> + </frames> </rendering:gui> <rendering:ogre> - <ambient>1.0 1.0 1.0 1.0</ambient> + <ambient>0.8 0.8 0.8 1.0</ambient> <sky> <material>Gazebo/CloudySky</material> </sky> @@ -58,7 +66,6 @@ <xi:include href="models/pioneer2dx.model" /> </include> - <model:physical name="box_model"> <xyz>0.15 0 0.125</xyz> <static>false</static> @@ -82,6 +89,7 @@ <model:physical name="bandit"> <xyz>0.02 0 0.49</xyz> + <static>false</static> <attach> <parentBody>box_body</parentBody> @@ -135,11 +143,11 @@ <model:renderable name="directional_white"> <light> <type>directional</type> - <direction>0 -0.5 -0.5</direction> - <diffuseColor>0.4 0.4 0.4</diffuseColor> + <direction>-1 1.0 -0.5</direction> + <diffuseColor>0.2 0.2 0.2</diffuseColor> <specularColor>0.0 0.0 0.0</specularColor> <attenuation>100 0.0 1.0 0.4</attenuation> </light> </model:renderable> - + </gazebo:world> Modified: code/gazebo/trunk/worlds/models/bandit.model =================================================================== --- code/gazebo/trunk/worlds/models/bandit.model 2008-08-28 01:30:51 UTC (rev 6985) +++ code/gazebo/trunk/worlds/models/bandit.model 2008-08-28 15:49:20 UTC (rev 6986) @@ -484,9 +484,117 @@ <highStop>20</highStop> </joint:hinge> + <body name="left_eyebrow_body"> + <xyz>0.035 0.041 0.034</xyz> + <rpy>85 0 -70</rpy> + + <geom:box name="left_eyebrow_geom"> + <size>0.045 0.02 0.002</size> + <mass>0</mass> + + <visual> + <size>0.045 0.02 0.002</size> + <mesh>bandit/right_eyebrow.mesh</mesh> + <material>Gazebo/Black</material> + </visual> + </geom:box> + </body> + + <joint:hinge name="left_eyebrow_hinge"> + <body1>head_body</body1> + <body2>left_eyebrow_body</body2> + <anchor>left_eyebrow_body</anchor> + <anchorOffset>-0.01 0.01 0</anchorOffset> + <axis>1 0 0</axis> + <lowStop>-70</lowStop> + <highStop>70</highStop> + </joint:hinge> + + <body name="right_eyebrow_body"> + <xyz>0.035 -0.041 0.034</xyz> + <rpy>95 16 -110</rpy> + + <geom:box name="right_eyebrow_geom"> + <size>0.045 0.02 0.002</size> + <mass>0</mass> + <visual> + <size>0.045 0.02 0.002</size> + <mesh>bandit/right_eyebrow.mesh</mesh> + <material>Gazebo/Black</material> + </visual> + </geom:box> + </body> + + <joint:hinge name="right_eyebrow_hinge"> + <body1>head_body</body1> + <body2>right_eyebrow_body</body2> + <anchor>right_eyebrow_body</anchor> + <anchorOffset>-0.01 -0.01 0</anchorOffset> + <axis>1 0 0</axis> + <lowStop>-70</lowStop> + <highStop>70</highStop> + </joint:hinge> + + <body name="upperlip_body"> + <xyz>0.044 -0.001 -0.05</xyz> + <rpy>0 0 0</rpy> + + <geom:box name="upperlip_geom"> + <size>0.02 0.08 0.014</size> + <mass>0</mass> + <visual> + <rpy>0 0 -91</rpy> + <xyz>0.006 -0.0015 0</xyz> + <scale>0.05 0.05 0.05</scale> + <mesh>bandit/upperlip.mesh</mesh> + <material>Gazebo/Red</material> + </visual> + </geom:box> + </body> + + <joint:hinge name="upperlip_hinge"> + <body1>head_body</body1> + <body2>upperlip_body</body2> + <anchor>upperlip_body</anchor> + <anchorOffset>0 -0.042 0.0</anchorOffset> + <axis>0 1 0</axis> + <lowStop>-20</lowStop> + <highStop>20</highStop> + </joint:hinge> + + <body name="lowerlip_body"> + <xyz>0.044 0.0 -0.06</xyz> + <rpy>0 0 0</rpy> + + <geom:box name="lowerlip_geom"> + <size>0.02 0.08 0.014</size> + <mass>0</mass> + + <visual> + <rpy>90 0 90</rpy> + <xyz>0.01 0 -0.015</xyz> + <scale>0.09 0.1 0.1</scale> + <mesh>bandit/lowerlip.mesh</mesh> + <material>Gazebo/Red</material> + </visual> + </geom:box> + </body> + + <joint:hinge name="lowerlip_hinge"> + <body1>head_body</body1> + <body2>lowerlip_body</body2> + <anchor>lowerlip_body</anchor> + <anchorOffset>0.0 -0.042 0</anchorOffset> + <axis>0 1 0</axis> + <lowStop>-20</lowStop> + <highStop>20</highStop> + </joint:hinge> + + + <controller:bandit_actarray name="bandit_controller"> <joint name="head_neck_hinge"> - <force>2</force> + <force>1</force> <gain>2</gain> </joint> @@ -565,6 +673,26 @@ <gain>2</gain> </joint> + <joint name="left_eyebrow_hinge"> + <force>1</force> + <gain>2</gain> + </joint> + + <joint name="right_eyebrow_hinge"> + <force>1</force> + <gain>2</gain> + </joint> + + <joint name="lowerlip_hinge"> + <force>1</force> + <gain>2</gain> + </joint> + + <joint name="upperlip_hinge"> + <force>1</force> + <gain>2</gain> + </joint> + <interface:actarray name="bandit_actarray_iface"/> </controller:bandit_actarray> Modified: code/gazebo/trunk/worlds/simpleshapes.world =================================================================== --- code/gazebo/trunk/worlds/simpleshapes.world 2008-08-28 01:30:51 UTC (rev 6985) +++ code/gazebo/trunk/worlds/simpleshapes.world 2008-08-28 15:49:20 UTC (rev 6986) @@ -38,16 +38,17 @@ <model:physical name="sphere1_model"> <xyz>1 0 0.5</xyz> <rpy>0.0 0.0 0.0</rpy> - <static>true</static> + <static>false</static> <body:sphere name="sphere1_body"> <geom:sphere name="sphere1_geom"> - <size>0.5</size> + <size>0.25</size> <mass>0.0</mass> <mu1>109999.0</mu1> <visual> + <size>0.5 0.5 0.5</size> <mesh>unit_sphere</mesh> <material>Gazebo/Rocky</material> </visual> @@ -74,7 +75,6 @@ </body:box> </model:physical> - <!-- <model:physical name="cylinder1_model"> <xyz>1 -1.5 0.5</xyz> <rpy>0.0 0.0 0.0</rpy> @@ -92,7 +92,6 @@ </geom:cylinder> </body:cylinder> </model:physical> - --> <!-- Ground Plane --> <model:physical name="plane1_model"> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-09-01 11:56:44
|
Revision: 7000 http://playerstage.svn.sourceforge.net/playerstage/?rev=7000&view=rev Author: natepak Date: 2008-09-01 18:56:47 +0000 (Mon, 01 Sep 2008) Log Message: ----------- Fixed Simulation GetPose, and updated the gui Modified Paths: -------------- code/gazebo/trunk/examples/player/simulation/simulation.cc code/gazebo/trunk/player/SimulationInterface.cc code/gazebo/trunk/player/SimulationInterface.hh code/gazebo/trunk/server/Entity.cc code/gazebo/trunk/server/Entity.hh code/gazebo/trunk/server/GazeboMessage.cc code/gazebo/trunk/server/GazeboMessage.hh code/gazebo/trunk/server/Model.cc code/gazebo/trunk/server/Model.hh code/gazebo/trunk/server/Param.hh code/gazebo/trunk/server/SConscript code/gazebo/trunk/server/World.cc code/gazebo/trunk/server/controllers/Controller.cc code/gazebo/trunk/server/controllers/Controller.hh code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.cc code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.hh code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.hh code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.cc code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.hh code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.cc code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.hh code/gazebo/trunk/server/controllers/ptz/generic/Generic_PTZ.cc code/gazebo/trunk/server/controllers/ptz/generic/Generic_PTZ.hh code/gazebo/trunk/server/gui/GLWindow.cc code/gazebo/trunk/server/gui/Gui.cc code/gazebo/trunk/server/gui/Gui.hh code/gazebo/trunk/server/gui/StatusBar.cc code/gazebo/trunk/server/gui/Toolbar.cc code/gazebo/trunk/server/gui/Toolbar.hh code/gazebo/trunk/server/physics/Body.cc code/gazebo/trunk/server/physics/Body.hh code/gazebo/trunk/server/physics/BoxGeom.cc code/gazebo/trunk/server/physics/BoxGeom.hh code/gazebo/trunk/server/physics/CylinderGeom.cc code/gazebo/trunk/server/physics/CylinderGeom.hh code/gazebo/trunk/server/physics/Geom.cc code/gazebo/trunk/server/physics/Geom.hh code/gazebo/trunk/server/physics/HeightmapGeom.cc code/gazebo/trunk/server/physics/HeightmapGeom.hh code/gazebo/trunk/server/physics/Hinge2Joint.cc code/gazebo/trunk/server/physics/Hinge2Joint.hh code/gazebo/trunk/server/physics/HingeJoint.cc code/gazebo/trunk/server/physics/HingeJoint.hh code/gazebo/trunk/server/physics/Joint.cc code/gazebo/trunk/server/physics/Joint.hh code/gazebo/trunk/server/physics/MapGeom.cc code/gazebo/trunk/server/physics/MapGeom.hh code/gazebo/trunk/server/physics/PhysicsEngine.cc code/gazebo/trunk/server/physics/PhysicsEngine.hh code/gazebo/trunk/server/physics/PlaneGeom.cc code/gazebo/trunk/server/physics/PlaneGeom.hh code/gazebo/trunk/server/physics/SliderJoint.cc code/gazebo/trunk/server/physics/SliderJoint.hh code/gazebo/trunk/server/physics/SphereGeom.cc code/gazebo/trunk/server/physics/SphereGeom.hh code/gazebo/trunk/server/physics/TrimeshGeom.cc code/gazebo/trunk/server/physics/TrimeshGeom.hh code/gazebo/trunk/server/physics/UniversalJoint.cc code/gazebo/trunk/server/physics/UniversalJoint.hh code/gazebo/trunk/server/physics/ode/ODEPhysics.cc code/gazebo/trunk/server/physics/ode/ODEPhysics.hh code/gazebo/trunk/server/rendering/OgreAdaptor.cc code/gazebo/trunk/server/rendering/OgreAdaptor.hh code/gazebo/trunk/server/rendering/OgreCamera.cc code/gazebo/trunk/server/rendering/OgreCamera.hh code/gazebo/trunk/server/rendering/OgreVisual.cc code/gazebo/trunk/server/rendering/OgreVisual.hh code/gazebo/trunk/server/sensors/Sensor.cc code/gazebo/trunk/server/sensors/Sensor.hh code/gazebo/trunk/server/sensors/ray/RaySensor.cc code/gazebo/trunk/server/sensors/ray/RaySensor.hh Modified: code/gazebo/trunk/examples/player/simulation/simulation.cc =================================================================== --- code/gazebo/trunk/examples/player/simulation/simulation.cc 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/examples/player/simulation/simulation.cc 2008-09-01 18:56:47 UTC (rev 7000) @@ -23,11 +23,14 @@ // Get the robot pose sp.GetPose3d((char*)"pioneer2dx_model1", x, y, z, roll, pitch, yaw, time); + printf("Robot Pose3d: XYZ[%f %f %f] RPY[%f %f %f]\n",x, y, z, roll, pitch, yaw); + sp.GetPose2d((char*)"pioneer2dx_model1", x, y, roll ); + printf("Robot Pose2d: XY[%f %f] Yaw[%f]\n", x, y, roll ); + // Set the robot pose //sp.SetPose3d((char*)"pioneer2dx_model1", 2.0, 0.0, 5.0, 0, 0, 0); - printf("Get Sim Time\n"); /// Get the simulation time sp.GetProperty((char*)"world",(char*)"sim_time", &time, sizeof(double)); @@ -40,6 +43,7 @@ /// Get the realtime sp.GetProperty((char*)"world",(char*)"real_time", &time, sizeof(time)); printf("Real Time[%f]\n",time); + } catch (PlayerCc::PlayerError e) { Modified: code/gazebo/trunk/player/SimulationInterface.cc =================================================================== --- code/gazebo/trunk/player/SimulationInterface.cc 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/player/SimulationInterface.cc 2008-09-01 18:56:47 UTC (rev 7000) @@ -53,6 +53,9 @@ this->iface = new SimulationIface(); this->responseQueue = NULL; + + memset( &this->pose3dReq, 0, sizeof(this->pose3dReq) ); + memset( &this->pose2dReq, 0, sizeof(this->pose2dReq) ); } /////////////////////////////////////////////////////////////////////////////// @@ -160,7 +163,8 @@ /// Get a 2D pose else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, - PLAYER_SIMULATION_REQ_GET_POSE2D, this->device_addr)) + PLAYER_SIMULATION_REQ_GET_POSE2D, + this->device_addr)) { gazebo::SimulationRequestData *gzReq = NULL; player_simulation_pose2d_req_t *req = @@ -248,39 +252,51 @@ case gazebo::SimulationRequestData::GET_POSE3D: { - player_simulation_pose3d_req_t req; - strcpy(req.name, response->modelName); - req.name_count = strlen(req.name); + if (this->pose3dReq.name_count != strlen(response->modelName)) + { + if (this->pose3dReq.name) + delete [] this->pose3dReq.name; + this->pose3dReq.name = new char[strlen(response->modelName)+1]; + } - req.pose.px = response->modelPose.pos.x; - req.pose.py = response->modelPose.pos.y; - req.pose.pz = response->modelPose.pos.z; + strcpy(this->pose3dReq.name, response->modelName); + this->pose3dReq.name_count = strlen(this->pose3dReq.name); - req.pose.proll = response->modelPose.roll; - req.pose.ppitch = response->modelPose.pitch; - req.pose.pyaw = response->modelPose.yaw; + this->pose3dReq.pose.px = response->modelPose.pos.x; + this->pose3dReq.pose.py = response->modelPose.pos.y; + this->pose3dReq.pose.pz = response->modelPose.pos.z; + this->pose3dReq.pose.proll = response->modelPose.roll; + this->pose3dReq.pose.ppitch = response->modelPose.pitch; + this->pose3dReq.pose.pyaw = response->modelPose.yaw; + this->driver->Publish(this->device_addr, *(this->responseQueue), PLAYER_MSGTYPE_RESP_ACK, PLAYER_SIMULATION_REQ_GET_POSE3D, - &req, sizeof(req), NULL); + &this->pose3dReq, sizeof(this->pose3dReq), NULL); break; } case gazebo::SimulationRequestData::GET_POSE2D: { - player_simulation_pose2d_req_t req; - strcpy(req.name, response->modelName); - req.name_count = strlen(req.name); + if (this->pose2dReq.name_count != strlen(response->modelName)) + { + if (this->pose2dReq.name) + delete [] this->pose2dReq.name; + this->pose2dReq.name = new char[strlen(response->modelName)+1]; + } - req.pose.px = response->modelPose.pos.x; - req.pose.py = response->modelPose.pos.y; - req.pose.pa = response->modelPose.yaw; + strcpy(this->pose2dReq.name, response->modelName); + this->pose2dReq.name_count = strlen(this->pose2dReq.name); + this->pose2dReq.pose.px = response->modelPose.pos.x; + this->pose2dReq.pose.py = response->modelPose.pos.y; + this->pose2dReq.pose.pa = response->modelPose.yaw; + this->driver->Publish(this->device_addr, *(this->responseQueue), PLAYER_MSGTYPE_RESP_ACK, PLAYER_SIMULATION_REQ_GET_POSE2D, - &req, sizeof(req), NULL); + &this->pose2dReq, sizeof(this->pose2dReq), NULL); break; } Modified: code/gazebo/trunk/player/SimulationInterface.hh =================================================================== --- code/gazebo/trunk/player/SimulationInterface.hh 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/player/SimulationInterface.hh 2008-09-01 18:56:47 UTC (rev 7000) @@ -83,6 +83,10 @@ public: SimulationIface *iface; private: QueuePointer *responseQueue; + + private: player_simulation_pose3d_req_t pose3dReq; + + private: player_simulation_pose2d_req_t pose2dReq; }; /// \} Modified: code/gazebo/trunk/server/Entity.cc =================================================================== --- code/gazebo/trunk/server/Entity.cc 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/Entity.cc 2008-09-01 18:56:47 UTC (rev 7000) @@ -42,8 +42,10 @@ id(++idCounter), visualNode(0) { - this->nameP = new Param<std::string>("name","",1); - this->staticP = new Param<bool>("static",false,0); + Param::Begin(&this->parameters); + this->nameP = new ParamT<std::string>("name","",1); + this->staticP = new ParamT<bool>("static",false,0); + Param::End(); this->selected = false; @@ -195,3 +197,12 @@ { return ent.GetName() == this->GetName(); } + +//////////////////////////////////////////////////////////////////////////////// +/// Get the parameters +std::vector<Param*> *Entity::GetParameters() +{ + return &this->parameters; +} + + Modified: code/gazebo/trunk/server/Entity.hh =================================================================== --- code/gazebo/trunk/server/Entity.hh 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/Entity.hh 2008-09-01 18:56:47 UTC (rev 7000) @@ -112,7 +112,10 @@ /// \brief Returns true if the entities are the same. Checks only the name public: bool operator==(const Entity &ent) const; - + + /// \brief Get the parameters + public: std::vector<Param*> *GetParameters(); + /// \brief Parent of this entity protected: Entity *parent; @@ -126,7 +129,7 @@ private: static unsigned int idCounter; // is this an static entity - protected: Param<bool> *staticP; + protected: ParamT<bool> *staticP; /// \brief Visual stuff protected: OgreVisual *visualNode; @@ -135,7 +138,10 @@ public: dSpaceID spaceId; /// \brief Name of the entity - protected: Param<std::string> *nameP; + protected: ParamT<std::string> *nameP; + + /// List of all the parameters + protected: std::vector<Param*> parameters; private: bool selected; Modified: code/gazebo/trunk/server/GazeboMessage.cc =================================================================== --- code/gazebo/trunk/server/GazeboMessage.cc 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/GazeboMessage.cc 2008-09-01 18:56:47 UTC (rev 7000) @@ -44,9 +44,10 @@ this->errStream = &std::cerr; this->level = 0; - - this->verbosityP = new Param<int>("verbosity",0,0); - this->logDataP = new Param<bool>("logData",false,0); + Param::Begin(&this->parameters); + this->verbosityP = new ParamT<int>("verbosity",0,0); + this->logDataP = new ParamT<bool>("logData",false,0); + Param::End(); } //////////////////////////////////////////////////////////////////////////////// Modified: code/gazebo/trunk/server/GazeboMessage.hh =================================================================== --- code/gazebo/trunk/server/GazeboMessage.hh 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/GazeboMessage.hh 2008-09-01 18:56:47 UTC (rev 7000) @@ -108,8 +108,9 @@ private: std::ostream *errStream; private: std::ofstream logStream; - private: Param<int> *verbosityP; - private: Param<bool> *logDataP; + private: ParamT<int> *verbosityP; + private: ParamT<bool> *logDataP; + private: std::vector<Param*> parameters; /// Pointer to myself private: static GazeboMessage *myself; Modified: code/gazebo/trunk/server/Model.cc =================================================================== --- code/gazebo/trunk/server/Model.cc 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/Model.cc 2008-09-01 18:56:47 UTC (rev 7000) @@ -59,13 +59,13 @@ this->type = ""; this->joint = NULL; - this->canonicalBodyNameP = new Param<std::string>("canonicalBody", + Param::Begin(&this->parameters); + this->canonicalBodyNameP = new ParamT<std::string>("canonicalBody", std::string(),0); - this->xyzP = new Param<Vector3>("xyz", Vector3(0,0,0), 0); - this->rpyP = new Param<Quatern>("rpy", Quatern(1,0,0,0), 0); + this->xyzP = new ParamT<Vector3>("xyz", Vector3(0,0,0), 0); + this->rpyP = new ParamT<Quatern>("rpy", Quatern(1,0,0,0), 0); + Param::End(); - this->parameters.push_back( this->canonicalBodyNameP ); - this->parentBodyNameP = NULL; this->myBodyNameP = NULL; } @@ -342,6 +342,8 @@ if (!this->canonicalBodyNameP->GetValue().empty()) { this->pose = this->bodies[this->canonicalBodyNameP->GetValue()]->GetPose(); + this->xyzP->SetValue(this->pose.pos); + this->rpyP->SetValue(this->pose.rot); } return this->UpdateChild(); @@ -632,8 +634,10 @@ { Model *parentModel = NULL; - this->parentBodyNameP = new Param<std::string>("parentBody","canonical",1); - this->myBodyNameP = new Param<std::string>("myBody",this->canonicalBodyNameP->GetValue(),1); + Param::Begin(&this->parameters); + this->parentBodyNameP = new ParamT<std::string>("parentBody","canonical",1); + this->myBodyNameP = new ParamT<std::string>("myBody",this->canonicalBodyNameP->GetValue(),1); + Param::End(); if (node) { Modified: code/gazebo/trunk/server/Model.hh =================================================================== --- code/gazebo/trunk/server/Model.hh 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/Model.hh 2008-09-01 18:56:47 UTC (rev 7000) @@ -179,12 +179,11 @@ /// \brief Light numbering variable to give a unique name to all light entities private: static uint lightNumber; - private: std::vector<boost::any> parameters; - private: Param<std::string> *canonicalBodyNameP; - private: Param<Vector3> *xyzP; - private: Param<Quatern> *rpyP; - private: Param<std::string> *parentBodyNameP; - private: Param<std::string> *myBodyNameP; + private: ParamT<std::string> *canonicalBodyNameP; + private: ParamT<Vector3> *xyzP; + private: ParamT<Quatern> *rpyP; + private: ParamT<std::string> *parentBodyNameP; + private: ParamT<std::string> *myBodyNameP; // Name of a light (if the model is renderable:light) private: std::string lightName; Modified: code/gazebo/trunk/server/Param.hh =================================================================== --- code/gazebo/trunk/server/Param.hh 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/Param.hh 2008-09-01 18:56:47 UTC (rev 7000) @@ -28,25 +28,61 @@ #define PARAM_HH #include <iostream> +#include <vector> #include <boost/lexical_cast.hpp> +#include <boost/any.hpp> +#include <typeinfo> #include <string> #include "XMLConfig.hh" namespace gazebo { - template< typename T > class Param { /// \brief Constructor - public: Param(std::string key, T defValue, int required); + public: Param(Param *newParam); + + /// \brief Destructor + public: virtual ~Param(); + + /// \brief Begin a block of "new Param<*>" + public: static void Begin(std::vector<Param*> *_params); + + /// \brief End a block of "new Param<*>" + public: static void End(); + + /// \brief The name of the key + public: std::string GetKey() const; + + /// \brief Get the name of the param's data type + public: std::string GetTypename() const; + + /// \brief Get the type + public: virtual std::string GetAsString() const {return std::string();} + + /// List of created parameters + private: static std::vector<Param*> *params; + + protected: std::string key; + protected: std::string typeName; + }; + + + template< typename T> + class ParamT : public Param + { + /// \brief Constructor + public: ParamT(std::string key, T defValue, int required); /// \brief Destructor - public: virtual ~Param(); + public: virtual ~ParamT(); /// \brief Load the param from an XML config file public: void Load(XMLConfigNode *node); + public: virtual std::string GetAsString() const; + /// \brief Get the value public: T GetValue() const; @@ -55,7 +91,7 @@ public: inline T operator*() const {return value;} - public: friend std::ostream &operator<<( std::ostream &out, const Param<T> &p) + public: friend std::ostream &operator<<( std::ostream &out, const ParamT<T> &p) { out << "<" << p.key << ">" << p.value << "</" << p.key << ">"; @@ -64,7 +100,6 @@ private: T value; - private: std::string key; private: T defaultValue; private: int required; @@ -74,25 +109,28 @@ ////////////////////////////////////////////////////////////////////////////// // Constructor template< typename T> - Param<T>::Param(std::string key, T defValue, int required) + ParamT<T>::ParamT(std::string key, T defValue, int required) + : Param(this) { this->key = key; this->defaultValue = defValue; this->required = required; this->value = this->defaultValue; + + this->typeName = typeid(T).name(); } ////////////////////////////////////////////////////////////////////////////// // Destructor template<typename T> - Param<T>::~Param() + ParamT<T>::~ParamT() { } ////////////////////////////////////////////////////////////////////////////// /// Load the param from an XML config file template<typename T> - void Param<T>::Load(XMLConfigNode *node) + void ParamT<T>::Load(XMLConfigNode *node) { std::ostringstream stream; stream << this->defaultValue; @@ -117,9 +155,17 @@ } ////////////////////////////////////////////////////////////////////////////// + // Get value as boost + template<typename T> + std::string ParamT<T>::GetAsString() const + { + return boost::lexical_cast<std::string>(this->value); + } + + ////////////////////////////////////////////////////////////////////////////// ///Get the value template<typename T> - T Param<T>::GetValue() const + T ParamT<T>::GetValue() const { return this->value; } @@ -127,7 +173,7 @@ ////////////////////////////////////////////////////////////////////////////// /// Set the value of the parameter template<typename T> - void Param<T>::SetValue(const T &v) + void ParamT<T>::SetValue(const T &v) { this->value = v; } Modified: code/gazebo/trunk/server/SConscript =================================================================== --- code/gazebo/trunk/server/SConscript 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/SConscript 2008-09-01 18:56:47 UTC (rev 7000) @@ -21,6 +21,7 @@ 'Model.cc', 'Simulator.cc', 'Angle.cc', + 'Param.cc' ] headers.append( Modified: code/gazebo/trunk/server/World.cc =================================================================== --- code/gazebo/trunk/server/World.cc 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/World.cc 2008-09-01 18:56:47 UTC (rev 7000) @@ -589,6 +589,7 @@ break; } + case SimulationRequestData::GET_POSE2D: case SimulationRequestData::GET_POSE3D: { Model *model = this->GetModelByName((char*)req->modelName); @@ -619,7 +620,7 @@ break; } - case SimulationRequestData::SET_POSE2D: + case SimulationRequestData::SET_POSE2D: { Model *model = this->GetModelByName((char*)req->modelName); if (model) Modified: code/gazebo/trunk/server/controllers/Controller.cc =================================================================== --- code/gazebo/trunk/server/controllers/Controller.cc 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/controllers/Controller.cc 2008-09-01 18:56:47 UTC (rev 7000) @@ -41,8 +41,10 @@ /// Constructor Controller::Controller(Entity *entity ) { - this->nameP = new Param<std::string>("name","",1); - this->updatePeriodP = new Param<double>("updateRate", 10, 0); + Param::Begin(&this->parameters); + this->nameP = new ParamT<std::string>("name","",1); + this->updatePeriodP = new ParamT<double>("updateRate", 10, 0); + Param::End(); if (!dynamic_cast<Model*>(entity) && !dynamic_cast<Sensor*>(entity)) { Modified: code/gazebo/trunk/server/controllers/Controller.hh =================================================================== --- code/gazebo/trunk/server/controllers/Controller.hh 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/controllers/Controller.hh 2008-09-01 18:56:47 UTC (rev 7000) @@ -100,14 +100,14 @@ public: std::string GetName() const; /// \brief The controller's name - protected: Param<std::string> *nameP; + protected: ParamT<std::string> *nameP; /// \brief The entity that owns this controller protected: Entity *parent; /// \brief Update period protected: double updatePeriod; - protected: Param<double> *updatePeriodP; + protected: ParamT<double> *updatePeriodP; private: std::string typeName; @@ -119,6 +119,8 @@ private: std::vector< std::string > ifaceTypes; private: std::vector< std::string > ifaceNames; + + protected: std::vector<Param*> parameters; }; /// \} Modified: code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.cc =================================================================== --- code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.cc 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.cc 2008-09-01 18:56:47 UTC (rev 7000) @@ -75,21 +75,23 @@ gzthrow("Bandit_Actarray controller requires a Actarray Iface"); + Param::Begin(&this->parameters); for (i=0, jNode = node->GetChild("joint"); jNode; i++) { - this->jointNamesP[i] = new Param<std::string>("name","",1); + this->jointNamesP[i] = new ParamT<std::string>("name","",1); this->jointNamesP[i]->Load(jNode); - this->forcesP[i] = new Param<float>("force",0.0,1); + this->forcesP[i] = new ParamT<float>("force",0.0,1); this->forcesP[i]->Load(jNode); - this->gainsP[i] = new Param<float>("gain",0.0,1); + this->gainsP[i] = new ParamT<float>("gain",0.0,1); this->gainsP[i]->Load(jNode); this->joints[i] = dynamic_cast<HingeJoint*>(this->myParent->GetJoint(this->jointNamesP[i]->GetValue())); jNode = jNode->GetNext("joint"); } + Param::End(); } //////////////////////////////////////////////////////////////////////////////// Modified: code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.hh =================================================================== --- code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.hh 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.hh 2008-09-01 18:56:47 UTC (rev 7000) @@ -92,10 +92,10 @@ /// The parent Model private: Model *myParent; - private: Param<std::string> *jointNamesP[JOINTCNT]; + private: ParamT<std::string> *jointNamesP[JOINTCNT]; private: HingeJoint *joints[JOINTCNT]; - private: Param<float> *forcesP[JOINTCNT]; - private: Param<float> *gainsP[JOINTCNT]; + private: ParamT<float> *forcesP[JOINTCNT]; + private: ParamT<float> *gainsP[JOINTCNT]; }; Modified: code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc =================================================================== --- code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc 2008-09-01 18:56:47 UTC (rev 7000) @@ -49,8 +49,10 @@ { this->myParent = dynamic_cast<StereoCameraSensor*>(this->parent); - this->leftCameraNameP = new Param<std::string>("leftcamera","", 1); - this->rightCameraNameP = new Param<std::string>("rightcamera","", 1); + Param::Begin(&this->parameters); + this->leftCameraNameP = new ParamT<std::string>("leftcamera","", 1); + this->rightCameraNameP = new ParamT<std::string>("rightcamera","", 1); + Param::End(); if (!this->myParent) gzthrow("Stereo_Camera controller requires a Stereo Camera Sensor as its parent"); Modified: code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.hh =================================================================== --- code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.hh 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.hh 2008-09-01 18:56:47 UTC (rev 7000) @@ -110,8 +110,8 @@ private: StereoCameraIface *stereoIface; private: std::map< std::string, CameraIface*> cameraIfaces; - private: Param<std::string> *leftCameraNameP; - private: Param<std::string> *rightCameraNameP; + private: ParamT<std::string> *leftCameraNameP; + private: ParamT<std::string> *rightCameraNameP; /// The parent sensor private: StereoCameraSensor *myParent; Modified: code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.cc =================================================================== --- code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.cc 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.cc 2008-09-01 18:56:47 UTC (rev 7000) @@ -67,8 +67,10 @@ if (!this->myIface) gzthrow("Pioneer2_Gripper controller requires a GripperIface"); - this->leftJointNameP = new Param<std::string>("leftJoint", "", 1); - this->rightJointNameP = new Param<std::string>("rightJoint", "", 1); + Param::Begin(&this->parameters); + this->leftJointNameP = new ParamT<std::string>("leftJoint", "", 1); + this->rightJointNameP = new ParamT<std::string>("rightJoint", "", 1); + Param::End(); this->joints[LEFT] = dynamic_cast<SliderJoint*>(this->myParent->GetJoint(this->leftJointNameP->GetValue())); this->joints[RIGHT] = dynamic_cast<SliderJoint*>(this->myParent->GetJoint(this->rightJointNameP->GetValue())); Modified: code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.hh =================================================================== --- code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.hh 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.hh 2008-09-01 18:56:47 UTC (rev 7000) @@ -95,8 +95,8 @@ private: SliderJoint *joints[2]; - private: Param<std::string> *leftJointNameP; - private: Param<std::string> *rightJointNameP; + private: ParamT<std::string> *leftJointNameP; + private: ParamT<std::string> *rightJointNameP; }; /** \} */ Modified: code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.cc =================================================================== --- code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.cc 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.cc 2008-09-01 18:56:47 UTC (rev 7000) @@ -60,11 +60,13 @@ this->prevUpdateTime = Simulator::Instance()->GetSimTime(); - this->leftJointNameP = new Param<std::string>("leftJoint", "", 1); - this->rightJointNameP = new Param<std::string>("rightJoint", "", 1); - this->wheelSepP = new Param<float>("wheelSeparation", 0.34,1); - this->wheelDiamP = new Param<float>("wheelDiameter", 0.15,1); - this->torqueP = new Param<float>("torque", 10.0, 1); + Param::Begin(&this->parameters); + this->leftJointNameP = new ParamT<std::string>("leftJoint", "", 1); + this->rightJointNameP = new ParamT<std::string>("rightJoint", "", 1); + this->wheelSepP = new ParamT<float>("wheelSeparation", 0.34,1); + this->wheelDiamP = new ParamT<float>("wheelDiameter", 0.15,1); + this->torqueP = new ParamT<float>("torque", 10.0, 1); + Param::End(); } //////////////////////////////////////////////////////////////////////////////// Modified: code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.hh =================================================================== --- code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.hh 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.hh 2008-09-01 18:56:47 UTC (rev 7000) @@ -103,13 +103,13 @@ private: Model *myParent; /// Separation between the wheels - private: Param<float> *wheelSepP; + private: ParamT<float> *wheelSepP; /// Diameter of the wheels - private: Param<float> *wheelDiamP; + private: ParamT<float> *wheelDiamP; ///Torque applied to the wheels - private: Param<float> *torqueP; + private: ParamT<float> *torqueP; /// Speeds of the wheels private: float wheelSpeed[2]; @@ -127,8 +127,8 @@ private: PhysicsEngine *physicsEngine; - private: Param<std::string> *leftJointNameP; - private: Param<std::string> *rightJointNameP; + private: ParamT<std::string> *leftJointNameP; + private: ParamT<std::string> *rightJointNameP; }; /** \} */ Modified: code/gazebo/trunk/server/controllers/ptz/generic/Generic_PTZ.cc =================================================================== --- code/gazebo/trunk/server/controllers/ptz/generic/Generic_PTZ.cc 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/controllers/ptz/generic/Generic_PTZ.cc 2008-09-01 18:56:47 UTC (rev 7000) @@ -58,10 +58,12 @@ this->panJoint = NULL; this->tiltJoint = NULL; - this->panJointNameP = new Param<std::string>("panJoint", "", 1); - this->tiltJointNameP = new Param<std::string>("tiltJoint", "", 1); - this->motionGainP = new Param<double>("motionGain",2,0); - this->forceP = new Param<double>("force",10,0); + Param::Begin(&this->parameters); + this->panJointNameP = new ParamT<std::string>("panJoint", "", 1); + this->tiltJointNameP = new ParamT<std::string>("tiltJoint", "", 1); + this->motionGainP = new ParamT<double>("motionGain",2,0); + this->forceP = new ParamT<double>("force",10,0); + Param::End(); } //////////////////////////////////////////////////////////////////////////////// Modified: code/gazebo/trunk/server/controllers/ptz/generic/Generic_PTZ.hh =================================================================== --- code/gazebo/trunk/server/controllers/ptz/generic/Generic_PTZ.hh 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/controllers/ptz/generic/Generic_PTZ.hh 2008-09-01 18:56:47 UTC (rev 7000) @@ -107,11 +107,11 @@ private: float cmdTilt; private: float cmdPan; - private: Param<double> *motionGainP; - private: Param<double> *forceP; + private: ParamT<double> *motionGainP; + private: ParamT<double> *forceP; - private: Param<std::string> *panJointNameP; - private: Param<std::string> *tiltJointNameP; + private: ParamT<std::string> *panJointNameP; + private: ParamT<std::string> *tiltJointNameP; }; /** /} */ Modified: code/gazebo/trunk/server/gui/GLWindow.cc =================================================================== --- code/gazebo/trunk/server/gui/GLWindow.cc 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/gui/GLWindow.cc 2008-09-01 18:56:47 UTC (rev 7000) @@ -33,6 +33,7 @@ #include <GL/glx.h> +#include "Param.hh" #include "Entity.hh" #include "OgreCamera.hh" #include "OgreCreator.hh" @@ -202,12 +203,11 @@ if (!this->mouseDrag) { Entity *ent = OgreAdaptor::Instance()->GetEntityAt(this->activeCamera, this->mousePos); + if (ent) { Simulator::Instance()->SetSelectedEntity( ent ); } - - } this->mouseDrag = false; Modified: code/gazebo/trunk/server/gui/Gui.cc =================================================================== --- code/gazebo/trunk/server/gui/Gui.cc 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/gui/Gui.cc 2008-09-01 18:56:47 UTC (rev 7000) @@ -51,8 +51,10 @@ { Fl::scheme("plastic"); - this->sizeP = new Param<Vector2<int> >("size", Vector2<int>(800, 600), 0); - this->posP = new Param<Vector2<int> >("pos",Vector2<int>(0,0),0); + Param::Begin(&this->parameters); + this->sizeP = new ParamT<Vector2<int> >("size", Vector2<int>(800, 600), 0); + this->posP = new ParamT<Vector2<int> >("pos",Vector2<int>(0,0),0); + Param::End(); // The order of creation matters! Menubar first, followed by FrameManager, // then statusbar Modified: code/gazebo/trunk/server/gui/Gui.hh =================================================================== --- code/gazebo/trunk/server/gui/Gui.hh 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/gui/Gui.hh 2008-09-01 18:56:47 UTC (rev 7000) @@ -85,8 +85,9 @@ private: bool hasFocus; - private: Param<Vector2<int> > *sizeP; - private: Param<Vector2<int> > *posP; + private: ParamT<Vector2<int> > *sizeP; + private: ParamT<Vector2<int> > *posP; + private: std::vector<Param*> parameters; }; } Modified: code/gazebo/trunk/server/gui/StatusBar.cc =================================================================== --- code/gazebo/trunk/server/gui/StatusBar.cc 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/gui/StatusBar.cc 2008-09-01 18:56:47 UTC (rev 7000) @@ -28,6 +28,7 @@ #include <FL/Fl_Value_Output.H> #include <FL/Fl_Output.H> #include <FL/Fl_Button.H> +#include <string.h> #include "Gui.hh" #include "Simulator.hh" Modified: code/gazebo/trunk/server/gui/Toolbar.cc =================================================================== --- code/gazebo/trunk/server/gui/Toolbar.cc 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/gui/Toolbar.cc 2008-09-01 18:56:47 UTC (rev 7000) @@ -42,33 +42,21 @@ Toolbar::Toolbar(int x, int y, int w, int h, const char *l) : Fl_Group(x,y,w,h,l) { - char *buffer = new char[256]; - this->box(FL_UP_BOX); - this->entityInfoGrp = new Fl_Group(x+10,y+20,w-20,25*3, "Entity"); + this->columnWidths[0] = 80; + this->columnWidths[1] = 120; + this->columnWidths[2] = 0; - // Camera Info Group - this->entityInfoGrp->box(FL_BORDER_BOX); + this->entityBrowser = new Fl_Hold_Browser(x+10, y+20, w-20, 25*5, "Attributes"); + this->entityBrowser->align(FL_ALIGN_TOP); + this->entityBrowser->column_char('~'); + this->entityBrowser->column_widths( columnWidths ); + this->entityBrowser->callback(&Toolbar::AttributeBrowserCB, this); - // Entity name output - x = this->entityInfoGrp->x()+50; - y = this->entityInfoGrp->y()+2; - this->entityName = new Fl_Output(x,y, this->entityInfoGrp->w()-55,20, "Name: "); - - y = this->entityName->y() + this->entityName->h() + 5; - this->entityPos = new Fl_Output(x,y, this->entityInfoGrp->w()-55,20, "XYZ: "); - - y = this->entityPos->y() + this->entityPos->h() + 5; - this->entityRot = new Fl_Output(x,y, this->entityInfoGrp->w()-55,20, "RPY: "); - - this->entityInfoGrp->end(); - this->end(); this->resizable(NULL); - - delete buffer; } Toolbar::~Toolbar() @@ -79,36 +67,84 @@ /// Update the toolbar data void Toolbar::Update() { - char *buffer = new char[256]; Entity *entity = Simulator::Instance()->GetSelectedEntity(); + std::vector<Param*> *parameters; + std::vector<Param*>::iterator iter; if (entity) { - Model *model = dynamic_cast<Model *>(entity); + //Model *model = dynamic_cast<Model *>(entity); - sprintf(buffer,"%s", entity->GetName().c_str()); - if (strcmp(buffer, this->entityName->value()) != 0) - this->entityName->value(buffer); + parameters = entity->GetParameters(); - if (model) + int i=0; + for (iter = parameters->begin(); iter != parameters->end(); iter++, i++) { - Pose3d pose = model->GetPose(); - Vector3 rpy = pose.rot.GetAsEuler(); + std::string value; + //boost::any anyValue = (*iter)->Get(); + //std::string typeName = (*iter)->GetTypename(); + std::string colorStr = ""; - sprintf(buffer,"%4.2f, %4.2f, %4.2f",pose.pos.x, pose.pos.y, pose.pos.z); - if (strcmp( buffer, this->entityPos->value()) != 0) - this->entityPos->value(buffer); + /*if ( i%2 == 0) + colorStr = "@B50"; + */ - sprintf(buffer,"%4.2f, %4.2f, %4.2f",rpy.x, rpy.y, rpy.z); - if (strcmp( buffer, this->entityRot->value()) != 0) - this->entityRot->value(buffer); + value = colorStr + "@b@s" + (*iter)->GetKey() + ":~" + colorStr + "@s"; + value += (*iter)->GetAsString(); + + // Convert the variable value to a string + /*if (typeName == typeid(float).name()) + value += boost::lexical_cast<std::string>( + boost::any_cast<float>( anyValue )); + else if (typeName == typeid(double).name()) + value += boost::lexical_cast<std::string>( + boost::any_cast<double>( anyValue )); + else if (typeName == typeid(int).name()) + value += boost::lexical_cast<std::string>( + boost::any_cast<int>( anyValue )); + else if (typeName == typeid(bool).name()) + value += boost::lexical_cast<std::string>( + boost::any_cast<bool>(anyValue) ); + else if (typeName == typeid(long).name()) + value += boost::lexical_cast<std::string>( + boost::any_cast<long>(anyValue) ); + else if (typeName == typeid(Quatern).name()) + value += boost::lexical_cast<std::string>( + boost::any_cast<Quatern>( anyValue )); + else if (typeName == typeid(Vector3).name()) + value += boost::lexical_cast<std::string>( + boost::any_cast<Vector3>( anyValue )); + else if (typeName == typeid(std::string).name()) + value += boost::any_cast<std::string>( anyValue ); + else + gzerr(0) << "Unknown typename[" << typeName << "]\n"; + */ + + if (!this->entityBrowser->text(i+1)) + { + this->entityBrowser->add( value.c_str() ); + } + else if (strcmp(this->entityBrowser->text(i+1), value.c_str()) != 0) + { + this->entityBrowser->text( i+1, value.c_str() ); + } } + + // Clear the remaining lines + while ( this->entityBrowser->text(i+1) != NULL ) + { + this->entityBrowser->text( i+1, "" ); + i++; + } + } - else - { - this->entityName->value(""); - this->entityPos->value(""); - this->entityRot->value(""); - } } + + +//////////////////////////////////////////////////////////////////////////////// +// Attribute browser callback +void Toolbar::AttributeBrowserCB( Fl_Widget * w, void *data) +{ + printf("Callback\n"); +} Modified: code/gazebo/trunk/server/gui/Toolbar.hh =================================================================== --- code/gazebo/trunk/server/gui/Toolbar.hh 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/gui/Toolbar.hh 2008-09-01 18:56:47 UTC (rev 7000) @@ -28,6 +28,7 @@ #define TOOLBAR_HH #include <FL/Fl_Group.H> +#include <FL/Fl_Hold_Browser.H> class Fl_Value_Output; class Fl_Output; @@ -48,12 +49,11 @@ /// \brief Update the toolbar data public: void Update(); - private: Fl_Group *entityInfoGrp; + public: static void AttributeBrowserCB( Fl_Widget * w, void *data); - private: Fl_Output *entityName; - private: Fl_Output *entityPos; - private: Fl_Output *entityRot; + private: Fl_Hold_Browser *entityBrowser; + private: int columnWidths[3]; }; } Modified: code/gazebo/trunk/server/physics/Body.cc =================================================================== --- code/gazebo/trunk/server/physics/Body.cc 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/physics/Body.cc 2008-09-01 18:56:47 UTC (rev 7000) @@ -64,8 +64,10 @@ this->bodyId = NULL; } - this->xyzP = new Param<Vector3>("xyz", Vector3(), 0); - this->rpyP = new Param<Quatern>("rpy", Quatern(), 0); + Param::Begin(&this->parameters); + this->xyzP = new ParamT<Vector3>("xyz", Vector3(), 0); + this->rpyP = new ParamT<Quatern>("rpy", Quatern(), 0); + Param::End(); } Modified: code/gazebo/trunk/server/physics/Body.hh =================================================================== --- code/gazebo/trunk/server/physics/Body.hh 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/physics/Body.hh 2008-09-01 18:56:47 UTC (rev 7000) @@ -172,8 +172,8 @@ private: Pose3d staticPose; private: Pose3d pose; - private: Param<Vector3> *xyzP; - private: Param<Quatern> *rpyP; + private: ParamT<Vector3> *xyzP; + private: ParamT<Quatern> *rpyP; }; /// \} Modified: code/gazebo/trunk/server/physics/BoxGeom.cc =================================================================== --- code/gazebo/trunk/server/physics/BoxGeom.cc 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/physics/BoxGeom.cc 2008-09-01 18:56:47 UTC (rev 7000) @@ -37,7 +37,9 @@ BoxGeom::BoxGeom(Body *body) : Geom(body) { - this->sizeP = new Param<Vector3>("size",Vector3(1,1,1),1); + Param::Begin(&this->parameters); + this->sizeP = new ParamT<Vector3>("size",Vector3(1,1,1),1); + Param::End(); } Modified: code/gazebo/trunk/server/physics/BoxGeom.hh =================================================================== --- code/gazebo/trunk/server/physics/BoxGeom.hh 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/physics/BoxGeom.hh 2008-09-01 18:56:47 UTC (rev 7000) @@ -87,7 +87,7 @@ /// \brief Set the size of the box public: void SetSize( Vector3 size ); - private: Param<Vector3> *sizeP; + private: ParamT<Vector3> *sizeP; }; /// \} Modified: code/gazebo/trunk/server/physics/CylinderGeom.cc =================================================================== --- code/gazebo/trunk/server/physics/CylinderGeom.cc 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/physics/CylinderGeom.cc 2008-09-01 18:56:47 UTC (rev 7000) @@ -35,7 +35,9 @@ CylinderGeom::CylinderGeom(Body *body) : Geom(body) { - this->sizeP = new Param<Vector2<double> >("size", Vector2<double>(1.0,1.0), 1); + Param::Begin(&this->parameters); + this->sizeP = new ParamT<Vector2<double> >("size", Vector2<double>(1.0,1.0), 1); + Param::End(); } ////////////////////////////////////////////////////////////////////////////// Modified: code/gazebo/trunk/server/physics/CylinderGeom.hh =================================================================== --- code/gazebo/trunk/server/physics/CylinderGeom.hh 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/physics/CylinderGeom.hh 2008-09-01 18:56:47 UTC (rev 7000) @@ -83,7 +83,7 @@ /// \brief Save child parameters protected: void SaveChild(std::string &prefix, std::ostream &stream); - private: Param<Vector2<double> > *sizeP; + private: ParamT<Vector2<double> > *sizeP; }; /// \} Modified: code/gazebo/trunk/server/physics/Geom.cc =================================================================== --- code/gazebo/trunk/server/physics/Geom.cc 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/physics/Geom.cc 2008-09-01 18:56:47 UTC (rev 7000) @@ -61,11 +61,13 @@ this->transparency = 0; - this->massP = new Param<double>("mass",0.001,0); - this->xyzP = new Param<Vector3>("xyz", Vector3(), 0); - this->rpyP = new Param<Quatern>("rpy", Quatern(), 0); - this->laserFiducialIdP = new Param<int>("laserFiducialId",-1,0); - this->laserRetroP = new Param<float>("laserRetro",-1,0); + Param::Begin(&this->parameters); + this->massP = new ParamT<double>("mass",0.001,0); + this->xyzP = new ParamT<Vector3>("xyz", Vector3(), 0); + this->rpyP = new ParamT<Quatern>("rpy", Quatern(), 0); + this->laserFiducialIdP = new ParamT<int>("laserFiducialId",-1,0); + this->laserRetroP = new ParamT<float>("laserRetro",-1,0); + Param::End(); } //////////////////////////////////////////////////////////////////////////////// Modified: code/gazebo/trunk/server/physics/Geom.hh =================================================================== --- code/gazebo/trunk/server/physics/Geom.hh 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/physics/Geom.hh 2008-09-01 18:56:47 UTC (rev 7000) @@ -162,14 +162,14 @@ /// mass of the body protected: dMass bodyMass; - private: Param<int> *laserFiducialIdP; - private: Param<float> *laserRetroP; + private: ParamT<int> *laserFiducialIdP; + private: ParamT<float> *laserRetroP; /// Mass as a double - protected: Param<double> *massP; + protected: ParamT<double> *massP; - private: Param<Vector3> *xyzP; - private: Param<Quatern> *rpyP; + private: ParamT<Vector3> *xyzP; + private: ParamT<Quatern> *rpyP; /// Special bounding box visual private: OgreVisual *bbVisual; Modified: code/gazebo/trunk/server/physics/HeightmapGeom.cc =================================================================== --- code/gazebo/trunk/server/physics/HeightmapGeom.cc 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/physics/HeightmapGeom.cc 2008-09-01 18:56:47 UTC (rev 7000) @@ -45,11 +45,13 @@ HeightmapGeom::HeightmapGeom(Body *body) : Geom(body) { - this->imageFilenameP = new Param<std::string>("image","",1); - this->worldTextureP = new Param<std::string>("worldTexture","",0); - this->detailTextureP = new Param<std::string>("detailTexture","",0); - this->sizeP = new Param<Vector3>("size",Vector3(10,10,10), 0); - this->offsetP = new Param<Vector3>("offset",Vector3(0,0,0), 0); + Param::Begin(&this->parameters); + this->imageFilenameP = new ParamT<std::string>("image","",1); + this->worldTextureP = new ParamT<std::string>("worldTexture","",0); + this->detailTextureP = new ParamT<std::string>("detailTexture","",0); + this->sizeP = new ParamT<Vector3>("size",Vector3(10,10,10), 0); + this->offsetP = new ParamT<Vector3>("offset",Vector3(0,0,0), 0); + Param::End(); } Modified: code/gazebo/trunk/server/physics/HeightmapGeom.hh =================================================================== --- code/gazebo/trunk/server/physics/HeightmapGeom.hh 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/physics/HeightmapGeom.hh 2008-09-01 18:56:47 UTC (rev 7000) @@ -124,11 +124,11 @@ private: std::vector<double> heights; - private: Param<std::string> *imageFilenameP; - private: Param<std::string> *worldTextureP; - private: Param<std::string> *detailTextureP; - private: Param<Vector3> *sizeP; - private: Param<Vector3> *offsetP; + private: ParamT<std::string> *imageFilenameP; + private: ParamT<std::string> *worldTextureP; + private: ParamT<std::string> *detailTextureP; + private: ParamT<Vector3> *sizeP; + private: ParamT<Vector3> *offsetP; }; Modified: code/gazebo/trunk/server/physics/Hinge2Joint.cc =================================================================== --- code/gazebo/trunk/server/physics/Hinge2Joint.cc 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/physics/Hinge2Joint.cc 2008-09-01 18:56:47 UTC (rev 7000) @@ -38,12 +38,14 @@ this->type = Joint::HINGE2; this->jointId = dJointCreateHinge2( worldId, NULL ); - this->axis1P = new Param<Vector3>("axis1",Vector3(0,0,1), 0); - this->axis2P = new Param<Vector3>("axis2",Vector3(0,0,1), 0); - this->loStop1P = new Param<Angle>("lowStop1",-M_PI,0); - this->hiStop1P = new Param<Angle>("highStop1",M_PI,0); - this->loStop2P = new Param<Angle>("lowStop2",-M_PI,0); - this->hiStop2P = new Param<Angle>("highStop2",M_PI,0); + Param::Begin(&this->parameters); + this->axis1P = new ParamT<Vector3>("axis1",Vector3(0,0,1), 0); + this->axis2P = new ParamT<Vector3>("axis2",Vector3(0,0,1), 0); + this->loStop1P = new ParamT<Angle>("lowStop1",-M_PI,0); + this->hiStop1P = new ParamT<Angle>("highStop1",M_PI,0); + this->loStop2P = new ParamT<Angle>("lowStop2",-M_PI,0); + this->hiStop2P = new ParamT<Angle>("highStop2",M_PI,0); + Param::End(); } Modified: code/gazebo/trunk/server/physics/Hinge2Joint.hh =================================================================== --- code/gazebo/trunk/server/physics/Hinge2Joint.hh 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/physics/Hinge2Joint.hh 2008-09-01 18:56:47 UTC (rev 7000) @@ -147,13 +147,13 @@ /// \brief Set the torque public: void SetTorque(double torque1, double torque2); - private: Param<Vector3> *axis1P; - private: Param<Angle> *loStop1P; - private: Param<Angle> *hiStop1P; + private: ParamT<Vector3> *axis1P; + private: ParamT<Angle> *loStop1P; + private: ParamT<Angle> *hiStop1P; - private: Param<Vector3> *axis2P; - private: Param<Angle> *loStop2P; - private: Param<Angle> *hiStop2P; + private: ParamT<Vector3> *axis2P; + private: ParamT<Angle> *loStop2P; + private: ParamT<Angle> *hiStop2P; }; /// \} Modified: code/gazebo/trunk/server/physics/HingeJoint.cc =================================================================== --- code/gazebo/trunk/server/physics/HingeJoint.cc 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/physics/HingeJoint.cc 2008-09-01 18:56:47 UTC (rev 7000) @@ -40,9 +40,11 @@ this->type = Joint::HINGE; this->jointId = dJointCreateHinge( worldId, NULL ); - this->axisP = new Param<Vector3>("axis",Vector3(0,1,0), 1); - this->loStopP = new Param<Angle>("lowStop",-M_PI,0); - this->hiStopP = new Param<Angle>("highStop",M_PI,0); + Param::Begin(&this->parameters); + this->axisP = new ParamT<Vector3>("axis",Vector3(0,1,0), 1); + this->loStopP = new ParamT<Angle>("lowStop",-M_PI,0); + this->hiStopP = new ParamT<Angle>("highStop",M_PI,0); + Param::End(); } Modified: code/gazebo/trunk/server/physics/HingeJoint.hh =================================================================== --- code/gazebo/trunk/server/physics/HingeJoint.hh 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/physics/HingeJoint.hh 2008-09-01 18:56:47 UTC (rev 7000) @@ -123,9 +123,9 @@ /// Set the torque of a joint. public: void SetTorque(double torque); - private: Param<Vector3> *axisP; - private: Param<Angle> *loStopP; - private: Param<Angle> *hiStopP; + private: ParamT<Vector3> *axisP; + private: ParamT<Angle> *loStopP; + private: ParamT<Angle> *hiStopP; }; /// \} Modified: code/gazebo/trunk/server/physics/Joint.cc =================================================================== --- code/gazebo/trunk/server/physics/Joint.cc 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/physics/Joint.cc 2008-09-01 18:56:47 UTC (rev 7000) @@ -40,14 +40,16 @@ this->visual = NULL; this->model = NULL; - this->nameP = new Param<std::string>("name","",1); - this->erpP = new Param<double>("erp",0.4,0); - this->cfmP = new Param<double>("cfm",10e-3,0); - this->suspensionCfmP = new Param<double>("suspensionCfm",0.0,0); - this->body1NameP = new Param<std::string>("body1",std::string(),1); - this->body2NameP = new Param<std::string>("body2",std::string(),1); - this->anchorBodyNameP = new Param<std::string>("anchor",std::string(),0); - this->anchorOffsetP = new Param<Vector3>("anchorOffset",Vector3(0,0,0), 0); + Param::Begin(&this->parameters); + this->nameP = new ParamT<std::string>("name","",1); + this->erpP = new ParamT<double>("erp",0.4,0); + this->cfmP = new ParamT<double>("cfm",10e-3,0); + this->suspensionCfmP = new ParamT<double>("suspensionCfm",0.0,0); + this->body1NameP = new ParamT<std::string>("body1",std::string(),1); + this->body2NameP = new ParamT<std::string>("body2",std::string(),1); + this->anchorBodyNameP = new ParamT<std::string>("anchor",std::string(),0); + this->anchorOffsetP = new ParamT<Vector3>("anchorOffset",Vector3(0,0,0), 0); + Param::End(); } Modified: code/gazebo/trunk/server/physics/Joint.hh =================================================================== --- code/gazebo/trunk/server/physics/Joint.hh 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/physics/Joint.hh 2008-09-01 18:56:47 UTC (rev 7000) @@ -150,15 +150,17 @@ private: Body *body2; /// Name of this joint - private: Param<std::string> *nameP; - private: Param<double> *erpP; - private: Param<double> *cfmP; - private: Param<double> *suspensionCfmP; - private: Param<std::string> *body1NameP; - private: Param<std::string> *body2NameP; - private: Param<std::string> *anchorBodyNameP; - private: Param<Vector3> *anchorOffsetP; + private: ParamT<std::string> *nameP; + private: ParamT<double> *erpP; + private: ParamT<double> *cfmP; + private: ParamT<double> *suspensionCfmP; + private: ParamT<std::string> *body1NameP; + private: ParamT<std::string> *body2NameP; + private: ParamT<std::string> *anchorBodyNameP; + private: ParamT<Vector3> *anchorOffsetP; + protected: std::vector<Param*> parameters; + private: OgreVisual *visual; private: Model *model; Modified: code/gazebo/trunk/server/physics/MapGeom.cc =================================================================== --- code/gazebo/trunk/server/physics/MapGeom.cc 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/physics/MapGeom.cc 2008-09-01 18:56:47 UTC (rev 7000) @@ -49,12 +49,14 @@ { this->root = new QuadNode(NULL); - this->negativeP = new Param<int>("negative", 0, 0); - this->thresholdP = new Param<double>( "threshold", 200.0, 0); - this->wallHeightP = new Param<double>( "height", 1.0, 0 ); - this->scaleP = new Param<double>("scale",1.0,0); - this->materialP = new Param<std::string>("material", "", 0); - this->granularityP = new Param<int>("granularity", 5, 0); + 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(); } Modified: code/gazebo/trunk/server/physics/MapGeom.hh =================================================================== --- code/gazebo/trunk/server/physics/MapGeom.hh 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/physics/MapGeom.hh 2008-09-01 18:56:47 UTC (rev 7000) @@ -125,21 +125,21 @@ private: void CreateBoxes(QuadNode *node); // The scale factor to apply to the geoms - private: Param<double> *scaleP; + private: ParamT<double> *scaleP; // Negative image? - private: Param<int> *negativeP; + private: ParamT<int> *negativeP; // Image color threshold used for extrusion - private: Param<double> *thresholdP; + private: ParamT<double> *thresholdP; // The color of the walls - private: Param<std::string> *materialP; + private: ParamT<std::string> *materialP; // The amount of acceptable error in the model - private: Param<int> *granularityP; + private: ParamT<int> *granularityP; - private: Param<double> *wallHeightP; + private: ParamT<double> *wallHeightP; private: Ogre::Image mapImage; Modified: code/gazebo/trunk/server/physics/PhysicsEngine.cc =================================================================== --- code/gazebo/trunk/server/physics/PhysicsEngine.cc 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/physics/PhysicsEngine.cc 2008-09-01 18:56:47 UTC (rev 7000) @@ -32,9 +32,11 @@ // Constructor PhysicsEngine::PhysicsEngine() { - this->gravityP = new Param<Vector3>("gravity",Vector3(0.0, -9.80665, 0.0), 0); - this->updateRateP = new Param<double>("maxUpdateRate", 0.0, 0); - this->stepTimeP = new Param<double>("stepTime",0.025,0); + Param::Begin(&this->parameters); + this->gravityP = new ParamT<Vector3>("gravity",Vector3(0.0, -9.80665, 0.0), 0); + this->updateRateP = new ParamT<double>("maxUpdateRate", 0.0, 0); + this->stepTimeP = new ParamT<double>("stepTime",0.025,0); + Param::End(); } //////////////////////////////////////////////////////////////////////////////// Modified: code/gazebo/trunk/server/physics/PhysicsEngine.hh =================================================================== --- code/gazebo/trunk/server/physics/PhysicsEngine.hh 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/physics/PhysicsEngine.hh 2008-09-01 18:56:47 UTC (rev 7000) @@ -120,15 +120,17 @@ public: double GetStepTime() const; /// The gravity vector - protected: Param<Vector3> *gravityP; + protected: ParamT<Vector3> *gravityP; /// time steps the physical engine will take /// how much time will pass on each update - protected: Param<double> *stepTimeP; + protected: ParamT<double> *stepTimeP; /// update rate of the physical engine, how many times /// it is called - protected: Param<double> *updateRateP; + protected: ParamT<double> *updateRateP; + + protected: std::vector<Param*> parameters; }; /** \}*/ Modified: code/gazebo/trunk/server/physics/PlaneGeom.cc =================================================================== --- code/gazebo/trunk/server/physics/PlaneGeom.cc 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/physics/PlaneGeom.cc 2008-09-01 18:56:47 UTC (rev 7000) @@ -40,16 +40,17 @@ PlaneGeom::PlaneGeom(Body *body) : Geom(body) { - this->normalP = new Param<Vector3>("normal",Vector3(0,0,1),0); - this->sizeP = new Param<Vector2<double> >("size", + Param::Begin(&this->parameters); + this->normalP = new ParamT<Vector3>("normal",Vector3(0,0,1),0); + this->sizeP = new ParamT<Vector2<double> >("size", Vector2<double>(1000, 1000), 0); - this->segmentsP = new Param<Vector2<double> >("segments", + this->segmentsP = new ParamT<Vector2<double> >("segments", Vector2<double>(10, 10), 0); - this->uvTileP = new Param<Vector2<double> >("uvTile", + this->uvTileP = new ParamT<Vector2<double> >("uvTile", Vector2<double>(1, 1), 0); - this->materialP = new Param<std::string>("material","",1); - this->castShadowsP = new Param<bool>("castShadows", false, 0); - + this->materialP = new ParamT<std::string>("material","",1); + this->castShadowsP = new ParamT<bool>("castShadows", false, 0); + Param::End(); } ////////////////////////////////////////////////////////////////////////////// Modified: code/gazebo/trunk/server/physics/PlaneGeom.hh =================================================================== --- code/gazebo/trunk/server/physics/PlaneGeom.hh 2008-09-01 02:30:34 UTC (rev 6999) +++ code/gazebo/trunk/server/physics/PlaneGeom.hh 2008-09-01 18:56:47 UTC (rev 7000) @@ -99,12 +99,12 @@ /// \brief Save child parameters protected: void SaveChild(std::string &prefix, std::ostream &stream); - private: Param<Vector3> *normalP; - private: Param<Vector2<double> > *sizeP; - private: Param<Vector2<double> > *segmentsP; - private: Param<Vector2<double> > *uvTileP; - private: Param<std::string> *materialP; - private: Param<bool> *castShadowsP; + private: ParamT<Vector3> *normalP; + private: ParamT<Vector2<double> > *sizeP; + private: ParamT<V... [truncated message content] |
From: <na...@us...> - 2008-09-09 06:30:49
|
Revision: 7007 http://playerstage.svn.sourceforge.net/playerstage/?rev=7007&view=rev Author: natepak Date: 2008-09-09 13:30:45 +0000 (Tue, 09 Sep 2008) Log Message: ----------- Applied patch 2101282 Modified Paths: -------------- code/gazebo/trunk/SConstruct code/gazebo/trunk/player/SConscript code/gazebo/trunk/server/SConscript code/gazebo/trunk/server/controllers/SConscript code/gazebo/trunk/server/controllers/actarray/SConscript code/gazebo/trunk/server/controllers/actarray/bandit/SConscript code/gazebo/trunk/server/controllers/audio/SConscript code/gazebo/trunk/server/controllers/camera/SConscript code/gazebo/trunk/server/controllers/camera/generic/SConscript code/gazebo/trunk/server/controllers/camera/stereo/SConscript code/gazebo/trunk/server/controllers/factory/SConscript code/gazebo/trunk/server/controllers/gripper/SConscript code/gazebo/trunk/server/controllers/gripper/pioneer2/SConscript code/gazebo/trunk/server/controllers/laser/SConscript code/gazebo/trunk/server/controllers/laser/sicklms200/SConscript code/gazebo/trunk/server/controllers/position2d/SConscript code/gazebo/trunk/server/controllers/position2d/differential/SConscript code/gazebo/trunk/server/controllers/position2d/steering/SConscript code/gazebo/trunk/server/controllers/ptz/SConscript code/gazebo/trunk/server/controllers/ptz/generic/SConscript code/gazebo/trunk/server/gui/SConscript code/gazebo/trunk/server/physics/Joint.cc code/gazebo/trunk/server/physics/SConscript code/gazebo/trunk/server/physics/ode/SConscript code/gazebo/trunk/server/rendering/SConscript code/gazebo/trunk/server/sensors/SConscript code/gazebo/trunk/server/sensors/camera/SConscript code/gazebo/trunk/server/sensors/ray/SConscript Modified: code/gazebo/trunk/SConstruct =================================================================== --- code/gazebo/trunk/SConstruct 2008-09-09 13:05:16 UTC (rev 7006) +++ code/gazebo/trunk/SConstruct 2008-09-09 13:30:45 UTC (rev 7007) @@ -164,14 +164,14 @@ # conf.env.Append(LIBS="boost_python") -staticObjs = [] +#staticObjs = [] sharedObjs = [] headers = [] # # Export the environment # -Export('env install_prefix version staticObjs sharedObjs headers subdirs') +Export('env install_prefix version sharedObjs headers subdirs') # # Process subdirectories @@ -183,13 +183,13 @@ # # Create the gazebo executable # -gazebo = env.Program('gazebo',staticObjs) +gazebo = env.Program('gazebo',sharedObjs) Depends(gazebo, 'libgazebo/libgazebo.so') # # Create static and shared libraries for the server # -libgazeboServerStatic = env.StaticLibrary('gazeboServer', staticObjs) +#libgazeboServerStatic = env.StaticLibrary('gazeboServer', staticObjs) libgazeboServerShared = env.SharedLibrary('gazeboServer', sharedObjs) # @@ -199,6 +199,6 @@ env.Install(install_prefix+'/bin',gazebo) env.Install(install_prefix+'/share/gazebo','Media') env.Install(install_prefix+'/include/gazebo',headers) -env.Install(install_prefix+'/lib',libgazeboServerStatic ) +#env.Install(install_prefix+'/lib',libgazeboServerStatic ) env.Install(install_prefix+'/lib',libgazeboServerShared ) env.Install(install_prefix+'/share/gazebo','worlds') Modified: code/gazebo/trunk/player/SConscript =================================================================== --- code/gazebo/trunk/player/SConscript 2008-09-09 13:05:16 UTC (rev 7006) +++ code/gazebo/trunk/player/SConscript 2008-09-09 13:30:45 UTC (rev 7007) @@ -1,5 +1,5 @@ import os -Import('env install_prefix staticObjs sharedObjs subdirs') +Import('env install_prefix sharedObjs subdirs') parseConfigs = ['pkg-config --cflags --libs playerc++'] @@ -40,14 +40,14 @@ if ('player' in subdirs): - staticPlayerObjs = playerEnv.StaticObject(sources) +#staticPlayerObjs = playerEnv.StaticObject(sources) sharedPlayerObjs = playerEnv.SharedObject(sources) - staticPlayerObjs.append(staticObjs); +#staticPlayerObjs.append(staticObjs); sharedPlayerObjs.append(sharedObjs); sharedLib = playerEnv.SharedLibrary('gazeboplugin', sharedPlayerObjs) - staticLib = playerEnv.StaticLibrary('gazeboplugin', staticPlayerObjs) +# staticLib = playerEnv.StaticLibrary('gazeboplugin', staticPlayerObjs) env.Install(install_prefix+'/lib', sharedLib) - env.Install(install_prefix+'/lib', staticLib) +#env.Install(install_prefix+'/lib', staticLib) Modified: code/gazebo/trunk/server/SConscript =================================================================== --- code/gazebo/trunk/server/SConscript 2008-09-09 13:05:16 UTC (rev 7006) +++ code/gazebo/trunk/server/SConscript 2008-09-09 13:30:45 UTC (rev 7007) @@ -1,5 +1,5 @@ #Import variables -Import('env install_prefix staticObjs sharedObjs headers') +Import('env install_prefix sharedObjs headers') dirs = Split('physics rendering sensors controllers gui')# bindings') @@ -46,5 +46,5 @@ '#/server/Param.hh' ] ) -staticObjs.append( env.StaticObject(sources) ) +#staticObjs.append( env.StaticObject(sources) ) sharedObjs.append( env.SharedObject(sources) ) Modified: code/gazebo/trunk/server/controllers/SConscript =================================================================== --- code/gazebo/trunk/server/controllers/SConscript 2008-09-09 13:05:16 UTC (rev 7006) +++ code/gazebo/trunk/server/controllers/SConscript 2008-09-09 13:30:45 UTC (rev 7007) @@ -1,5 +1,5 @@ #Import variable -Import('env staticObjs sharedObjs headers') +Import('env sharedObjs headers') dirs = Split('position2d laser camera factory gripper actarray ptz') @@ -17,5 +17,5 @@ '#/server/controllers/Controller.hh' ] ) -staticObjs.append(env.StaticObject(sources)) +#staticObjs.append(env.StaticObject(sources)) sharedObjs.append(env.SharedObject(sources)) Modified: code/gazebo/trunk/server/controllers/actarray/SConscript =================================================================== --- code/gazebo/trunk/server/controllers/actarray/SConscript 2008-09-09 13:05:16 UTC (rev 7006) +++ code/gazebo/trunk/server/controllers/actarray/SConscript 2008-09-09 13:30:45 UTC (rev 7007) @@ -1,5 +1,5 @@ #Import variable -Import('env staticObjs sharedObjs') +Import('env sharedObjs') dirs = Split('bandit') Modified: code/gazebo/trunk/server/controllers/actarray/bandit/SConscript =================================================================== --- code/gazebo/trunk/server/controllers/actarray/bandit/SConscript 2008-09-09 13:05:16 UTC (rev 7006) +++ code/gazebo/trunk/server/controllers/actarray/bandit/SConscript 2008-09-09 13:30:45 UTC (rev 7007) @@ -1,7 +1,7 @@ #Import variable -Import('env staticObjs sharedObjs') +Import('env sharedObjs') sources = Split('Bandit_Actarray.cc') -staticObjs.append(env.StaticObject(sources)) +#staticObjs.append(env.StaticObject(sources)) sharedObjs.append(env.SharedObject(sources)) Modified: code/gazebo/trunk/server/controllers/audio/SConscript =================================================================== --- code/gazebo/trunk/server/controllers/audio/SConscript 2008-09-09 13:05:16 UTC (rev 7006) +++ code/gazebo/trunk/server/controllers/audio/SConscript 2008-09-09 13:30:45 UTC (rev 7007) @@ -1,7 +1,7 @@ #Import variable -Import('env staticObjs sharedObjs') +Import('env sharedObjs') sources = Split('Audio.cc') -staticObjs.append(env.StaticObject(sources)) +#staticObjs.append(env.StaticObject(sources)) sharedObjs.append(env.SharedObject(sources)) Modified: code/gazebo/trunk/server/controllers/camera/SConscript =================================================================== --- code/gazebo/trunk/server/controllers/camera/SConscript 2008-09-09 13:05:16 UTC (rev 7006) +++ code/gazebo/trunk/server/controllers/camera/SConscript 2008-09-09 13:30:45 UTC (rev 7007) @@ -1,5 +1,5 @@ #Import variable -Import('env staticObjs sharedObjs') +Import('env sharedObjs') dirs = [ 'generic', Modified: code/gazebo/trunk/server/controllers/camera/generic/SConscript =================================================================== --- code/gazebo/trunk/server/controllers/camera/generic/SConscript 2008-09-09 13:05:16 UTC (rev 7006) +++ code/gazebo/trunk/server/controllers/camera/generic/SConscript 2008-09-09 13:30:45 UTC (rev 7007) @@ -1,7 +1,7 @@ #Import variable -Import('env staticObjs sharedObjs') +Import('env sharedObjs') sources = Split('Generic_Camera.cc') -staticObjs.append(env.StaticObject(sources)) +#staticObjs.append(env.StaticObject(sources)) sharedObjs.append(env.SharedObject(sources)) Modified: code/gazebo/trunk/server/controllers/camera/stereo/SConscript =================================================================== --- code/gazebo/trunk/server/controllers/camera/stereo/SConscript 2008-09-09 13:05:16 UTC (rev 7006) +++ code/gazebo/trunk/server/controllers/camera/stereo/SConscript 2008-09-09 13:30:45 UTC (rev 7007) @@ -1,7 +1,7 @@ #Import variable -Import('env staticObjs sharedObjs') +Import('env sharedObjs') sources = Split('Stereo_Camera.cc') -staticObjs.append(env.StaticObject(sources)) +#staticObjs.append(env.StaticObject(sources)) sharedObjs.append(env.SharedObject(sources)) Modified: code/gazebo/trunk/server/controllers/factory/SConscript =================================================================== --- code/gazebo/trunk/server/controllers/factory/SConscript 2008-09-09 13:05:16 UTC (rev 7006) +++ code/gazebo/trunk/server/controllers/factory/SConscript 2008-09-09 13:30:45 UTC (rev 7007) @@ -1,7 +1,7 @@ #Import variable -Import('env staticObjs sharedObjs') +Import('env sharedObjs') sources = Split('Factory.cc') -staticObjs.append(env.StaticObject(sources)) +#staticObjs.append(env.StaticObject(sources)) sharedObjs.append(env.SharedObject(sources)) Modified: code/gazebo/trunk/server/controllers/gripper/SConscript =================================================================== --- code/gazebo/trunk/server/controllers/gripper/SConscript 2008-09-09 13:05:16 UTC (rev 7006) +++ code/gazebo/trunk/server/controllers/gripper/SConscript 2008-09-09 13:30:45 UTC (rev 7007) @@ -1,5 +1,5 @@ #Import variable -Import('env staticObjs sharedObjs') +Import('env sharedObjs') dirs = Split('pioneer2') Modified: code/gazebo/trunk/server/controllers/gripper/pioneer2/SConscript =================================================================== --- code/gazebo/trunk/server/controllers/gripper/pioneer2/SConscript 2008-09-09 13:05:16 UTC (rev 7006) +++ code/gazebo/trunk/server/controllers/gripper/pioneer2/SConscript 2008-09-09 13:30:45 UTC (rev 7007) @@ -1,7 +1,7 @@ #Import variable -Import('env staticObjs sharedObjs') +Import('env sharedObjs') sources = Split('Pioneer2_Gripper.cc') -staticObjs.append(env.StaticObject(sources)) +#staticObjs.append(env.StaticObject(sources)) sharedObjs.append(env.SharedObject(sources)) Modified: code/gazebo/trunk/server/controllers/laser/SConscript =================================================================== --- code/gazebo/trunk/server/controllers/laser/SConscript 2008-09-09 13:05:16 UTC (rev 7006) +++ code/gazebo/trunk/server/controllers/laser/SConscript 2008-09-09 13:30:45 UTC (rev 7007) @@ -1,5 +1,5 @@ #Import variable -Import('env staticObjs sharedObjs') +Import('env sharedObjs') dirs = Split('sicklms200') Modified: code/gazebo/trunk/server/controllers/laser/sicklms200/SConscript =================================================================== --- code/gazebo/trunk/server/controllers/laser/sicklms200/SConscript 2008-09-09 13:05:16 UTC (rev 7006) +++ code/gazebo/trunk/server/controllers/laser/sicklms200/SConscript 2008-09-09 13:30:45 UTC (rev 7007) @@ -1,7 +1,7 @@ #Import variable -Import('env staticObjs sharedObjs') +Import('env sharedObjs') sources = Split('SickLMS200_Laser.cc') -staticObjs.append(env.StaticObject(sources)) +#staticObjs.append(env.StaticObject(sources)) sharedObjs.append(env.SharedObject(sources)) Modified: code/gazebo/trunk/server/controllers/position2d/SConscript =================================================================== --- code/gazebo/trunk/server/controllers/position2d/SConscript 2008-09-09 13:05:16 UTC (rev 7006) +++ code/gazebo/trunk/server/controllers/position2d/SConscript 2008-09-09 13:30:45 UTC (rev 7007) @@ -1,5 +1,5 @@ #Import variable -Import('env staticObjs sharedObjs') +Import('env sharedObjs') dirs = Split('differential steering') Modified: code/gazebo/trunk/server/controllers/position2d/differential/SConscript =================================================================== --- code/gazebo/trunk/server/controllers/position2d/differential/SConscript 2008-09-09 13:05:16 UTC (rev 7006) +++ code/gazebo/trunk/server/controllers/position2d/differential/SConscript 2008-09-09 13:30:45 UTC (rev 7007) @@ -1,7 +1,7 @@ #Import variable -Import('env staticObjs sharedObjs') +Import('env sharedObjs') sources = Split('Differential_Position2d.cc') -staticObjs.append(env.StaticObject(sources)) +#staticObjs.append(env.StaticObject(sources)) sharedObjs.append(env.SharedObject(sources)) Modified: code/gazebo/trunk/server/controllers/position2d/steering/SConscript =================================================================== --- code/gazebo/trunk/server/controllers/position2d/steering/SConscript 2008-09-09 13:05:16 UTC (rev 7006) +++ code/gazebo/trunk/server/controllers/position2d/steering/SConscript 2008-09-09 13:30:45 UTC (rev 7007) @@ -1,7 +1,7 @@ #Import variable -Import('env staticObjs sharedObjs') +Import('env sharedObjs') sources = Split('Steering_Position2d.cc Wheel.cc DriveWheel.cc FullWheel.cc') -staticObjs.append(env.StaticObject(sources)) +#staticObjs.append(env.StaticObject(sources)) sharedObjs.append(env.SharedObject(sources)) Modified: code/gazebo/trunk/server/controllers/ptz/SConscript =================================================================== --- code/gazebo/trunk/server/controllers/ptz/SConscript 2008-09-09 13:05:16 UTC (rev 7006) +++ code/gazebo/trunk/server/controllers/ptz/SConscript 2008-09-09 13:30:45 UTC (rev 7007) @@ -1,5 +1,5 @@ #Import variable -Import('env staticObjs sharedObjs') +Import('env sharedObjs') dirs = Split('generic') Modified: code/gazebo/trunk/server/controllers/ptz/generic/SConscript =================================================================== --- code/gazebo/trunk/server/controllers/ptz/generic/SConscript 2008-09-09 13:05:16 UTC (rev 7006) +++ code/gazebo/trunk/server/controllers/ptz/generic/SConscript 2008-09-09 13:30:45 UTC (rev 7007) @@ -1,7 +1,7 @@ #Import variable -Import('env staticObjs sharedObjs') +Import('env sharedObjs') sources = Split('Generic_PTZ.cc') -staticObjs.append(env.StaticObject(sources)) +#staticObjs.append(env.StaticObject(sources)) sharedObjs.append(env.SharedObject(sources)) Modified: code/gazebo/trunk/server/gui/SConscript =================================================================== --- code/gazebo/trunk/server/gui/SConscript 2008-09-09 13:05:16 UTC (rev 7006) +++ code/gazebo/trunk/server/gui/SConscript 2008-09-09 13:30:45 UTC (rev 7007) @@ -1,5 +1,5 @@ #Import variable -Import('env staticObjs sharedObjs headers') +Import('env sharedObjs headers') sources = ['Gui.cc', 'GLWindow.cc', @@ -21,5 +21,5 @@ 'server/gui/GLFrame.hh', ] ) -staticObjs.append(env.StaticObject(sources)) +#staticObjs.append(env.StaticObject(sources)) sharedObjs.append(env.SharedObject(sources)) Modified: code/gazebo/trunk/server/physics/Joint.cc =================================================================== --- code/gazebo/trunk/server/physics/Joint.cc 2008-09-09 13:05:16 UTC (rev 7006) +++ code/gazebo/trunk/server/physics/Joint.cc 2008-09-09 13:30:45 UTC (rev 7007) @@ -87,6 +87,9 @@ this->body2NameP->Load(node); this->anchorBodyNameP->Load(node); this->anchorOffsetP->Load(node); + this->erpP->Load(node); + this->cfmP->Load(node); + this->suspensionCfmP->Load(node); Body *body1 = this->model->GetBody( **(this->body1NameP)); Body *body2 = this->model->GetBody(**(this->body2NameP)); @@ -100,26 +103,13 @@ Vector3 anchorVec = anchorBody->GetPosition() + **(this->anchorOffsetP); - this->Attach(body1,body2); - - // Set the anchor vector - if (anchorBody) - { - this->SetAnchor(anchorVec); - //joint->SetAnchor(anchorBody->GetPosition()); - } - /*else - { - joint->SetAnchor(anchorVec); - this->bodies.erase(node->GetString("anchor",std::string(),0)); - }*/ - - // Set joint parameters this->SetParam(dParamSuspensionERP, **(this->erpP)); this->SetParam(dParamSuspensionCFM, **(this->suspensionCfmP)); this->SetParam(dParamCFM, **(this->cfmP)); + this->Attach(body1,body2); + /// Add a renderable for the joint this->visual = new OgreVisual(this->model->GetVisualNode()); this->visual->AttachMesh("joint_anchor"); @@ -140,6 +130,12 @@ this->line2->AddPoint(Vector3(0,0,0)); this->LoadChild(node); + + // Set the anchor vector + if (anchorBody) + { + this->SetAnchor(anchorVec); + } } //////////////////////////////////////////////////////////////////////////////// Modified: code/gazebo/trunk/server/physics/SConscript =================================================================== --- code/gazebo/trunk/server/physics/SConscript 2008-09-09 13:05:16 UTC (rev 7006) +++ code/gazebo/trunk/server/physics/SConscript 2008-09-09 13:30:45 UTC (rev 7007) @@ -1,5 +1,5 @@ #Import variable -Import('env staticObjs sharedObjs headers') +Import('env sharedObjs headers') dirs = Split('ode') @@ -47,5 +47,5 @@ 'server/physics/MapGeom.hh' ] ) -staticObjs.append( env.StaticObject(sources) ) +#staticObjs.append( env.StaticObject(sources) ) sharedObjs.append( env.SharedObject(sources) ) Modified: code/gazebo/trunk/server/physics/ode/SConscript =================================================================== --- code/gazebo/trunk/server/physics/ode/SConscript 2008-09-09 13:05:16 UTC (rev 7006) +++ code/gazebo/trunk/server/physics/ode/SConscript 2008-09-09 13:30:45 UTC (rev 7007) @@ -1,7 +1,7 @@ #Import variable -Import('env staticObjs sharedObjs') +Import('env sharedObjs') sources = Split('ODEPhysics.cc') -staticObjs.append(env.StaticObject('ODEPhysics.cc')) +#staticObjs.append(env.StaticObject('ODEPhysics.cc')) sharedObjs.append(env.SharedObject('ODEPhysics.cc')) Modified: code/gazebo/trunk/server/rendering/SConscript =================================================================== --- code/gazebo/trunk/server/rendering/SConscript 2008-09-09 13:05:16 UTC (rev 7006) +++ code/gazebo/trunk/server/rendering/SConscript 2008-09-09 13:30:45 UTC (rev 7007) @@ -1,5 +1,5 @@ #Import variable -Import('env staticObjs sharedObjs headers') +Import('env sharedObjs headers') sources = ['OgreCreator.cc', 'OgreAdaptor.cc', @@ -30,5 +30,5 @@ '#/server/rendering/UserCamera.hh' ] ) -staticObjs.append( env.StaticObject(sources) ) +#staticObjs.append( env.StaticObject(sources) ) sharedObjs.append( env.SharedObject(sources) ) Modified: code/gazebo/trunk/server/sensors/SConscript =================================================================== --- code/gazebo/trunk/server/sensors/SConscript 2008-09-09 13:05:16 UTC (rev 7006) +++ code/gazebo/trunk/server/sensors/SConscript 2008-09-09 13:30:45 UTC (rev 7007) @@ -1,5 +1,5 @@ #Import variable -Import('env staticObjs sharedObjs headers') +Import('env sharedObjs headers') dirs = Split('camera ray') @@ -14,5 +14,5 @@ 'server/sensors/Sensor.hh' ] ) -staticObjs.append( env.StaticObject(sources) ) +#staticObjs.append( env.StaticObject(sources) ) sharedObjs.append( env.SharedObject(sources) ) Modified: code/gazebo/trunk/server/sensors/camera/SConscript =================================================================== --- code/gazebo/trunk/server/sensors/camera/SConscript 2008-09-09 13:05:16 UTC (rev 7006) +++ code/gazebo/trunk/server/sensors/camera/SConscript 2008-09-09 13:30:45 UTC (rev 7007) @@ -1,10 +1,10 @@ #Import variable -Import('env staticObjs sharedObjs') +Import('env sharedObjs') sources = [ 'MonoCameraSensor.cc', 'StereoCameraSensor.cc' ] -staticObjs.append(env.StaticObject(sources)) +#staticObjs.append(env.StaticObject(sources)) sharedObjs.append(env.SharedObject(sources)) Modified: code/gazebo/trunk/server/sensors/ray/SConscript =================================================================== --- code/gazebo/trunk/server/sensors/ray/SConscript 2008-09-09 13:05:16 UTC (rev 7006) +++ code/gazebo/trunk/server/sensors/ray/SConscript 2008-09-09 13:30:45 UTC (rev 7007) @@ -1,7 +1,7 @@ #Import variable -Import('env staticObjs sharedObjs') +Import('env sharedObjs') sources = Split('RaySensor.cc') -staticObjs.append(env.StaticObject(sources)) +#staticObjs.append(env.StaticObject(sources)) sharedObjs.append(env.SharedObject(sources)) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-09-09 07:06:13
|
Revision: 7010 http://playerstage.svn.sourceforge.net/playerstage/?rev=7010&view=rev Author: natepak Date: 2008-09-09 14:06:20 +0000 (Tue, 09 Sep 2008) Log Message: ----------- Applied patch 1998580 and 1998581 Modified Paths: -------------- code/gazebo/trunk/libgazebo/Iface.cc code/gazebo/trunk/libgazebo/gazebo.h code/gazebo/trunk/player/GazeboDriver.cc code/gazebo/trunk/player/SConscript code/gazebo/trunk/server/controllers/SConscript code/gazebo/trunk/server/physics/Joint.cc code/gazebo/trunk/server/physics/Joint.hh Added Paths: ----------- code/gazebo/trunk/player/OpaqueInterface.cc code/gazebo/trunk/player/OpaqueInterface.hh code/gazebo/trunk/server/controllers/opaque/ code/gazebo/trunk/server/controllers/opaque/SConscript code/gazebo/trunk/server/controllers/opaque/jointforce/ code/gazebo/trunk/server/controllers/opaque/jointforce/JointForce.cc code/gazebo/trunk/server/controllers/opaque/jointforce/JointForce.hh code/gazebo/trunk/server/controllers/opaque/jointforce/SConscript Modified: code/gazebo/trunk/libgazebo/Iface.cc =================================================================== --- code/gazebo/trunk/libgazebo/Iface.cc 2008-09-09 13:44:47 UTC (rev 7009) +++ code/gazebo/trunk/libgazebo/Iface.cc 2008-09-09 14:06:20 UTC (rev 7010) @@ -57,6 +57,7 @@ GZ_REGISTER_IFACE("actarray", ActarrayIface); GZ_REGISTER_IFACE("ptz", PTZIface); GZ_REGISTER_IFACE("stereocamera", StereoCameraIface); +GZ_REGISTER_IFACE("opaque", OpaqueIface); ////////////////////////////////////////////////////////////////////////////// // Create an interface Modified: code/gazebo/trunk/libgazebo/gazebo.h =================================================================== --- code/gazebo/trunk/libgazebo/gazebo.h 2008-09-09 13:44:47 UTC (rev 7009) +++ code/gazebo/trunk/libgazebo/gazebo.h 2008-09-09 14:06:20 UTC (rev 7010) @@ -1413,6 +1413,63 @@ /** \} */ /// \} +/***************************************************************************/ +/// \addtogroup libgazebo_iface +/// \{ +/** \defgroup opaque_iface opaque + + \brief Interface for arbitrary data transfer +The opaque interface can transmit any data + +\{ + */ +/// Maximum amount of data we will be sending. 8MB is the maximum dictated by Player +#define GAZEBO_MAX_OPAQUE_DATA 1024*1024*8 +/// \brief opaque data +class OpaqueData +{ + public: GazeboData head; + + /// The length of the data (in bytes) + public: uint32_t data_count; + + /// The data we will be sending + public: uint8_t data[GAZEBO_MAX_OPAQUE_DATA]; +}; + + +/// \brief Opaque interface +class OpaqueIface : public Iface +{ + /// \brief Constructor + public: OpaqueIface():Iface("opaque", sizeof(OpaqueIface)+sizeof(OpaqueData)) {} + + /// \brief Destructor + public: virtual ~OpaqueIface() {this->data = NULL;} + + /// \brief Create the server + public: virtual void Create(Server *server, std::string id) + { + Iface::Create(server,id); + this->data = (OpaqueData*)this->mMap; + } + + /// \brief Open the iface + public: virtual void Open(Client *client, std::string id) + { + Iface::Open(client,id); + this->data = (OpaqueData*)this->mMap; + } + + /// Pointer to the opaque data + public: OpaqueData *data; +}; + +/** \} */ +/// \} + + + } Modified: code/gazebo/trunk/player/GazeboDriver.cc =================================================================== --- code/gazebo/trunk/player/GazeboDriver.cc 2008-09-09 13:44:47 UTC (rev 7009) +++ code/gazebo/trunk/player/GazeboDriver.cc 2008-09-09 14:06:20 UTC (rev 7010) @@ -37,6 +37,7 @@ #include "FiducialInterface.hh" #include "Position3dInterface.hh" #include "ActarrayInterface.hh" +#include "OpaqueInterface.hh" #include "PTZInterface.hh" #include "GripperInterface.hh" @@ -295,6 +296,11 @@ ifsrc = new ActarrayInterface( playerAddr, this, cf, section ); break; + case PLAYER_OPAQUE_CODE: + if (!player_quiet_startup) printf(" an opaque interface.\n"); + ifsrc = new OpaqueInterface( playerAddr, this, cf, section ); + break; + case PLAYER_PTZ_CODE: if (!player_quiet_startup) printf(" a ptz interface.\n"); ifsrc = new PTZInterface( playerAddr, this, cf, section ); @@ -314,13 +320,7 @@ if (!player_quiet_startup) printf(" a sonar interface.\n"); ifsrc = new SonarInterface( playerAddr, this, cf, section ); break; - case PLAYER_TRUTH_CODE: - if (!player_quiet_startup) printf(" a truth interface.\n"); - ifsrc = new TruthInterface( playerAddr, this, cf, section ); - break; - - case PLAYER_GPS_CODE: if (!player_quiet_startup) printf(" a gps interface.\n"); ifsrc = new GpsInterface( playerAddr, this, cf, section ); Added: code/gazebo/trunk/player/OpaqueInterface.cc =================================================================== --- code/gazebo/trunk/player/OpaqueInterface.cc (rev 0) +++ code/gazebo/trunk/player/OpaqueInterface.cc 2008-09-09 14:06:20 UTC (rev 7010) @@ -0,0 +1,146 @@ +/* + * 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: Opaque Interface for Player + * Author: Benjamin Kloster + * Date: 13 March 2008 + */ + +/** +@addtogroup player +@par Opaque Interface +*/ +/* TODO +Do we need these? +- PLAYER_OPAQUE_DATA_STATE +- PLAYER_OPAQUE_CMD_DATA +- PLAYER_OPAQUE_REQ_DATA +*/ +#include <math.h> + +#include "GazeboError.hh" +#include "gazebo.h" +#include "GazeboDriver.hh" +#include "OpaqueInterface.hh" + +using namespace gazebo; + +/////////////////////////////////////////////////////////////////////////////// +// Constructor +OpaqueInterface::OpaqueInterface(player_devaddr_t addr, + GazeboDriver *driver, ConfigFile *cf, int section) + : GazeboInterface(addr, driver, cf, section) +{ + // Get the ID of the interface + this->gz_id = (char*) calloc(1024, sizeof(char)); + strcat(this->gz_id, GazeboClient::prefixId); + strcat(this->gz_id, cf->ReadString(section, "gz_id", "")); + + // Allocate a Position Interface + this->iface = new OpaqueIface(); + + this->datatime = -1; +} + +/////////////////////////////////////////////////////////////////////////////// +// Destructor +OpaqueInterface::~OpaqueInterface() +{ + // Release this interface + delete this->iface; +} + +/////////////////////////////////////////////////////////////////////////////// +// Handle all messages. This is called from GazeboDriver +int OpaqueInterface::ProcessMessage(QueuePointer &respQueue, + player_msghdr_t *hdr, void *data) +{ + if (this->iface->Lock(1)) + { + // nothing yet + return 0; + } + else + this->Unsubscribe(); + + return -1; +} + +/////////////////////////////////////////////////////////////////////////////// +// Update this interface, publish new info. This is +// called from GazeboDriver::Update +void OpaqueInterface::Update() +{ + player_opaque_data_t data; + struct timeval ts; + + memset(&data, 0, sizeof(data)); + if (this->iface->Lock(1)) + { + // Only Update when new data is present + if (this->iface->data->head.time > this->datatime) + { + this->datatime = this->iface->data->head.time; + + ts.tv_sec = (int) (this->iface->data->head.time); + ts.tv_usec = (int) (fmod(this->iface->data->head.time, 1) * 1e6); + + data.data_count = this->iface->data->data_count; + data.data = this->iface->data->data; + + this->driver->Publish( this->device_addr, + PLAYER_MSGTYPE_DATA, + PLAYER_OPAQUE_DATA_STATE, + (void*)&data, sizeof(data), &this->datatime ); + } + + this->iface->Unlock(); + } + else + this->Unsubscribe(); +} + + +/////////////////////////////////////////////////////////////////////////////// +// Open a SHM interface when a subscription is received. This is called from +// GazeboDriver::Subscribe +void OpaqueInterface::Subscribe() +{ + // Open the interface + try + { + this->iface->Open(GazeboClient::client, this->gz_id); + } + catch (std::string e) + { + //std::ostringstream stream; + std::cout <<"Error Subscribing to Gazebo Opaque Interface\n" + << e << "\n"; + //gzthrow(stream.str()); + exit(0); + } +} + +/////////////////////////////////////////////////////////////////////////////// +// Close a SHM interface. This is called from GazeboDriver::Unsubscribe +void OpaqueInterface::Unsubscribe() +{ + this->iface->Close(); +} Added: code/gazebo/trunk/player/OpaqueInterface.hh =================================================================== --- code/gazebo/trunk/player/OpaqueInterface.hh (rev 0) +++ code/gazebo/trunk/player/OpaqueInterface.hh 2008-09-09 14:06:20 UTC (rev 7010) @@ -0,0 +1,83 @@ +/* + * 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: Opaque Interface for Player + * Author: Benjamin Kloster + * Date: 13 March 2008 + */ + +#ifndef OPAQUEINTERFACE_HH +#define OPAQUEINTERFACE_HH + +#include "GazeboInterface.hh" + +namespace gazebo +{ + +/// \addtogroup player_iface +/// \{ +/// \defgroup opaque_player Opaque Interface +/// \brief Opaque Player interface +/// \{ + +// Forward declarations +class OpaqueIface; + +/// \brief Opaque Player interface +class OpaqueInterface : public GazeboInterface +{ + /// \brief Constructor + public: OpaqueInterface(player_devaddr_t addr, GazeboDriver *driver, + ConfigFile *cf, int section); + + /// \brief Destructor + public: virtual ~OpaqueInterface(); + + /// \brief Handle all messages. This is called from GazeboDriver + public: virtual int ProcessMessage(QueuePointer &respQueue, + player_msghdr_t *hdr, void *data); + + /// \brief Update this interface, publish new info. + public: virtual void Update(); + + /// \brief Open a SHM interface when a subscription is received. + /// This is called fromGazeboDriver::Subscribe + public: virtual void Subscribe(); + + /// \brief Close a SHM interface. This is called from + /// GazeboDriver::Unsubscribe + public: virtual void Unsubscribe(); + + private: OpaqueIface *iface; + + /// \brief Gazebo id. This needs to match and ID in a Gazebo WorldFile + private: char *gz_id; + + /// \brief Timestamp on last data update + private: double datatime; +}; + +/// \} +/// \} + + +} + +#endif Modified: code/gazebo/trunk/player/SConscript =================================================================== --- code/gazebo/trunk/player/SConscript 2008-09-09 13:44:47 UTC (rev 7009) +++ code/gazebo/trunk/player/SConscript 2008-09-09 14:06:20 UTC (rev 7010) @@ -14,6 +14,7 @@ 'CameraInterface.cc', 'FiducialInterface.cc', 'PTZInterface.cc', + 'OpaqueInterface.cc', 'ActarrayInterface.cc', 'GripperInterface.cc' ] Modified: code/gazebo/trunk/server/controllers/SConscript =================================================================== --- code/gazebo/trunk/server/controllers/SConscript 2008-09-09 13:44:47 UTC (rev 7009) +++ code/gazebo/trunk/server/controllers/SConscript 2008-09-09 14:06:20 UTC (rev 7010) @@ -1,7 +1,7 @@ #Import variable Import('env sharedObjs headers') -dirs = Split('position2d laser camera factory gripper actarray ptz') +dirs = Split('position2d laser camera factory gripper actarray ptz opaque') if env['with_audio'] == 'yes': dirs+=Split('audio') Added: code/gazebo/trunk/server/controllers/opaque/SConscript =================================================================== --- code/gazebo/trunk/server/controllers/opaque/SConscript (rev 0) +++ code/gazebo/trunk/server/controllers/opaque/SConscript 2008-09-09 14:06:20 UTC (rev 7010) @@ -0,0 +1,7 @@ +#Import variable +Import('env sharedObjs') + +dirs = Split('jointforce contact') + +for subdir in dirs : + SConscript('%s/SConscript' % subdir) Added: code/gazebo/trunk/server/controllers/opaque/jointforce/JointForce.cc =================================================================== --- code/gazebo/trunk/server/controllers/opaque/jointforce/JointForce.cc (rev 0) +++ code/gazebo/trunk/server/controllers/opaque/jointforce/JointForce.cc 2008-09-09 14:06:20 UTC (rev 7010) @@ -0,0 +1,121 @@ +/* + * 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: Joint Force controller + * Author: Benjamin Kloster + * Date: 13 March 2008 + */ + +#include "Global.hh" +#include "XMLConfig.hh" +#include "Model.hh" +#include "World.hh" +#include "gazebo.h" +#include "GazeboError.hh" +#include "ControllerFactory.hh" +#include "Simulator.hh" +#include "JointForce.hh" + +using namespace gazebo; + +GZ_REGISTER_STATIC_CONTROLLER("jointforce", JointForce); + +//////////////////////////////////////////////////////////////////////////////// +// Constructor +JointForce::JointForce(Entity *parent ) + : Controller(parent) +{ + this->myParent = dynamic_cast<Model*>(this->parent); + + if (!this->myParent) + gzthrow("ControllerStub controller requires a Model as its parent"); +} + +//////////////////////////////////////////////////////////////////////////////// +// Destructor +JointForce::~JointForce() +{ +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void JointForce::LoadChild(XMLConfigNode *node) +{ + XMLConfigNode *jNode; + Joint *joint; + std::string jointName; + dJointFeedback *jFeedback = new dJointFeedback; + int i =0; + this->myIface = dynamic_cast<OpaqueIface*>(this->ifaces[0]); + if (!this->myIface) { + gzthrow("JointForce controller requires an OpaqueIface"); + } + jNode = node->GetChild("joint"); + while(jNode && i < GAZEBO_JOINTFORCE_CONTROLLER_MAX_FEEDBACKS) + { + jointName = jNode->GetString("name","",1); + joint = this->myParent->GetJoint(jointName); + jFeedback = joint->GetFeedback(); + if(jFeedback) { + this->jointfeedbacks[i] = jFeedback; + i++; + } + jNode = jNode->GetNext("joint"); + } + this->n_joints = i; +} + +//////////////////////////////////////////////////////////////////////////////// +// Initialize the controller +void JointForce::InitChild() +{ + //this->myIface->data->data = new uint8_t[sizeof(dJointFeedback)*n_joints]; +} + +//////////////////////////////////////////////////////////////////////////////// +// Update the controller +void JointForce::UpdateChild() +{ + this->myIface->Lock(1); + this->myIface->data->head.time = Simulator::Instance()->GetSimTime(); + // Let me explain this number: each joint reports 4 vectors: Force and torque + // on each jointed object, respectively. These vectors have 4 elements: x,y,z + // and a fourth one. So we transmit 4 dReals per vector. + this->myIface->data->data_count = n_joints*4*4*sizeof(dReal); + + for(int i=0; i< n_joints; i++) { + // Copy vector for force on first object + memcpy(this->myIface->data->data + (i*4 + 0)*4*sizeof(dReal), this->jointfeedbacks[i]->f1, 4*sizeof(dReal)); + // Copy vector for torque on first object + memcpy(this->myIface->data->data + (i*4 + 1)*4*sizeof(dReal), this->jointfeedbacks[i]->t1, 4*sizeof(dReal)); + // Copy vector for force on second object + memcpy(this->myIface->data->data + (i*4 + 2)*4*sizeof(dReal), this->jointfeedbacks[i]->f2, 4*sizeof(dReal)); + // Copy vector for torque on second object + memcpy(this->myIface->data->data + (i*4 + 3)*4*sizeof(dReal), this->jointfeedbacks[i]->t2, 4*sizeof(dReal)); + } + this->myIface->Unlock(); +} + +//////////////////////////////////////////////////////////////////////////////// +// Finalize the controller +void JointForce::FiniChild() +{ +} Added: code/gazebo/trunk/server/controllers/opaque/jointforce/JointForce.hh =================================================================== --- code/gazebo/trunk/server/controllers/opaque/jointforce/JointForce.hh (rev 0) +++ code/gazebo/trunk/server/controllers/opaque/jointforce/JointForce.hh 2008-09-09 14:06:20 UTC (rev 7010) @@ -0,0 +1,92 @@ +/* + * 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: Joint Force Controller + * Author: Benjamin Kloster + * Date: 13 March 2008 + */ +#ifndef JOINTFORCE_CONTROLLER_HH +#define JOINTFORCE_CONTROLLER_HH + +/// Maximum number of joints that can be watched by one controller +#define GAZEBO_JOINTFORCE_CONTROLLER_MAX_FEEDBACKS 16 + +#include "Controller.hh" +#include "Entity.hh" +#include <ode/ode.h> +#include <sys/time.h> + + +namespace gazebo +{ +/// \addtogroup gazebo_controller +/// \{ +/** \defgroup jointforce_controller jointforce + + \brief A controller that measures forces and torques exerted by joints + + \{ +*/ + +/// \brief A JointForce controller +class JointForce : public Controller +{ + /// Constructor + public: JointForce(Entity *parent ); + + /// Destructor + public: virtual ~JointForce(); + + /// Load the controller + /// \param node XML config node + /// \return 0 on success + protected: virtual void LoadChild(XMLConfigNode *node); + + /// Init the controller + /// \return 0 on success + protected: virtual void InitChild(); + + /// Update the controller + /// \return 0 on success + protected: virtual void UpdateChild(); + + /// Finalize the controller + /// \return 0 on success + protected: virtual void FiniChild(); + + /// The parent Model + private: Model *myParent; + + /// The Iface. The dJointFeedback structs are rather arbitrary, so we use an Opaque Interface + private: OpaqueIface *myIface; + /// The joint feedbacks + private: dJointFeedback *jointfeedbacks[GAZEBO_JOINTFORCE_CONTROLLER_MAX_FEEDBACKS]; + /// The number of joints we are watching + private: int n_joints; +}; + +/** \} */ +/// \} + +} + +#endif + Added: code/gazebo/trunk/server/controllers/opaque/jointforce/SConscript =================================================================== --- code/gazebo/trunk/server/controllers/opaque/jointforce/SConscript (rev 0) +++ code/gazebo/trunk/server/controllers/opaque/jointforce/SConscript 2008-09-09 14:06:20 UTC (rev 7010) @@ -0,0 +1,7 @@ +#Import variable +Import('env sharedObjs') + +sources = Split('JointForce.cc') + +#staticObjs.append(env.StaticObject(sources)) +sharedObjs.append(env.SharedObject(sources)) Modified: code/gazebo/trunk/server/physics/Joint.cc =================================================================== --- code/gazebo/trunk/server/physics/Joint.cc 2008-09-09 13:44:47 UTC (rev 7009) +++ code/gazebo/trunk/server/physics/Joint.cc 2008-09-09 14:06:20 UTC (rev 7010) @@ -49,6 +49,7 @@ this->body2NameP = new ParamT<std::string>("body2",std::string(),1); this->anchorBodyNameP = new ParamT<std::string>("anchor",std::string(),0); this->anchorOffsetP = new ParamT<Vector3>("anchorOffset",Vector3(0,0,0), 0); + this->provideFeedbackP = new ParamT<bool>("provideFeedback", false, 0); Param::End(); } @@ -66,6 +67,7 @@ delete this->body2NameP; delete this->anchorBodyNameP; delete this->anchorOffsetP; + delete this->provideFeedbackP; } @@ -90,6 +92,7 @@ this->erpP->Load(node); this->cfmP->Load(node); this->suspensionCfmP->Load(node); + this->provideFeedbackP->Load(node); Body *body1 = this->model->GetBody( **(this->body1NameP)); Body *body2 = this->model->GetBody(**(this->body2NameP)); @@ -129,6 +132,12 @@ this->line2->AddPoint(Vector3(0,0,0)); this->line2->AddPoint(Vector3(0,0,0)); + if (**this->provideFeedbackP) + { + this->feedback = new dJointFeedback; + dJointSetFeedback(this->jointId, this->feedback); + } + this->LoadChild(node); // Set the anchor vector @@ -337,6 +346,13 @@ } //////////////////////////////////////////////////////////////////////////////// +/// Get the feedback data structure for this joint, if set +dJointFeedback *Joint::GetFeedback() +{ + return dJointGetFeedback(this->jointId); +} + +//////////////////////////////////////////////////////////////////////////////// /// Get the high stop of an axis(index). double Joint::GetHighStop(int index) { Modified: code/gazebo/trunk/server/physics/Joint.hh =================================================================== --- code/gazebo/trunk/server/physics/Joint.hh 2008-09-09 13:44:47 UTC (rev 7009) +++ code/gazebo/trunk/server/physics/Joint.hh 2008-09-09 14:06:20 UTC (rev 7010) @@ -131,6 +131,9 @@ /// \brief Get the CFM of this joint public: double GetCFM(); + /// \brief Get the feedback data structure for this joint, if set + public: dJointFeedback *GetFeedback(); + /// \brief Get the high stop of an axis(index). public: double GetHighStop(int index=0); @@ -158,7 +161,11 @@ private: ParamT<std::string> *body2NameP; private: ParamT<std::string> *anchorBodyNameP; private: ParamT<Vector3> *anchorOffsetP; + private: ParamT<bool> *provideFeedbackP; + /// Feedback data for this joint + private: dJointFeedback *feedback; + protected: std::vector<Param*> parameters; private: OgreVisual *visual; This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-09-09 07:25:29
|
Revision: 7011 http://playerstage.svn.sourceforge.net/playerstage/?rev=7011&view=rev Author: natepak Date: 2008-09-09 14:25:39 +0000 (Tue, 09 Sep 2008) Log Message: ----------- Applied patch 1905901 Modified Paths: -------------- code/gazebo/trunk/libgazebo/gazebo.h code/gazebo/trunk/server/controllers/position2d/SConscript Added Paths: ----------- code/gazebo/trunk/server/controllers/position2d/holonome3sw/ code/gazebo/trunk/server/controllers/position2d/holonome3sw/Holonome3Sw_Position2d.cc code/gazebo/trunk/server/controllers/position2d/holonome3sw/Holonome3Sw_Position2d.hh code/gazebo/trunk/server/controllers/position2d/holonome3sw/SConscript code/gazebo/trunk/worlds/models/wizbot.model code/gazebo/trunk/worlds/wizbot.world Modified: code/gazebo/trunk/libgazebo/gazebo.h =================================================================== --- code/gazebo/trunk/libgazebo/gazebo.h 2008-09-09 14:06:20 UTC (rev 7010) +++ code/gazebo/trunk/libgazebo/gazebo.h 2008-09-09 14:25:39 UTC (rev 7011) @@ -346,8 +346,6 @@ /// Type of model that owns this interface public: std::string modelType; - - }; /** \} */ Modified: code/gazebo/trunk/server/controllers/position2d/SConscript =================================================================== --- code/gazebo/trunk/server/controllers/position2d/SConscript 2008-09-09 14:06:20 UTC (rev 7010) +++ code/gazebo/trunk/server/controllers/position2d/SConscript 2008-09-09 14:25:39 UTC (rev 7011) @@ -1,12 +1,7 @@ #Import variable Import('env sharedObjs') -dirs = Split('differential steering') +dirs = Split('differential steering holonome3sw') for subdir in dirs : SConscript('%s/SConscript' % subdir) - -#sources = Split('') - -#staticObjs.append(env.StaticObject(sources)) -#sharedObjs.append(env.SharedObject(sources)) Added: code/gazebo/trunk/server/controllers/position2d/holonome3sw/Holonome3Sw_Position2d.cc =================================================================== --- code/gazebo/trunk/server/controllers/position2d/holonome3sw/Holonome3Sw_Position2d.cc (rev 0) +++ code/gazebo/trunk/server/controllers/position2d/holonome3sw/Holonome3Sw_Position2d.cc 2008-09-09 14:25:39 UTC (rev 7011) @@ -0,0 +1,269 @@ +/* + * 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: Position2d controller for a Holonome3Sw drive. + * Author: Christian Gagneraud (ch'Gans), based on Differential_Position2d.cc + * Date: 21 Feb 2007 + * SVN info: $Id$ + */ + +#include "XMLConfig.hh" +#include "Model.hh" +#include "Global.hh" +#include "HingeJoint.hh" +#include "World.hh" +#include "Simulator.hh" +#include "gazebo.h" +#include "GazeboError.hh" +#include "ControllerFactory.hh" +#include "Holonome3Sw_Position2d.hh" + +using namespace gazebo; + +GZ_REGISTER_STATIC_CONTROLLER("holonome3sw_position2d", Holonome3Sw_Position2d); + +enum {RIGHT, LEFT}; + +//////////////////////////////////////////////////////////////////////////////// +// Constructor +Holonome3Sw_Position2d::Holonome3Sw_Position2d(Entity *parent ) + : Controller(parent) +{ + this->myParent = dynamic_cast<Model*>(this->parent); + + if (!this->myParent) + gzthrow("Holonome3Sxw_Position2d controller requires a Model as its parent"); + + ResetData(); +} + +//////////////////////////////////////////////////////////////////////////////// +// Destructor +Holonome3Sw_Position2d::~Holonome3Sw_Position2d() +{ +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void Holonome3Sw_Position2d::LoadChild(XMLConfigNode *node) +{ + this->myIface = dynamic_cast<PositionIface*>(this->ifaces[0]); + + if (!this->myIface) + gzthrow("Holonome3Sw_Position2d controller requires a PositionIface"); + + // Get wheels child + node = node->GetChild("wheels"); + if (!node) + gzthrow("Holonome3Sw_Position2d controller requires a <wheels> node"); + + // Get default params + float distdef = node->GetFloat("distance", 0.0, 0); + float radiusdef = node->GetFloat("radius", 0.0, 0); + float maxtorquedef = node->GetFloat("torque", 0.0, 0); + float betadef = node->GetFloat("beta", 0.0, 0); + float gammadef = node->GetFloat("gamma", 0.0, 0); + + // Get params for each wheels + for (size_t i = 0; i < 3; ++i) + { + std::ostringstream sw; sw << "swedish" << i; + XMLConfigNode *lnode = node->GetChild(sw.str()); + if (!lnode) + gzthrow("The controller couldn't get swedish wheels " + sw.str()); + + std::string joint = lnode->GetString("joint", "", 1); + this->joint[i] = dynamic_cast<HingeJoint*>(this->myParent->GetJoint(joint)); + if (!this->joint[i]) + gzthrow("The controller couldn't get hinge joint for " + sw.str()); + + this->ALPHA[i] = lnode->GetFloat("alpha", 1000000, 1); + if (this->ALPHA[i] == 1000000) + gzthrow("The controller could't get alpha param for " + sw.str()); + + this->DIST[i] = lnode->GetFloat("distance", distdef, 0); + if (!this->DIST[i]) + gzthrow("The controller could't get distance param for " + sw.str()); + + this->BETA[i] = lnode->GetFloat("beta", betadef, 0); + + this->GAMMA[i] = lnode->GetFloat("gamma", gammadef, 0); + + this->RADIUS[i] = lnode->GetFloat("radius", radiusdef, 0); + if (!this->RADIUS[i]) + gzthrow("The controller could't get radius param for " + sw.str()); + + this->MAXTORQUE[i] = lnode->GetFloat("torque", maxtorquedef, 0); + if (!this->MAXTORQUE[i]) + gzthrow("The controller could't get torque param for " + sw.str()); + + // Pre-compute some constants + A[i] = (2*3.14159265358979)*fmodf(ALPHA[i]+BETA[i]+GAMMA[i], 360)/360; + L[i] = DIST[i]*cos((2*3.14159265358979)*fmodf(BETA[i]+GAMMA[i], 360)/360); + R[i] = RADIUS[i]*cos((2*3.14159265358979)*fmodf(GAMMA[i], 360)/360); + } + +#if 0 + std::cout << "Holonomous robot, here are the params:" << std::endl; + for (size_t i = 0; i < 3; ++i) + { + std::cout << " - swedish" << i << std::endl; + std::cout << " alpha = " << ALPHA[i] << std::endl; + std::cout << " beta = " << BETA[i] << std::endl; + std::cout << " gamma = " << GAMMA[i] << std::endl; + std::cout << " dist = " << DIST[i] << std::endl; + std::cout << " radius = " << RADIUS[i] << std::endl; + std::cout << " max-torque = " << MAXTORQUE[i] << std::endl; + std::cout << " A = " << A[i] << std::endl; + std::cout << " L = " << L[i] << std::endl; + std::cout << " R = " << R[i] << std::endl; + std::cout << std::endl; + } +#endif +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +/*void Holonome3Sw_Position2d::SaveChild(XMLConfigNode *node) +{ + node = node->GetChild("wheels"); + if (!node) + gzthrow("No such child node: wheels"); + for (size_t i = 0; i < 3; ++i) + { + std::ostringstream sw; sw << "swedish" << i; + XMLConfigNode *lnode = node->GetChild(sw.str()); + if (!lnode) + gzthrow("No such child node wheels/" + sw.str()); + lnode->SetValue("alpha", this->ALPHA[i]); + lnode->SetValue("distance", this->DIST[i]); + lnode->SetValue("beta", this->BETA[i]); + lnode->SetValue("gamma", this->GAMMA[i]); + lnode->SetValue("radius", this->RADIUS[i]); + lnode->SetValue("max-torque", this->MAXTORQUE[i]); + // "joint" ??? + } +}*/ + +//////////////////////////////////////////////////////////////////////////////// +// Initialize the controller +void Holonome3Sw_Position2d::InitChild() +{ + ResetData(); +} + +void Holonome3Sw_Position2d::ResetChild() +{ + ResetData(); +} + +//////////////////////////////////////////////////////////////////////////////// +// Update the controller +void Holonome3Sw_Position2d::UpdateChild() +{ + this->GetPositionCmd(); + + //std::cout << "Anchors: {"; + for (size_t i = 0; i < 3; ++i) + { + //std::cout << "[" << this->joint[i]->GetAnchor() << "] "; + if (this->enableMotors) + { + this->joint[i]->SetParam( dParamVel, this->PhiP[i]); + this->joint[i]->SetParam( dParamFMax, this->MAXTORQUE[i] ); + } + else + { + this->joint[i]->SetParam( dParamVel, 0 ); + this->joint[i]->SetParam( dParamVel, 0 ); + } + } + //std::cout << "}" << std::endl; + this->PutPositionData(); +} + +//////////////////////////////////////////////////////////////////////////////// +// Finalize the controller +void Holonome3Sw_Position2d::FiniChild() +{ +} + +//////////////////////////////////////////////////////////////////////////////// +// Reset internal data to 0 +void Holonome3Sw_Position2d::ResetData() +{ + for (size_t i = 0; i < 3; ++i) + { + Xi[i] = 0; + XiP[i] = 0; + PhiP[i] = 0; + } + this->enableMotors = true; +} + +////////////////////////////////////////////////////////////////////////////// +// Get commands from the external interface +void Holonome3Sw_Position2d::GetPositionCmd() +{ + if (this->myIface->Lock(1)) + { + double vx = this->myIface->data->cmdVelocity.pos.x; + double vy = this->myIface->data->cmdVelocity.pos.y; + double va = this->myIface->data->cmdVelocity.yaw; + for (size_t i = 0; i < 3; ++i) + { + this->PhiP[i] = -(1/R[i])*(-sin(A[i])*vx + + cos(A[i])*vy + + L[i]*va); + } + this->enableMotors = this->myIface->data->cmdEnableMotors > 0; + this->myIface->Unlock(); +#if 0 + std::cout << "Xi=[" << Xi[0] << ", " << Xi[1] << ", " << Xi[2] << "]; "; + std::cout << "vx=" << vx <<", vy=" << vy << ", va=" << va << "; "; + std::cout << "PhiP=[" << PhiP[0] << ", " << PhiP[1] << ", " << PhiP[2] << "];\n"; +#endif + } +} + +////////////////////////////////////////////////////////////////////////////// +// Update the data in the interface +void Holonome3Sw_Position2d::PutPositionData() +{ + if (this->myIface->Lock(1)) + { + // TODO: Data timestamp + this->myIface->data->head.time = Simulator::Instance()->GetSimTime(); + + this->myIface->data->pose.pos.x = this->Xi[0]; + this->myIface->data->pose.pos.y = this->Xi[1]; + this->myIface->data->pose.yaw = NORMALIZE(this->Xi[2]); + + this->myIface->data->velocity.pos.x = 0; + this->myIface->data->velocity.pos.y = 0; + this->myIface->data->velocity.yaw = 0; + + // TODO + this->myIface->data->stall = 0; + + this->myIface->Unlock(); + } +} Added: code/gazebo/trunk/server/controllers/position2d/holonome3sw/Holonome3Sw_Position2d.hh =================================================================== --- code/gazebo/trunk/server/controllers/position2d/holonome3sw/Holonome3Sw_Position2d.hh (rev 0) +++ code/gazebo/trunk/server/controllers/position2d/holonome3sw/Holonome3Sw_Position2d.hh 2008-09-09 14:25:39 UTC (rev 7011) @@ -0,0 +1,211 @@ +/* + * 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: Position2d controller for a Holonome3Sw drive. + * Author: Christian Gagneraud (ch'Gans), based on Differential_Position2d.hh + * Date: 21 Feb 2007 + * SVN: $Id$ + */ + +#ifndef HOLONOME3SW_POSITION2D_HH +#define HOLONOME3SW_POSITION2D_HH + +#include <map> + +#include "Controller.hh" + +namespace gazebo +{ + class HingeJoint; + class Entity; + class PositionIface; + +/// \addtogroup gazebo_controller +/// \{ +/** \defgroup holonome3sw_position2d holonome3sw_position2d + + \brief Position2D controller for an holonomous robot using 3 swedish + wheels. + + This is a controller that simulates the motion of an holonomous + robot using 3 swedish wheels (such as WizBot). + + Kinematics model: + \verbatim + J1.R(Th).XiP + J2.PhiP = 0 + + J1 = [ -sin(A1) cos(A1) L1 ] + [ -sin(A2) cos(A2) L2 ] + [ -sin(A3) cos(A3) L3 ] + + J2 = [ R1 0 0 ] + [ 0 R2 0 ] + [ 0 0 R3 ] + + R(t) = [ cos(t) sin(t) 0 ] + [ -sin(t) cos(t) 0 ] + [ 0 0 1 ] + + With: + A1 = alpha1+beta1+gamma1; + L1 = l1*cos(beta1+gamma1) + R1 = r1*cos(gamma1) + \endverbatim + + \verbatim + <controller:holonome3sw_position2d name="controller-name"> + <wheels> + <-- Global to all wheels, optional --> + <distance>d</distance> + <-- Global to all wheels, optional --> + <radius>r</radius> + <-- Global to all wheels, optional --> + <max-torque>t</max-torque> + <-- Global to all wheels, optional --> + <beta>b</beta> + <-- Global to all wheels, optional --> + <gamma>g</gamma> + <-- 1st wheel --> + <swedish0> + <joint-name>joint-name</joint-name> + <alpha>a</alpha> + <-- default to wheels/distance --> + <distance>d</distance> + <-- default to wheels/radius --> + <radius>r</radius> + <-- default to wheels/max-torque --> + <max-torque>max-torque</max-torque> + <-- default to wheels/beta if exist or to zero --> + <beta>b</beta> + <-- default to wheels/beta if exist or to zero --> + <gamma>g</gamma> + </swedish0> + <-- 2nd wheel --> + <swedish1> + ... + </swedish> + <-- 3rd wheel --> + <swedish2> + ... + </swedish> + </wheels> + <interface:position name="iface-name"/> + </controller:holonome3sw_position2d> + \endverbatim + + Where: + - P: Reference point on the robot frame. + - A: Point representing the center of a wheel + - distance, alpha: Polar coordinates of A in the robot frame. + - beta: Orientation of the plane of the wheel with respect with (PA). + - gamma: Direction, with respect to the wheel plane, of the zero + component of the velocity of the contact point. + + \{ +*/ + +/// \brief WizBot Position2D controller. +/// This is a controller that simulates a WizBot motion +class Holonome3Sw_Position2d : public Controller +{ + /// Constructor + public: Holonome3Sw_Position2d(Entity *parent ); + + /// Destructor + public: virtual ~Holonome3Sw_Position2d(); + + /// Load the controller + /// \param node XML config node + protected: virtual void LoadChild(XMLConfigNode *node); + + /// Init the controller + protected: virtual void InitChild(); + + /// \brief Reset the controller + protected: void ResetChild(); + + /// Update the controller + protected: virtual void UpdateChild(); + + /// Finalize the controller + protected: virtual void FiniChild(); + + + /// Update the data in the interface + private: void PutPositionData(); + + /// Get the position command from libgazebo + private: void GetPositionCmd(); + + /// Reset internal datas + private: void ResetData(); + + /// The Position interface + private: PositionIface *myIface; + + /// The parent Model + private: Model *myParent; + + + // The wheel joints. + private: HingeJoint *joint[3]; + + /// Polar coordinate of the center of the wheels + private: float DIST[3]; + + /// Polar coordinate of the center of the wheels + private: float ALPHA[3]; + + /// Orientation of the plane of the wheel + private: float BETA[3]; + + /// Direction of the zero component of the velocity of the contact point. + private: float GAMMA[3]; + + /// Radius of the wheels + private: float RADIUS[3]; + + /// Max torque to applied to the wheels + private: float MAXTORQUE[3]; + + // Robot posture coordinates in the world: x, y, theta + private: float Xi[3]; + // Robot speed coordinates in the world: vx, vy, vtheta + private: float XiP[3]; + // Rotation speed coordinates (wheels, rad/s) + private: float PhiP[3]; + + /// True = enable motors + private: bool enableMotors; + +private: + float A[3], L[3], R[3]; + +}; + +/** \} */ +/// \} + +} + +#endif + Added: code/gazebo/trunk/server/controllers/position2d/holonome3sw/SConscript =================================================================== --- code/gazebo/trunk/server/controllers/position2d/holonome3sw/SConscript (rev 0) +++ code/gazebo/trunk/server/controllers/position2d/holonome3sw/SConscript 2008-09-09 14:25:39 UTC (rev 7011) @@ -0,0 +1,6 @@ +#Import variable +Import('env sharedObjs') + +sources = Split('Holonome3Sw_Position2d.cc') + +sharedObjs.append(env.SharedObject(sources)) Added: code/gazebo/trunk/worlds/models/wizbot.model =================================================================== --- code/gazebo/trunk/worlds/models/wizbot.model (rev 0) +++ code/gazebo/trunk/worlds/models/wizbot.model 2008-09-09 14:25:39 UTC (rev 7011) @@ -0,0 +1,234 @@ +<?xml version="1.0"?> + +<!-- Generic WizBot Model --> +<model:physical + name="default_wizbot_model" + xmlns:xi="http://www.w3.org/2001/XInclude" + xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model" + xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" + xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body" + xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom" + xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint" + > + + <xyz>0 0 0.0</xyz> + <rpy>0.0 0.0 0.0</rpy> + + <canonicalBody>chassis_body</canonicalBody> + + <body:cylinder name="chassis_body"> + <xyz>0.0 0.0 0.0</xyz> + <rpy>0.0 0.0 0.0</rpy> + + <geom:cylinder name="chassis_geom"> + <xyz>0 0 0</xyz> + <size>0.140 0.2</size> + <mass>2</mass> + <visual> + <size>0.280 0.280 0.2</size> + <mesh>unit_cylinder</mesh> + <material>Gazebo/RustyBarrel</material> + </visual> + </geom:cylinder> + </body:cylinder> + + <body:cylinder name="wheel0"> + <xyz>0.175 0 -0.0725</xyz> + <rpy>0 90 0</rpy> + + <geom:cylinder name="wheel0_geom"> + <size>0.075 0.05</size> + <mass>0.237</mass> + + <visual> + <rpy>-90 0 0</rpy> + <size>0.125 0.05 0.125</size> + <mesh>Pioneer2dx/tire.mesh</mesh> + <material>Gazebo/Black</material> + </visual> + + <visual> + <rpy>-90 0 0</rpy> + <size>0.073 0.05 0.073</size> + <mesh>Pioneer2at/wheel.mesh</mesh> + <material>Gazebo/Gold</material> + </visual> + + <visual> + <rpy>0 0 0</rpy> + <xyz>0 0 -0.015</xyz> + <size>0.04 0.04 0.08</size> + <mesh>unit_cylinder</mesh> + <material>Gazebo/Black</material> + </visual> + + <surface> + <type>0</type> + <mu1>1.7</mu1> + <bounce>0.1</bounce> + <bounceVel>0.04</bounceVel> + </surface> + + <surface> + <type>1</type> + <mu1>1.7</mu1> + <bounce>0.1</bounce> + <bounceVel>0.04</bounceVel> + </surface> + + <surface> + <type>2</type> + <mu1>0.01</mu1> + <mu2>1.7</mu2> + <mu1Dir>0 0 1</mu1Dir> + <bounce>0.1</bounce> + <bounceVel>0.04</bounceVel> + </surface> + + </geom:cylinder> + </body:cylinder> + + <body:cylinder name="wheel1"> + <xyz>-0.0875 0.151554445662 -0.0725</xyz> + <rpy>0 90 120</rpy> + + <geom:cylinder name="wheel1_geom"> + <size>0.075 0.05</size> + <mass>0.237</mass> + + <visual> + <rpy>-90 0 0</rpy> + <size>0.125 0.05 0.125</size> + <mesh>Pioneer2dx/tire.mesh</mesh> + <material>Gazebo/Black</material> + </visual> + + <visual> + <rpy>-90 0 0</rpy> + <size>0.073 0.05 0.073 </size> + <mesh>Pioneer2at/wheel.mesh</mesh> + <material>Gazebo/Gold</material> + </visual> + + <visual> + <rpy>0 0 0</rpy> + <xyz>0 0 -0.015</xyz> + <size>0.04 0.04 0.08 </size> + <mesh>unit_cylinder</mesh> + <material>Gazebo/Black</material> + </visual> + + <surface> + <type>0</type> + <mu1>1.7</mu1> + <bounce>0.1</bounce> + <bounceVel>0.04</bounceVel> + </surface> + + <surface> + <type>1</type> + <mu1>1.7</mu1> + <bounce>0.1</bounce> + <bounceVel>0.04</bounceVel> + </surface> + + <surface> + <type>2</type> + <mu1>0.01</mu1> + <mu2>1.7</mu2> + <mu1Dir>0 0 1</mu1Dir> + <bounce>0.1</bounce> + <bounceVel>0.04</bounceVel> + </surface> + + </geom:cylinder> + </body:cylinder> + + <body:cylinder name="wheel2"> + <xyz>-0.0875 -0.151554445662 -0.0725</xyz> + <rpy>0 90 240</rpy> + + <geom:cylinder name="wheel2_geom"> + <size>0.075 0.05</size> + <mass>0.237</mass> + + <visual> + <rpy>-90 0 0</rpy> + <size>0.125 0.05 0.125</size> + <mesh>Pioneer2dx/tire.mesh</mesh> + <material>Gazebo/Black</material> + </visual> + + <visual> + <rpy>-90 0 0</rpy> + <size>0.073 0.05 0.073 </size> + <mesh>Pioneer2at/wheel.mesh</mesh> + <material>Gazebo/Gold</material> + </visual> + + <visual> + <rpy>0 0 0</rpy> + <xyz>0 0 -0.015</xyz> + <size>0.04 0.04 0.08 </size> + <mesh>unit_cylinder</mesh> + <material>Gazebo/Black</material> + </visual> + + <surface> + <type>0</type> + <mu1>1.7</mu1> + <bounce>0.1</bounce> + <bounceVel>0.04</bounceVel> + </surface> + + <surface> + <type>1</type> + <mu1>1.7</mu1> + <bounce>0.1</bounce> + <bounceVel>0.04</bounceVel> + </surface> + + <surface> + <type>2</type> + <mu1>0.01</mu1> + <mu2>1.7</mu2> + <mu1Dir>0 0 1</mu1Dir> + <bounce>0.1</bounce> + <bounceVel>0.04</bounceVel> + </surface> + + </geom:cylinder> + </body:cylinder> + + + <joint:hinge name="wheel0_hinge"> + <body1>wheel0</body1> + <body2>chassis_body</body2> + <anchor>wheel0</anchor> + <anchorOffset>-0.04 0 0</anchorOffset> + <axis>1 0 0 </axis> + <erp>0.8</erp> + <cfm>10e-5</cfm> + </joint:hinge> + + <joint:hinge name="wheel1_hinge"> + <body1>wheel1</body1> + <body2>chassis_body</body2> + <anchor>wheel1</anchor> + <anchorOffset>0.02 -0.034641016 0</anchorOffset> + <axis>-0.5 0.866025404 0</axis> + <erp>0.8</erp> + <cfm>10e-5</cfm> + </joint:hinge> + + <joint:hinge name="wheel2_hinge"> + <body1>wheel2</body1> + <body2>chassis_body</body2> + <anchor>wheel2</anchor> + <anchorOffset>0.02 0.034641016 0</anchorOffset> + <axis>-0.5 -0.866025404 0</axis> + <erp>0.8</erp> + <cfm>10e-5</cfm> + </joint:hinge> + +</model:physical> Added: code/gazebo/trunk/worlds/wizbot.world =================================================================== --- code/gazebo/trunk/worlds/wizbot.world (rev 0) +++ code/gazebo/trunk/worlds/wizbot.world 2008-09-09 14:25:39 UTC (rev 7011) @@ -0,0 +1,175 @@ +<?xml version="1.0"?> + +<gazebo:world + xmlns:xi="http://www.w3.org/2001/XInclude" + xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz" + xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model" + xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" + xmlns:window="http://playerstage.sourceforge.net/gazebo/xmlschema/#window" + xmlns:param="http://playerstage.sourceforge.net/gazebo/xmlschema/#param" + xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body" + xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom" + xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint" + xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" + xmlns:ui="http://playerstage.sourceforge.net/gazebo/xmlschema/#ui" + xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering" + xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" + xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics" > + + <verbosity>5</verbosity> + + <physics:ode> + <stepTime>0.03</stepTime> + <gravity>0 0 -9.81</gravity> + <cfm>0.008</cfm> + <erp>0.2</erp> + </physics:ode> + + <rendering:gui> + <type>fltk</type> + <size>800 600</size> + <pos>0 0</pos> + </rendering:gui> + + <rendering:ogre> + <ambient>1.0 1.0 1.0 1.0</ambient> + <sky> + <material>Gazebo/CloudySky</material> + </sky> + + </rendering:ogre> + + <!-- Ground Plane --> + <model:physical name="plane1_model"> + <xyz>0 0 0</xyz> + <rpy>0 0 0</rpy> + <static>true</static> + + <body:plane name="plane1_body"> + <geom:plane name="plane1_geom"> + <normal>0 0 1</normal> + <size>2000 2000</size> + <segments>10 10</segments> + <uvTile>100 100</uvTile> + <material>Gazebo/GrassFloor</material> + <surface> + <mu1>10</mu1> + </surface> + </geom:plane> + </body:plane> + </model:physical> + + <!-- Main Camera --> + <model:physical name="cam1_model"> + <xyz>-2 0 2</xyz> + <rpy>0 25 0</rpy> + <static>true</static> + <body:empty name="cam1_body"> + <sensor:camera name="cam1_sensor"> + <imageSize>640 480</imageSize> + <hfov>60</hfov> + <nearClip>0.1</nearClip> + <farClip>100</farClip> + <saveFrames>false</saveFrames> + <saveFramePath>frames</saveFramePath> + <controller:generic_camera name="controller1"> + <interface:camera name="camera_iface_0"/> + </controller:generic_camera> + </sensor:camera> + </body:empty> + </model:physical> + + <model:physical name="cylinder1_model"> + <xyz>1 1 0.5</xyz> + <rpy>0 0 0</rpy> + <body:cylinder name="cylinder1_body"> + <geom:cylinder name="cylinder1_geom"> + <size>0.5 1</size> + <mass>1</mass> + <visual> + <mesh>unit_cylinder</mesh> + <material>Gazebo/RustyBarrel</material> + </visual> + </geom:cylinder> + </body:cylinder> + </model:physical> + + + <model:physical name="pioneer2dx_model1"> + <xyz>1 0 0.145</xyz> + <rpy>0.0 0.0 0.0</rpy> + + <controller:differential_position2d name="controller1"> + <leftJoint>left_wheel_hinge</leftJoint> + <rightJoint>right_wheel_hinge</rightJoint> + <wheelSeparation>0.34</wheelSeparation> + <wheelDiameter>0.15</wheelDiameter> + <torque>5</torque> + <interface:position name="position_iface_1"/> + </controller:differential_position2d> + + <model:physical name="laser"> + <xyz>0.15 0 0.18</xyz> + + <attach> + <parentBody>chassis_body</parentBody> + <myBody>laser_body</myBody> + </attach> + + <include embedded="true"> + <xi:include href="models/sicklms200.model" /> + </include> + </model:physical> + + <include embedded="true"> + <xi:include href="models/pioneer2dx.model" /> + </include> + </model:physical> + + <model:physical name="wizbot_model1"> + <xyz>0 0 0.1</xyz> + <rpy>0 0 0</rpy> + + <controller:holonome3sw_position2d name="controller1"> + + <wheels> + <radius>0.075</radius> + <distance>0.175</distance> + <torque>2</torque> + <swedish0> + <joint>wheel0_hinge</joint> + <alpha>0</alpha> + </swedish0> + <swedish1> + <joint>wheel1_hinge</joint> + <alpha>120</alpha> + </swedish1> + <swedish2> + <joint>wheel2_hinge</joint> + <alpha>240</alpha> + </swedish2> + </wheels> + + <interface:position name="position_iface_0"/> + + </controller:holonome3sw_position2d> + + <include embedded="true"> + <xi:include href="models/wizbot.model" /> + </include> + + </model:physical> + + + <!-- White Directional light --> + <model:renderable name="directional_white"> + <light> + <type>directional</type> + <direction>0 -0.6 -0.4</direction> + <diffuseColor>1.0 1.0 1.0</diffuseColor> + <specularColor>0.2 0.2 0.2</specularColor> + <attenuation>1000 1.0 0.0 0</attenuation> + </light> + </model:renderable> + +</gazebo:world> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-09-09 15:04:27
|
Revision: 7016 http://playerstage.svn.sourceforge.net/playerstage/?rev=7016&view=rev Author: natepak Date: 2008-09-09 22:04:34 +0000 (Tue, 09 Sep 2008) Log Message: ----------- Added contact sensor, bumper controller, and bumper interface Modified Paths: -------------- code/gazebo/trunk/SConstruct code/gazebo/trunk/libgazebo/Iface.cc code/gazebo/trunk/libgazebo/gazebo.h code/gazebo/trunk/player/GazeboDriver.cc code/gazebo/trunk/player/SConscript code/gazebo/trunk/server/controllers/SConscript code/gazebo/trunk/server/physics/ContactParams.hh code/gazebo/trunk/server/physics/Geom.cc code/gazebo/trunk/server/physics/ode/ODEPhysics.cc code/gazebo/trunk/server/sensors/SConscript Added Paths: ----------- code/gazebo/trunk/player/BumperInterface.cc code/gazebo/trunk/player/BumperInterface.hh code/gazebo/trunk/server/controllers/bumper/ code/gazebo/trunk/server/controllers/bumper/SConscript code/gazebo/trunk/server/controllers/bumper/generic/ code/gazebo/trunk/server/controllers/bumper/generic/Generic_Bumper.cc code/gazebo/trunk/server/controllers/bumper/generic/Generic_Bumper.hh code/gazebo/trunk/server/controllers/bumper/generic/SConscript code/gazebo/trunk/server/sensors/contact/ code/gazebo/trunk/server/sensors/contact/ContactSensor.cc code/gazebo/trunk/server/sensors/contact/ContactSensor.hh code/gazebo/trunk/server/sensors/contact/SConscript code/gazebo/trunk/worlds/bumper.world Modified: code/gazebo/trunk/SConstruct =================================================================== --- code/gazebo/trunk/SConstruct 2008-09-09 22:02:13 UTC (rev 7015) +++ code/gazebo/trunk/SConstruct 2008-09-09 22:04:34 UTC (rev 7016) @@ -45,6 +45,7 @@ '#server/sensors', '#server/sensors/camera', '#server/sensors/ray', + '#server/sensors/contact', '#server/physics', '#server/physics/ode', '#server/controllers', Modified: code/gazebo/trunk/libgazebo/Iface.cc =================================================================== --- code/gazebo/trunk/libgazebo/Iface.cc 2008-09-09 22:02:13 UTC (rev 7015) +++ code/gazebo/trunk/libgazebo/Iface.cc 2008-09-09 22:04:34 UTC (rev 7016) @@ -58,6 +58,7 @@ GZ_REGISTER_IFACE("ptz", PTZIface); GZ_REGISTER_IFACE("stereocamera", StereoCameraIface); GZ_REGISTER_IFACE("opaque", OpaqueIface); +GZ_REGISTER_IFACE("bumper", BumperIface); ////////////////////////////////////////////////////////////////////////////// // Create an interface Modified: code/gazebo/trunk/libgazebo/gazebo.h =================================================================== --- code/gazebo/trunk/libgazebo/gazebo.h 2008-09-09 22:02:13 UTC (rev 7015) +++ code/gazebo/trunk/libgazebo/gazebo.h 2008-09-09 22:04:34 UTC (rev 7016) @@ -1411,9 +1411,66 @@ /** \} */ /// \} + /***************************************************************************/ /// \addtogroup libgazebo_iface /// \{ +/** \defgroup bumper_iface Bumper + + \brief Bumper interface + +The bumper interface allows a client to read data from a bumper/contact sensor +\{ +*/ + +#define GAZEBO_MAX_BUMPER_COUNT 128 + +/// \brief Bumper data +class BumperData +{ + public: GazeboData head; + + /// State of the bumpers + public: uint8_t bumpers[GAZEBO_MAX_BUMPER_COUNT]; + + /// Bumper count + public: unsigned int bumper_count; +}; + +/// \brief Bumper interface +class BumperIface : public Iface +{ + /// \brief Constructor + public: BumperIface():Iface("bumper", sizeof(BumperIface)+sizeof(BumperData)) {} + + /// \brief Destructor + public: virtual ~BumperIface() {this->data = NULL;} + + /// \brief Create the server + public: virtual void Create(Server *server, std::string id) + { + Iface::Create(server,id); + this->data = (BumperData*)this->mMap; + } + + /// \brief Open the iface + public: virtual void Open(Client *client, std::string id) + { + Iface::Open(client,id); + this->data = (BumperData*)this->mMap; + } + + /// Pointer to the bumper data + public: BumperData *data; +}; + +/** \} */ +/// \} + + +/***************************************************************************/ +/// \addtogroup libgazebo_iface +/// \{ /** \defgroup opaque_iface opaque \brief Interface for arbitrary data transfer Added: code/gazebo/trunk/player/BumperInterface.cc =================================================================== --- code/gazebo/trunk/player/BumperInterface.cc (rev 0) +++ code/gazebo/trunk/player/BumperInterface.cc 2008-09-09 22:04:34 UTC (rev 7016) @@ -0,0 +1,140 @@ +/* + * 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: Bumper Interface for Player + * Author: Nate Koenig + * Date: 09 Sept. 2008 + * CVS: $Id:$ + */ + +/** +@addtogroup player +@par Bumper Interface +- PLAYER_BUMPER_DATA_STATE +*/ + +/* TODO +- PLAYER_BUMPER_REQ_GET_GEOM +*/ + +#include <iostream> +#include <math.h> + +#include "gazebo.h" +#include "GazeboDriver.hh" +#include "BumperInterface.hh" + +using namespace gazebo; + +/////////////////////////////////////////////////////////////////////////////// +// Constructor +BumperInterface::BumperInterface(player_devaddr_t addr, + GazeboDriver *driver, ConfigFile *cf, + int section) + : GazeboInterface(addr, driver, cf, section) +{ + // Get the ID of the interface + this->gz_id = (char*) calloc(1024, sizeof(char)); + strcat(this->gz_id, GazeboClient::prefixId); + strcat(this->gz_id, cf->ReadString(section, "gz_id", "")); + + // Allocate a Position Interface + this->iface = new BumperIface(); + + this->datatime = -1; + + this->data.bumpers_count = 1; + this->data.bumpers = new uint8_t[1]; +} + +/////////////////////////////////////////////////////////////////////////////// +// Destructor +BumperInterface::~BumperInterface() +{ + // Release this interface + delete this->iface; + + delete [] this->data.bumpers; +} + +/////////////////////////////////////////////////////////////////////////////// +// Handle all messages. This is called from GazeboDriver +int BumperInterface::ProcessMessage(QueuePointer &respQueue, + player_msghdr_t *hdr, void *data) +{ + return -1; +} + +/////////////////////////////////////////////////////////////////////////////// +// Update this interface, publish new info. This is +// called from GazeboDriver::Update +void BumperInterface::Update() +{ + this->iface->Lock(1); + + // Only Update when new data is present + if (this->iface->data->head.time > this->datatime) + { + this->datatime = this->iface->data->head.time; + + if (this->data.bumpers_count < this->iface->data->bumper_count) + { + delete [] this->data.bumpers; + this->data.bumpers = new uint8_t[this->iface->data->bumper_count]; + } + + // Copy bumper data + this->data.bumpers_count = this->iface->data->bumper_count; + memcpy( this->data.bumpers, this->iface->data->bumpers, + sizeof(uint8_t) * this->data.bumpers_count ); + + this->driver->Publish( this->device_addr, + PLAYER_MSGTYPE_DATA, + PLAYER_BUMPER_DATA_STATE, + (void*)&data, sizeof(data), &this->datatime ); + } + + this->iface->Unlock(); +} + + +/////////////////////////////////////////////////////////////////////////////// +// Open a SHM interface when a subscription is received. This is called from +// GazeboDriver::Subscribe +void BumperInterface::Subscribe() +{ + try + { + this->iface->Open( GazeboClient::client, this->gz_id); + } + catch (std::string e) + { + std::cerr << "Error subscribing to Gazebo Bumper Interface\n" + << e << "\n"; + exit(0); + } +} + +/////////////////////////////////////////////////////////////////////////////// +// Close a SHM interface. This is called from GazeboDriver::Unsubscribe +void BumperInterface::Unsubscribe() +{ + this->iface->Close(); +} Added: code/gazebo/trunk/player/BumperInterface.hh =================================================================== --- code/gazebo/trunk/player/BumperInterface.hh (rev 0) +++ code/gazebo/trunk/player/BumperInterface.hh 2008-09-09 22:04:34 UTC (rev 7016) @@ -0,0 +1,83 @@ +/* + * 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: Bumper Interface for Player + * Author: Nate Koenig + * Date: 09 Sept. 2008 + * SVN: $Id:$ + */ + +#ifndef BUMPERINTERFACE_HH +#define BUMPERINTERFACE_HH + +#include "GazeboInterface.hh" + +namespace gazebo +{ + /// \addtogroup player_iface + /// \{ + /// \defgroup gripper_player Bumper Interface + /// \brief Bumper Player interface + /// \{ + + class BumperIface; + + /// \brief Bumper interface + class BumperInterface : public GazeboInterface + { + /// \brief Constructor + public: BumperInterface(player_devaddr_t addr, GazeboDriver *driver, + ConfigFile *cf, int section); + + /// \brief Destructor + public: virtual ~BumperInterface(); + + /// \brief Handle all messages. This is called from GazeboDriver + public: virtual int ProcessMessage(QueuePointer &respQueue, + player_msghdr_t *hdr, void *data); + + /// \brief Update this interface, publish new info. + public: virtual void Update(); + + /// \brief Open a SHM interface when a subscription is received. + /// This is called fromGazeboDriver::Subscribe + public: virtual void Subscribe(); + + /// \brief Close a SHM interface. This is called from + /// GazeboDriver::Unsubscribe + public: virtual void Unsubscribe(); + + private: BumperIface *iface; + + /// \brief Gazebo id. This needs to match and ID in a Gazebo WorldFile + private: char *gz_id; + + /// \brief Timestamp on last data update + private: double datatime; + + private: player_bumper_data_t data; + }; + + /// \} + /// \} + +} + +#endif Modified: code/gazebo/trunk/player/GazeboDriver.cc =================================================================== --- code/gazebo/trunk/player/GazeboDriver.cc 2008-09-09 22:02:13 UTC (rev 7015) +++ code/gazebo/trunk/player/GazeboDriver.cc 2008-09-09 22:04:34 UTC (rev 7016) @@ -39,6 +39,7 @@ #include "ActarrayInterface.hh" #include "OpaqueInterface.hh" #include "PTZInterface.hh" +#include "BumperInterface.hh" #include "GripperInterface.hh" /* @@ -311,6 +312,12 @@ ifsrc = new GripperInterface( playerAddr, this, cf, section ); break; + case PLAYER_BUMPER_CODE: + if (!player_quiet_startup) printf(" a bumper interface.\n"); + ifsrc = new BumperInterface( playerAddr, this, cf, section ); + break; + + /* case PLAYER_POWER_CODE: if (!player_quiet_startup) printf(" a power interface.\n"); ifsrc = new PowerInterface( playerAddr, this, cf, section ); Modified: code/gazebo/trunk/player/SConscript =================================================================== --- code/gazebo/trunk/player/SConscript 2008-09-09 22:02:13 UTC (rev 7015) +++ code/gazebo/trunk/player/SConscript 2008-09-09 22:04:34 UTC (rev 7016) @@ -16,7 +16,8 @@ 'PTZInterface.cc', 'OpaqueInterface.cc', 'ActarrayInterface.cc', - 'GripperInterface.cc' + 'GripperInterface.cc', + 'BumperInterface.cc' ] # Position3dInterface.cc PowerInterface.cc SonarInterface.ccGpsInterface.cc ') Modified: code/gazebo/trunk/server/controllers/SConscript =================================================================== --- code/gazebo/trunk/server/controllers/SConscript 2008-09-09 22:02:13 UTC (rev 7015) +++ code/gazebo/trunk/server/controllers/SConscript 2008-09-09 22:04:34 UTC (rev 7016) @@ -1,7 +1,7 @@ #Import variable Import('env sharedObjs headers') -dirs = Split('position2d laser camera factory gripper actarray ptz opaque') +dirs = Split('position2d laser camera factory gripper actarray ptz opaque bumper') if env['with_audio'] == 'yes': dirs+=Split('audio') Added: code/gazebo/trunk/server/controllers/bumper/SConscript =================================================================== --- code/gazebo/trunk/server/controllers/bumper/SConscript (rev 0) +++ code/gazebo/trunk/server/controllers/bumper/SConscript 2008-09-09 22:04:34 UTC (rev 7016) @@ -0,0 +1,7 @@ +#Import variable +Import('env sharedObjs') + +dirs = Split('generic') + +for subdir in dirs : + SConscript('%s/SConscript' % subdir) Added: code/gazebo/trunk/server/controllers/bumper/generic/Generic_Bumper.cc =================================================================== --- code/gazebo/trunk/server/controllers/bumper/generic/Generic_Bumper.cc (rev 0) +++ code/gazebo/trunk/server/controllers/bumper/generic/Generic_Bumper.cc 2008-09-09 22:04:34 UTC (rev 7016) @@ -0,0 +1,99 @@ +/* + * 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: Bumper controller + * Author: Nate Koenig + * Date: 09 Setp. 2008 + */ + +#include "Global.hh" +#include "XMLConfig.hh" +#include "ContactSensor.hh" +#include "World.hh" +#include "gazebo.h" +#include "GazeboError.hh" +#include "ControllerFactory.hh" +#include "Simulator.hh" +#include "Generic_Bumper.hh" + +using namespace gazebo; + +GZ_REGISTER_STATIC_CONTROLLER("bumper", Generic_Bumper); + +//////////////////////////////////////////////////////////////////////////////// +// Constructor +Generic_Bumper::Generic_Bumper(Entity *parent ) + : Controller(parent) +{ + this->myParent = dynamic_cast<ContactSensor*>(this->parent); + + if (!this->myParent) + gzthrow("Bumper controller requires a Contact Sensor as its parent"); +} + +//////////////////////////////////////////////////////////////////////////////// +// Destructor +Generic_Bumper::~Generic_Bumper() +{ +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void Generic_Bumper::LoadChild(XMLConfigNode *node) +{ + this->myIface = dynamic_cast<BumperIface*>(this->ifaces[0]); + + if (!this->myIface) + { + gzthrow("Generic_Bumper controller requires an BumperIface"); + } +} + +//////////////////////////////////////////////////////////////////////////////// +// Initialize the controller +void Generic_Bumper::InitChild() +{ +} + +//////////////////////////////////////////////////////////////////////////////// +// Update the controller +void Generic_Bumper::UpdateChild() +{ + this->myIface->Lock(1); + + this->myIface->data->bumper_count = this->myParent->GetContactCount(); + + for (unsigned int i=0; i < this->myParent->GetContactCount(); i++) + { + this->myIface->data->head.time = this->myParent->GetContactTime(i); + this->myIface->data->bumpers[i] = this->myParent->GetContactState(i); + } + + this->myParent->ResetContactStates(); + + this->myIface->Unlock(); +} + +//////////////////////////////////////////////////////////////////////////////// +// Finalize the controller +void Generic_Bumper::FiniChild() +{ +} Added: code/gazebo/trunk/server/controllers/bumper/generic/Generic_Bumper.hh =================================================================== --- code/gazebo/trunk/server/controllers/bumper/generic/Generic_Bumper.hh (rev 0) +++ code/gazebo/trunk/server/controllers/bumper/generic/Generic_Bumper.hh 2008-09-09 22:04:34 UTC (rev 7016) @@ -0,0 +1,86 @@ +/* + * 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: Bumper Controller + * Author: Nate Koenig + * Date: 09 Sept 2008 + */ +#ifndef GENERICBUMPER_CONTROLLER_HH +#define GENERICBUMPER_CONTROLLER_HH + +#include <sys/time.h> + +#include "Controller.hh" +#include "Entity.hh" + +namespace gazebo +{ + class ContactSensor; + + /// \addtogroup gazebo_controller + /// \{ + /** \defgroup bumper bumper + + \brief A controller that returns bump contacts + + \{ + */ + + /// \brief A Bumper controller + class Generic_Bumper : public Controller + { + /// Constructor + public: Generic_Bumper(Entity *parent ); + + /// Destructor + public: virtual ~Generic_Bumper(); + + /// Load the controller + /// \param node XML config node + /// \return 0 on success + protected: virtual void LoadChild(XMLConfigNode *node); + + /// Init the controller + /// \return 0 on success + protected: virtual void InitChild(); + + /// Update the controller + /// \return 0 on success + protected: virtual void UpdateChild(); + + /// Finalize the controller + /// \return 0 on success + protected: virtual void FiniChild(); + + /// The parent Model + private: ContactSensor *myParent; + + /// The Iface. + private: BumperIface *myIface; + }; + + /** \} */ + /// \} + +} + +#endif + Added: code/gazebo/trunk/server/controllers/bumper/generic/SConscript =================================================================== --- code/gazebo/trunk/server/controllers/bumper/generic/SConscript (rev 0) +++ code/gazebo/trunk/server/controllers/bumper/generic/SConscript 2008-09-09 22:04:34 UTC (rev 7016) @@ -0,0 +1,6 @@ +#Import variable +Import('env sharedObjs') + +sources = Split('Generic_Bumper.cc') + +sharedObjs.append(env.SharedObject(sources)) Modified: code/gazebo/trunk/server/physics/ContactParams.hh =================================================================== --- code/gazebo/trunk/server/physics/ContactParams.hh 2008-09-09 22:02:13 UTC (rev 7015) +++ code/gazebo/trunk/server/physics/ContactParams.hh 2008-09-09 22:04:34 UTC (rev 7016) @@ -27,47 +27,60 @@ #ifndef CONTACTPARAMS_HH #define CONTACTPARAMS_HH +#include <boost/signal.hpp> +#include <boost/bind.hpp> + namespace gazebo { + class Geom; + class XMLConfigNode; -/// \brief Contact params -class ContactParams -{ - /// Constructor - public: ContactParams(); - - /// \brief Load the contact params - public: void Load(XMLConfigNode *node); - - /// Spring constant - public: double kp; - - /// Damping constant - public: double kd; - - /// 0..1, 0=no bounciness - public: double bounce; + /// \brief Contact params + class ContactParams + { + /// Constructor + public: ContactParams(); - /// first coefficient of friction - public: double mu1; + /// \brief Load the contact params + public: void Load(XMLConfigNode *node); + + public: template< typename C> + void Callback( void (C::*func)(Geom *, Geom*), C *c ) + { + contactSignal.connect( boost::bind( func, c, _1, _2 ) ); + } + + /// Spring constant + public: double kp; + + /// Damping constant + public: double kd; + + /// 0..1, 0=no bounciness + public: double bounce; + + /// first coefficient of friction + public: double mu1; + + /// second coefficient of friction + public: double mu2; + + /// Force-dependent-slip direction 1 + public: double slip1; + + /// Force-dependent-slip direction 2 + public: double slip2; + + /// \brief bounce vel + public: double bounceVel; + + /// \brief soft constraint force mixing + public: double softCfm; + + public: boost::signal< void (Geom*, Geom*)> contactSignal; + }; +} - /// second coefficient of friction - public: double mu2; - - /// Force-dependent-slip direction 1 - public: double slip1; - - /// Force-dependent-slip direction 2 - public: double slip2; - - /// \brief bounce vel - public: double bounceVel; - - /// \brief soft constraint force mixing - public: double softCfm; -}; - -} #endif Modified: code/gazebo/trunk/server/physics/Geom.cc =================================================================== --- code/gazebo/trunk/server/physics/Geom.cc 2008-09-09 22:02:13 UTC (rev 7015) +++ code/gazebo/trunk/server/physics/Geom.cc 2008-09-09 22:04:34 UTC (rev 7016) @@ -517,4 +517,3 @@ this->body->UpdateCoM(); } - Modified: code/gazebo/trunk/server/physics/ode/ODEPhysics.cc =================================================================== --- code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2008-09-09 22:02:13 UTC (rev 7015) +++ code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2008-09-09 22:04:34 UTC (rev 7016) @@ -287,10 +287,13 @@ contact.surface.slip2 = MIN(geom1->contact->slip2, geom2->contact->slip2); + dJointID c = dJointCreateContact (self->worldId, + self->contactGroup, &contact); + // Call the geom's contact callbacks + geom1->contact->contactSignal( geom1, geom2 ); + geom2->contact->contactSignal( geom2, geom1 ); - dJointID c = dJointCreateContact (self->worldId, - self->contactGroup, &contact); dJointAttach (c,b1,b2); } } Modified: code/gazebo/trunk/server/sensors/SConscript =================================================================== --- code/gazebo/trunk/server/sensors/SConscript 2008-09-09 22:02:13 UTC (rev 7015) +++ code/gazebo/trunk/server/sensors/SConscript 2008-09-09 22:04:34 UTC (rev 7016) @@ -1,7 +1,7 @@ #Import variable Import('env sharedObjs headers') -dirs = Split('camera ray') +dirs = Split('camera ray contact') for subdir in dirs : SConscript('%s/SConscript' % subdir) @@ -14,5 +14,4 @@ 'server/sensors/Sensor.hh' ] ) -#staticObjs.append( env.StaticObject(sources) ) sharedObjs.append( env.SharedObject(sources) ) Added: code/gazebo/trunk/server/sensors/contact/ContactSensor.cc =================================================================== --- code/gazebo/trunk/server/sensors/contact/ContactSensor.cc (rev 0) +++ code/gazebo/trunk/server/sensors/contact/ContactSensor.cc 2008-09-09 22:04:34 UTC (rev 7016) @@ -0,0 +1,212 @@ +/* + * 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: Contact sensor + * Author: Nate Koenig + * Date: 09 Sept. 2008 + * SVN: $Id:$ +*/ + +#include <assert.h> +#include <float.h> +#include <sstream> + +#include "Global.hh" +//#include "World.hh" +//#include "Controller.hh" + +#include "GazeboError.hh" +#include "XMLConfig.hh" +#include "Simulator.hh" +#include "SensorFactory.hh" +#include "Geom.hh" +#include "ContactParams.hh" +#include "ContactSensor.hh" + +#include "Vector3.hh" + +using namespace gazebo; + +GZ_REGISTER_STATIC_SENSOR("contact", ContactSensor); + +////////////////////////////////////////////////////////////////////////////// +// Constructor +ContactSensor::ContactSensor(Body *body) + : Sensor(body) +{ + this->active = false; + + this->typeName = "contact"; + + this->contactCount = 0; + this->contactStates = NULL; +} + + +////////////////////////////////////////////////////////////////////////////// +// Destructor +ContactSensor::~ContactSensor() +{ + std::vector< ParamT<std::string> *>::iterator iter; + + if (this->contactStates) + delete [] this->contactStates; + + if (this->contactTimes) + delete [] this->contactTimes; + + for (iter = this->geomNamesP.begin(); iter != this->geomNamesP.end(); iter++) + { + delete *iter; + } + this->geomNamesP.clear(); +} + +////////////////////////////////////////////////////////////////////////////// +/// Get the contact time +double ContactSensor::GetContactTime(unsigned int index) const +{ + if (index < this->contactCount) + return this->contactTimes[ index ]; + + return 0; +} + +////////////////////////////////////////////////////////////////////////////// +/// Return the number of contacts +unsigned int ContactSensor::GetContactCount() const +{ + return this->contactCount; +} + +////////////////////////////////////////////////////////////////////////////// +/// Return the contact states +uint8_t ContactSensor::GetContactState(unsigned int index) const +{ + if (index < this->contactCount) + return this->contactStates[index]; + + return 0; +} + +////////////////////////////////////////////////////////////////////////////// +/// Reset the contact states +void ContactSensor::ResetContactStates() +{ + memset(this->contactStates, 0, sizeof(uint8_t) * this->contactCount); +} + +////////////////////////////////////////////////////////////////////////////// +/// Load the ray using parameter from an XMLConfig node +void ContactSensor::LoadChild(XMLConfigNode *node) +{ + XMLConfigNode *geomNode = NULL; + + if (this->body == NULL) + { + gzthrow("Null body in the contact sensor"); + } + + Param::Begin(&this->parameters); + geomNode = node->GetChild("geom"); + while (geomNode) + { + ParamT<std::string> *geomName = new ParamT<std::string>("geom","",1); + geomName->SetValue( geomNode->GetValue() ); + this->geomNamesP.push_back(geomName); + geomNode = geomNode->GetNext("geom"); + } + Param::End(); + + this->contactCount = this->geomNamesP.size(); + this->contactTimes = new double[ this->contactCount ]; + this->contactStates = new uint8_t[ this->contactCount ]; + + memset(this->contactStates,0, sizeof(uint8_t) * this->contactCount); + memset(this->contactStates,0, sizeof(double) * this->contactCount); +} + +////////////////////////////////////////////////////////////////////////////// +/// Save the sensor info in XML format +void ContactSensor::SaveChild(std::string &prefix, std::ostream &stream) +{ + std::vector< ParamT<std::string> *>::iterator iter; + + for (iter = this->geomNamesP.begin(); iter != this->geomNamesP.end(); iter++) + { + stream << prefix << " " << *(iter) << "\n"; + } +} + +////////////////////////////////////////////////////////////////////////////// +// Init the contact +void ContactSensor::InitChild() +{ + std::vector< ParamT<std::string> *>::iterator iter; + + for (iter = this->geomNamesP.begin(); iter != this->geomNamesP.end(); iter++) + { + // Get the geom from the body + Geom *geom = this->body->GetGeom( **(*iter) ); + + // Set the callback + if (geom) + { + geom->contact->Callback( &ContactSensor::ContactCallback, this ); + } + else + { + gzthrow("Unable to find geom[" + **(*iter) + "]"); + } + } + +} + +////////////////////////////////////////////////////////////////////////////// +// Init the ray +void ContactSensor::FiniChild() +{ +} + +////////////////////////////////////////////////////////////////////////////// +// Update the sensor information +void ContactSensor::UpdateChild() +{ +} + +////////////////////////////////////////////////////////////////////////////// +/// Contact callback +void ContactSensor::ContactCallback(Geom *g1, Geom *g2) +{ + std::vector< ParamT<std::string> *>::iterator iter; + int i = 0; + + + for (iter = this->geomNamesP.begin(); iter != this->geomNamesP.end(); + iter++, i++) + { + if ( **(*iter) == g1->GetName() || **(*iter) == g2->GetName() ) + { + this->contactStates[i] = 1; + this->contactTimes[i] = Simulator::Instance()->GetRealTime(); + } + } + +} Added: code/gazebo/trunk/server/sensors/contact/ContactSensor.hh =================================================================== --- code/gazebo/trunk/server/sensors/contact/ContactSensor.hh (rev 0) +++ code/gazebo/trunk/server/sensors/contact/ContactSensor.hh 2008-09-09 22:04:34 UTC (rev 7016) @@ -0,0 +1,104 @@ +/* + * 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: Contact sensor + * Author: Nate Koenig + * Date: 09 Sept. 2008 + * SVN: $Id:$ +*/ + +#ifndef CONTACTSENSOR_HH +#define CONTACTSENSOR_HH + +#include <vector> + +#include "Angle.hh" +#include "Sensor.hh" +#include "Body.hh" + +namespace gazebo +{ + + class XMLConfigNode; + class ContactParams; + + /// \addtogroup gazebo_sensor + /// \brief Contact sensor. + /// \{ + /// \defgroup gazebo_ray Contact + /// \brief Contact sensor. + // \{ + + /// \brief Contact sensor. + /// + /// This sensor detects and reports contacts between objects + class ContactSensor: public Sensor + { + /// \brief Constructor + /// \param body The underlying collision test uses an ODE geom, so + /// ray sensors must be attached to a body. + public: ContactSensor(Body *body); + + /// \brief Destructor + public: virtual ~ContactSensor(); + + /// \brief Return the number of contacts + public: unsigned int GetContactCount() const; + + /// \brief Get a contact time + public: double GetContactTime(unsigned int index) const; + + /// \brief Return a contact state + public: uint8_t GetContactState(unsigned int index) const; + + /// \brief Reset the contact states + public: void ResetContactStates(); + + /// Load the contact sensor using parameter from an XMLConfig node + /// \param node The XMLConfig node + protected: virtual void LoadChild(XMLConfigNode *node); + + /// \brief Save the sensor info in XML format + protected: virtual void SaveChild(std::string &prefix,std::ostream &stream); + + /// Initialize the sensor + protected: virtual void InitChild(); + + /// Update sensed values + protected: virtual void UpdateChild(); + + /// Finalize the sensor + protected: virtual void FiniChild(); + + /// \brief Contact callback + private: void ContactCallback(Geom *g1, Geom *g2); + + /// Geom name parameter + private: std::vector< ParamT<std::string> *> geomNamesP; + + private: uint8_t *contactStates; + private: double *contactTimes; + private: unsigned int contactCount; + }; + /// \} + /// \} +} + +#endif Added: code/gazebo/trunk/server/sensors/contact/SConscript =================================================================== --- code/gazebo/trunk/server/sensors/contact/SConscript (rev 0) +++ code/gazebo/trunk/server/sensors/contact/SConscript 2008-09-09 22:04:34 UTC (rev 7016) @@ -0,0 +1,6 @@ +#Import variable +Import('env sharedObjs') + +sources = Split('ContactSensor.cc') + +sharedObjs.append(env.SharedObject(sources)) Added: code/gazebo/trunk/worlds/bumper.world =================================================================== --- code/gazebo/trunk/worlds/bumper.world (rev 0) +++ code/gazebo/trunk/worlds/bumper.world 2008-09-09 22:04:34 UTC (rev 7016) @@ -0,0 +1,149 @@ +<?xml version="1.0"?> + +<gazebo:world + xmlns:xi="http://www.w3.org/2001/XInclude" + xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz" + xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model" + xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" + xmlns:window="http://playerstage.sourceforge.net/gazebo/xmlschema/#window" + xmlns:param="http://playerstage.sourceforge.net/gazebo/xmlschema/#param" + xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body" + xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom" + xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint" + xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" + xmlns:ui="http://playerstage.sourceforge.net/gazebo/xmlschema/#ui" + xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering" + xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" + xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics" > + + <verbosity>5</verbosity> + + <physics:ode> + <stepTime>0.03</stepTime> + <gravity>0 0 -9.8</gravity> + <cfm>10e-5</cfm> + <erp>0.8</erp> + <maxUpdateRate>0</maxUpdateRate> + </physics:ode> + + <rendering:gui> + <size>800 600</size> + <pos>100 100</pos> + <frames> + <row height="100%"> + <camera width="100%"> + <xyz>-1 0 4</xyz> + <rpy>0 34 0</rpy> + </camera> + </row> + </frames> + </rendering:gui> + + <rendering:ogre> + <ambient>0.5 0.5 0.5 1.0</ambient> + <sky> + <material>Gazebo/CloudySky</material> + </sky> + + <fog> + <color>1.0 1.0 1.0</color> + <linearStart>10</linearStart> + <linearEnd>100</linearEnd> + </fog> + </rendering:ogre> + + <model:physical name="plane1_model"> + <xyz>0 0 0</xyz> + <rpy>0 0 0</rpy> + <static>true</static> + + <body:plane name="plane1_body"> + <geom:plane name="plane1_geom"> + <normal>0 0 1</normal> + <size>2000 2000</size> + <segments>10 10</segments> + <uvTile>100 100</uvTile> + <material>Gazebo/Grey</material> + </geom:plane> + </body:plane> + </model:physical> + + <model:physical name="sphere1_model"> + <xyz>2.15 -1.68 1.7</xyz> + <rpy>0.0 0.0 0.0</rpy> + <static>false</static> + + <body:sphere name="sphere1_body"> + <geom:sphere name="sphere1_geom"> + <size>0.1</size> + + <visual> + <scale>0.1 0.1 0.1</scale> + <mesh>unit_sphere</mesh> + <material>Gazebo/Rocky</material> + </visual> + </geom:sphere> + + <sensor:contact name="contact_sensor"> + <geom>sphere1_geom</geom> + + <controller:bumper name="bumper_controller"> + <interface:bumper name="bumper_iface" /> + </controller:bumper> + </sensor:contact> + + </body:sphere> + </model:physical> + + <!-- + Include the complete model described in the .model file + This assumes the root node is a <model:...> + --> + <!-- <include embedded="false"> + <xi:include href="pioneer2dx.model" /> + </include> + --> + + <model:physical name="pioneer2dx_model1"> + <xyz>0 0 0.145</xyz> + <rpy>0.0 0.0 0.0</rpy> + + <controller:differential_position2d name="controller1"> + <leftJoint>left_wheel_hinge</leftJoint> + <rightJoint>right_wheel_hinge</rightJoint> + <wheelSeparation>0.39</wheelSeparation> + <wheelDiameter>0.15</wheelDiameter> + <torque>5</torque> + <interface:position name="position_iface_0"/> + </controller:differential_position2d> + + <model:physical name="laser"> + <xyz>0.15 0 0.18</xyz> + + <attach> + <parentBody>chassis_body</parentBody> + <myBody>laser_body</myBody> + </attach> + + <include embedded="true"> + <xi:include href="models/sicklms200.model" /> + </include> + </model:physical> + + <include embedded="true"> + <xi:include href="models/pioneer2dx.model" /> + </include> + </model:physical> + + <!-- White Directional light --> + <model:renderable name="directional_white"> + <light> + <type>directional</type> + <direction>0 -0.6 -0.4</direction> + <diffuseColor>1.0 1.0 1.0</diffuseColor> + <specularColor>0.2 0.2 0.2</specularColor> + <attenuation>1000 1.0 0.0 0</attenuation> + </light> + </model:renderable> + +</gazebo:world> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-09-12 07:37:12
|
Revision: 7021 http://playerstage.svn.sourceforge.net/playerstage/?rev=7021&view=rev Author: natepak Date: 2008-09-12 14:37:21 +0000 (Fri, 12 Sep 2008) Log Message: ----------- Fixed a time issue with the bump controller Modified Paths: -------------- code/gazebo/trunk/player/BumperInterface.cc code/gazebo/trunk/server/controllers/bumper/generic/Generic_Bumper.cc Modified: code/gazebo/trunk/player/BumperInterface.cc =================================================================== --- code/gazebo/trunk/player/BumperInterface.cc 2008-09-12 01:26:55 UTC (rev 7020) +++ code/gazebo/trunk/player/BumperInterface.cc 2008-09-12 14:37:21 UTC (rev 7021) @@ -102,6 +102,7 @@ // Copy bumper data this->data.bumpers_count = this->iface->data->bumper_count; + memcpy( this->data.bumpers, this->iface->data->bumpers, sizeof(uint8_t) * this->data.bumpers_count ); Modified: code/gazebo/trunk/server/controllers/bumper/generic/Generic_Bumper.cc =================================================================== --- code/gazebo/trunk/server/controllers/bumper/generic/Generic_Bumper.cc 2008-09-12 01:26:55 UTC (rev 7020) +++ code/gazebo/trunk/server/controllers/bumper/generic/Generic_Bumper.cc 2008-09-12 14:37:21 UTC (rev 7021) @@ -81,9 +81,10 @@ this->myIface->data->bumper_count = this->myParent->GetContactCount(); + this->myIface->data->head.time = Simulator::Instance()->GetRealTime(); + for (unsigned int i=0; i < this->myParent->GetContactCount(); i++) { - this->myIface->data->head.time = this->myParent->GetContactTime(i); this->myIface->data->bumpers[i] = this->myParent->GetContactState(i); } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-09-21 22:55:52
|
Revision: 7033 http://playerstage.svn.sourceforge.net/playerstage/?rev=7033&view=rev Author: natepak Date: 2008-09-21 22:55:45 +0000 (Sun, 21 Sep 2008) Log Message: ----------- added patch 2118734 Modified Paths: -------------- code/gazebo/trunk/SConstruct code/gazebo/trunk/libgazebo/Iface.cc code/gazebo/trunk/libgazebo/gazebo.h code/gazebo/trunk/server/controllers/SConscript code/gazebo/trunk/server/sensors/SConscript Modified: code/gazebo/trunk/SConstruct =================================================================== --- code/gazebo/trunk/SConstruct 2008-09-21 22:47:47 UTC (rev 7032) +++ code/gazebo/trunk/SConstruct 2008-09-21 22:55:45 UTC (rev 7033) @@ -46,6 +46,7 @@ '#server/sensors/camera', '#server/sensors/ray', '#server/sensors/contact', + '#server/sensors/imu', '#server/physics', '#server/physics/ode', '#server/controllers', Modified: code/gazebo/trunk/libgazebo/Iface.cc =================================================================== --- code/gazebo/trunk/libgazebo/Iface.cc 2008-09-21 22:47:47 UTC (rev 7032) +++ code/gazebo/trunk/libgazebo/Iface.cc 2008-09-21 22:55:45 UTC (rev 7033) @@ -59,6 +59,7 @@ GZ_REGISTER_IFACE("stereocamera", StereoCameraIface); GZ_REGISTER_IFACE("opaque", OpaqueIface); GZ_REGISTER_IFACE("bumper", BumperIface); +GZ_REGISTER_IFACE("imu", ImuIface); ////////////////////////////////////////////////////////////////////////////// // Create an interface Modified: code/gazebo/trunk/libgazebo/gazebo.h =================================================================== --- code/gazebo/trunk/libgazebo/gazebo.h 2008-09-21 22:47:47 UTC (rev 7032) +++ code/gazebo/trunk/libgazebo/gazebo.h 2008-09-21 22:55:45 UTC (rev 7033) @@ -765,8 +765,28 @@ /** @} */ /// @} +class ImuData +{ + public: GazeboData head; + public: Pose velocity; +}; - +class ImuIface : public Iface +{ + public: ImuIface():Iface("imu", sizeof(ImuIface)+sizeof(ImuData)) {} + public: virtual ~ImuIface() {this->data = NULL;} + public: virtual void Create(Server *server, std::string id) + { + Iface::Create(server,id); + this->data = (ImuData*)this->mMap; + } + public: virtual void Open(Client *client, std::string id) + { + Iface::Open(client,id); + this->data = (ImuData*)this->mMap; + } + public: ImuData *data; +}; /***************************************************************************/ /// @addtogroup libgazebo_iface /// @{ Modified: code/gazebo/trunk/server/controllers/SConscript =================================================================== --- code/gazebo/trunk/server/controllers/SConscript 2008-09-21 22:47:47 UTC (rev 7032) +++ code/gazebo/trunk/server/controllers/SConscript 2008-09-21 22:55:45 UTC (rev 7033) @@ -1,7 +1,7 @@ #Import variable Import('env sharedObjs headers') -dirs = Split('position2d laser camera factory gripper actarray ptz opaque bumper') +dirs = Split('position2d laser camera factory gripper actarray ptz opaque bumper imu') if env['with_audio'] == 'yes': dirs+=Split('audio') Modified: code/gazebo/trunk/server/sensors/SConscript =================================================================== --- code/gazebo/trunk/server/sensors/SConscript 2008-09-21 22:47:47 UTC (rev 7032) +++ code/gazebo/trunk/server/sensors/SConscript 2008-09-21 22:55:45 UTC (rev 7033) @@ -1,7 +1,7 @@ #Import variable Import('env sharedObjs headers') -dirs = Split('camera ray contact') +dirs = Split('camera ray contact imu') for subdir in dirs : SConscript('%s/SConscript' % subdir) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |