[Playerstage-commit] SF.net SVN: playerstage:[7443] code/branches/federation/gazebo

[<na...@us...>]

Revision: 7443
          http://playerstage.svn.sourceforge.net/playerstage/?rev=7443&view=rev
Author:   natepak
Date:     2009-03-10 09:37:04 +0000 (Tue, 10 Mar 2009)

Log Message:
-----------
Improved graphics interface

Modified Paths:
--------------
    code/branches/federation/gazebo/examples/libgazebo/position/SConstruct
    code/branches/federation/gazebo/examples/libgazebo/position/position.cc
    code/branches/federation/gazebo/libgazebo/SConscript
    code/branches/federation/gazebo/libgazebo/gazebo.h
    code/branches/federation/gazebo/server/Entity.cc
    code/branches/federation/gazebo/server/Entity.hh
    code/branches/federation/gazebo/server/Model.cc
    code/branches/federation/gazebo/server/Model.hh
    code/branches/federation/gazebo/server/SConscript
    code/branches/federation/gazebo/server/Simulator.cc
    code/branches/federation/gazebo/server/World.cc
    code/branches/federation/gazebo/server/World.hh
    code/branches/federation/gazebo/server/controllers/SConscript
    code/branches/federation/gazebo/server/controllers/factory/Factory.hh
    code/branches/federation/gazebo/server/controllers/position2d/differential/Differential_Position2d.cc
    code/branches/federation/gazebo/server/main.cc
    code/branches/federation/gazebo/server/physics/Body.cc
    code/branches/federation/gazebo/server/physics/Body.hh
    code/branches/federation/gazebo/server/physics/ContactParams.cc
    code/branches/federation/gazebo/server/physics/ContactParams.hh
    code/branches/federation/gazebo/server/physics/Geom.cc
    code/branches/federation/gazebo/server/physics/Geom.hh
    code/branches/federation/gazebo/server/physics/Joint.cc
    code/branches/federation/gazebo/server/physics/ode/ODEPhysics.cc
    code/branches/federation/gazebo/server/rendering/OgreAdaptor.cc
    code/branches/federation/gazebo/server/rendering/OgreCreator.cc
    code/branches/federation/gazebo/server/rendering/OgreCreator.hh
    code/branches/federation/gazebo/server/rendering/OgreVisual.cc
    code/branches/federation/gazebo/server/rendering/OgreVisual.hh
    code/branches/federation/gazebo/server/rendering/SConscript
    code/branches/federation/gazebo/server/sensors/Sensor.cc
    code/branches/federation/gazebo/server/sensors/Sensor.hh
    code/branches/federation/gazebo/server/sensors/ir/IRSensor.cc
    code/branches/federation/gazebo/server/sensors/ray/RaySensor.cc
    code/branches/federation/gazebo/worlds/pioneer2dx.world

Added Paths:
-----------
    code/branches/federation/gazebo/examples/libgazebo/graphics3d/
    code/branches/federation/gazebo/examples/libgazebo/graphics3d/.graphics3d.cc.swo
    code/branches/federation/gazebo/examples/libgazebo/graphics3d/.sconsign.dblite
    code/branches/federation/gazebo/examples/libgazebo/graphics3d/SConstruct
    code/branches/federation/gazebo/examples/libgazebo/graphics3d/graphics3d.cc

Added: code/branches/federation/gazebo/examples/libgazebo/graphics3d/.graphics3d.cc.swo
===================================================================
(Binary files differ)


Property changes on: code/branches/federation/gazebo/examples/libgazebo/graphics3d/.graphics3d.cc.swo
___________________________________________________________________
Added: svn:mime-type
   + application/octet-stream

Added: code/branches/federation/gazebo/examples/libgazebo/graphics3d/.sconsign.dblite
===================================================================
(Binary files differ)


Property changes on: code/branches/federation/gazebo/examples/libgazebo/graphics3d/.sconsign.dblite
___________________________________________________________________
Added: svn:mime-type
   + application/octet-stream

Added: code/branches/federation/gazebo/examples/libgazebo/graphics3d/SConstruct
===================================================================
--- code/branches/federation/gazebo/examples/libgazebo/graphics3d/SConstruct	                        (rev 0)
+++ code/branches/federation/gazebo/examples/libgazebo/graphics3d/SConstruct	2009-03-10 09:37:04 UTC (rev 7443)
@@ -0,0 +1,14 @@
+
+# 3rd party packages
+parseConfigs=['pkg-config --cflags --libs libgazebo']
+
+env = Environment (
+  CC = 'g++',
+  CCFLAGS = Split ('-pthread -pipe  -W -Wall -O2'),
+)
+
+# Parse all the pacakge configurations
+for cfg in parseConfigs:
+  env.ParseConfig(cfg)
+
+env.Program('graphics3d','graphics3d.cc')

Added: code/branches/federation/gazebo/examples/libgazebo/graphics3d/graphics3d.cc
===================================================================
--- code/branches/federation/gazebo/examples/libgazebo/graphics3d/graphics3d.cc	                        (rev 0)
+++ code/branches/federation/gazebo/examples/libgazebo/graphics3d/graphics3d.cc	2009-03-10 09:37:04 UTC (rev 7443)
@@ -0,0 +1,110 @@
+#include <string.h>
+#include <string>
+#include <iostream>
+#include <gazebo/gazebo.h>
+
+int main()
+{
+  gazebo::Client *client = new gazebo::Client();
+  gazebo::Graphics3dIface *g3dIface = new gazebo::Graphics3dIface();
+  gazebo::Graphics3dIface *pioneerG3DIface = new gazebo::Graphics3dIface();
+
+  int serverId = 0;
+
+  /// Connect to the libgazebo server
+  try
+  {
+    client->ConnectWait(serverId, GZ_CLIENT_ID_USER_FIRST);
+  }
+  catch (std::string e)
+  {
+    std::cout << "Gazebo error: Unable to connect\n" << e << "\n";
+    return -1;
+  }
+
+  /// Open the global graphics Interface
+  try
+  {
+    g3dIface->Open(client, "default");
+    pioneerG3DIface->Open(client,"pioneer2dx_model1");
+  }
+  catch (std::string e)
+  {
+    std::cerr << "Gazebo error: Unable to connect to the graphics3d interface\n" << e << "\n";
+    return -1;
+  }
+
+  float radius = 0.2;
+  float theta = 0;
+
+  float size = 0.4;
+  float h = 0;
+  int dir = 1;
+  std::string name = "square";
+  int count = 0;
+
+  gazebo::Vec3 spherePos, sphereSize;
+  sphereSize.x = 0.1;
+  sphereSize.y = 0.1;
+  sphereSize.z = 0.1;
+
+  gazebo::Vec3 vec[5];
+  gazebo::Color clr;
+  while (true)
+  {
+    vec[0].x = size;
+    vec[0].y = size;
+    vec[0].z = h;
+
+    vec[1].x = -size;
+    vec[1].y = size;
+    vec[1].z = h;
+
+    vec[2].x = -size;
+    vec[2].y = -size;
+    vec[2].z = h;
+
+    vec[3].x = size;
+    vec[3].y = -size;
+    vec[3].z = h;
+
+    vec[4].x = size;
+    vec[4].y = size;
+    vec[4].z = h;
+
+    clr.r = 1.0;
+    clr.g = 0.0;
+    clr.b = 0.0;
+    clr.a = 1.0;
+
+    spherePos.x = radius * cos(theta);
+    spherePos.y = radius * sin(theta);
+    spherePos.z = 0.8;
+
+    // Draw the bouncing square
+    pioneerG3DIface->DrawSimple(name.c_str(), 
+        gazebo::Graphics3dDrawData::LINE_STRIP, vec, 5, clr);
+
+    // Draw the sliding billboard
+    pioneerG3DIface->DrawBillboard("mybillboard", "heartpsg.png",
+      gazebo::Vec3(h,0,0.4), gazebo::Vec2(0.2, 0.2)  );
+
+
+    pioneerG3DIface->DrawShape("mysphere",
+        gazebo::Graphics3dDrawData::SPHERE, spherePos, sphereSize, clr);
+
+    h += dir*0.01;
+    if (h >= 0.2 || h < 0)
+      dir *= -1;
+
+    theta += 0.1;
+    usleep(10000);
+  }
+  
+
+  g3dIface->Close();
+
+  delete g3dIface;
+  return 0;
+}
+

Modified: code/branches/federation/gazebo/examples/libgazebo/position/SConstruct
===================================================================
--- code/branches/federation/gazebo/examples/libgazebo/position/SConstruct	2009-03-10 09:07:34 UTC (rev 7442)
+++ code/branches/federation/gazebo/examples/libgazebo/position/SConstruct	2009-03-10 09:37:04 UTC (rev 7443)
@@ -1,7 +1,6 @@
 
 # 3rd party packages
-parseConfigs=['pkg-config --cflags --libs libgazebo',
-              'pkg-config --cflags --libs libgazeboServer']
+parseConfigs=['pkg-config --cflags --libs libgazebo']
 
 
 env = Environment (

Modified: code/branches/federation/gazebo/examples/libgazebo/position/position.cc
===================================================================
--- code/branches/federation/gazebo/examples/libgazebo/position/position.cc	2009-03-10 09:07:34 UTC (rev 7442)
+++ code/branches/federation/gazebo/examples/libgazebo/position/position.cc	2009-03-10 09:37:04 UTC (rev 7443)
@@ -14,7 +14,7 @@
   {
     client->ConnectWait(serverId, GZ_CLIENT_ID_USER_FIRST);
   }
-  catch (gazebo::GazeboError e)
+  catch (std::string e)
   {
     std::cout << "Gazebo error: Unable to connect\n" << e << "\n";
     return -1;
@@ -25,7 +25,7 @@
   {
     simIface->Open(client, "default");
   }
-  catch (gazebo::GazeboError e)
+  catch (std::string e)
   {
     std::cout << "Gazebo error: Unable to connect to the sim interface\n" << e << "\n";
     return -1;

Modified: code/branches/federation/gazebo/libgazebo/SConscript
===================================================================
--- code/branches/federation/gazebo/libgazebo/SConscript	2009-03-10 09:07:34 UTC (rev 7442)
+++ code/branches/federation/gazebo/libgazebo/SConscript	2009-03-10 09:37:04 UTC (rev 7443)
@@ -42,7 +42,7 @@
 pkgconfig = env.PkgConfig(target='libgazebo.pc', source=Value(install_prefix))
 env.Install(dir=install_prefix+'/lib/pkgconfig', source=pkgconfig)
 
-sources = Split('Server.cc Client.cc Iface.cc IfaceFactory.cc SimIface.cc')
+sources = Split('Server.cc Client.cc Iface.cc IfaceFactory.cc SimIface.cc Graphics3dIface.cc')
 headers = Split('gazebo.h IfaceFactory.hh')
 
 sharedLib = myEnv.SharedLibrary('gazebo', sources)

Modified: code/branches/federation/gazebo/libgazebo/gazebo.h
===================================================================
--- code/branches/federation/gazebo/libgazebo/gazebo.h	2009-03-10 09:07:34 UTC (rev 7442)
+++ code/branches/federation/gazebo/libgazebo/gazebo.h	2009-03-10 09:37:04 UTC (rev 7443)
@@ -59,6 +59,28 @@
 /// \addtogroup libgazebo
 /// \{
 
+
+/// \brief Vector 2 class
+class Vec2
+{
+  /// \brief Default Constructor
+  public: Vec2() : x(0), y(0) {}
+          
+  /// \brief Constructor
+  public: Vec2(float _x, float _y) : x(_x), y(_y) {}
+
+  /// \brief Copy constructor
+  public: Vec2(const Vec2 &vec) : x(vec.x), y(vec.y) {}
+
+  /// X value
+  public: float x;
+
+  /// Y value
+  public: float y;
+
+};
+
+
 /// \brief Vector 3 class
 class Vec3
 {
@@ -476,7 +498,6 @@
             this->Unlock();
 
             {
-              //boost::mutex::scoped_lock lock(*this->mutex);
               if (this->currentConnection.connected())
                 this->currentConnection.disconnect();
 
@@ -778,55 +799,81 @@
 
 /// Maximum number of points that can be drawn
 #define GAZEBO_GRAPHICS3D_MAX_POINTS 1024
+#define GAZEBO_GRAPHICS3D_MAX_NAME 128
+#define GAZEBO_GRAPHICS3D_MAX_COMMANDS 128
 
-/// \brief Graphics3d interface data
-class Graphics3dData
+class Graphics3dDrawData
 {
   /// Type of drawing to perform
-  enum DrawMode {POINTS, LINES, LINE_STRIP, LINE_LOOP, TRIANGLES, TRIANGLE_STRIP, TRIANGLE_FAN, QUADS, QUAD_STRIP, POLYGON};
+  public: enum DrawMode { POINTS, LINES, LINE_STRIP, TRIANGLES, TRIANGLE_STRIP, 
+                          TRIANGLE_FAN, PLANE, SPHERE, CUBE, CYLINDER,
+                          BILLBOARD };
 
   /// Drawing mode
-  public: DrawMode drawmode;
+  public: DrawMode drawMode;
 
+  public: char name[GAZEBO_GRAPHICS3D_MAX_NAME];
+
   /// Number of vertices
-  public: unsigned int point_count; 
+  public: unsigned int pointCount; 
 
   /// Vertices 
   public: Vec3 points[GAZEBO_GRAPHICS3D_MAX_POINTS];
 
   /// Drawing color
   public: Color color;
+
+  /// Texture to apply to a billboard. Only applicable when 
+  //  drawMode == BILLBOARD
+  public: char billboardTexture[GAZEBO_GRAPHICS3D_MAX_NAME];
+
+  /// Pose at which to draw a shape
+  public: Pose pose;
+
+  /// Size of the shape
+  public: Vec3 size;
 };
 
+/// \brief Graphics3d interface data
+class Graphics3dData
+{
+  public: Graphics3dDrawData commands[GAZEBO_GRAPHICS3D_MAX_COMMANDS];
+  public: unsigned int cmdCount;
+};
+
+
 /// \brief Graphics3d interface
 class Graphics3dIface : public Iface
 {
 
   /// \brief Constructor
-  public: Graphics3dIface():Iface("graphics3d", sizeof(Graphics3dIface)+sizeof(Graphics3dData)) {}
+  public: Graphics3dIface();
 
   /// \brief Destructor
-  public: virtual ~Graphics3dIface() {this->data = NULL;}
+  public: virtual ~Graphics3dIface();
 
   /// \brief Create the interface (used by Gazebo server)
   /// \param server Pointer to the server
   /// \param id Id of the interface
-  public: virtual void Create(Server *server, std::string id)
-          {
-            Iface::Create(server,id); 
-            this->data = (Graphics3dData*)this->mMap; 
-          }
+  public: virtual void Create(Server *server, std::string id);
 
   /// \brief Open an existing interface
   /// \param client Pointer to the client
   /// \param id Id of the interface
-  public: virtual void Open(Client *client, std::string id)
-          {
-            Iface::Open(client,id); 
-            this->data = (Graphics3dData*)this->mMap; 
-          }
+  public: virtual void Open(Client *client, std::string id);
 
+  /// \brief Draw a simple object, that is defined by a series of points
+  public: void DrawSimple(const char *name, Graphics3dDrawData::DrawMode mode, 
+                          Vec3 *point, unsigned int numPoints, Color clr );
 
+  /// \brief Draw a shape
+  public: void DrawShape(const char *name, Graphics3dDrawData::DrawMode mode,
+                         Vec3 pos, Vec3 size, Color clr);
+
+  /// \brief Draw a billboard
+  public: void DrawBillboard(const char *name, const char *texture, 
+                             Vec3 pos, Vec2 size); 
+
   /// Pointer to the graphics3d data
   public: Graphics3dData *data;
 };

Modified: code/branches/federation/gazebo/server/Entity.cc
===================================================================
--- code/branches/federation/gazebo/server/Entity.cc	2009-03-10 09:07:34 UTC (rev 7442)
+++ code/branches/federation/gazebo/server/Entity.cc	2009-03-10 09:37:04 UTC (rev 7443)
@@ -29,6 +29,7 @@
 #include "GazeboError.hh"
 #include "Global.hh"
 #include "OgreVisual.hh"
+#include "OgreVisualManager.hh"
 #include "World.hh"
 #include "PhysicsEngine.hh"
 #include "Entity.hh"
@@ -48,15 +49,20 @@
  
   this->selected = false;
 
+  std::ostringstream visname;
+  visname << "Entity_" << this->GetId() << "_VISUAL";
+
   if (this->parent)
   {
     this->parent->AddChild(this);
-    this->visualNode=new OgreVisual(this->parent->GetVisualNode());
+    this->visualNode = OgreVisualManager::Instance()->CreateVisual(
+        visname.str(), this->parent->GetVisualNode(), this);
     this->SetStatic(parent->IsStatic());
   }
   else
   {
-    this->visualNode = new OgreVisual(NULL);
+    this->visualNode = OgreVisualManager::Instance()->CreateVisual( 
+                                                              visname.str(), NULL, this );
   }
 
   // Add this to the phyic's engine
@@ -180,8 +186,30 @@
 }
 
 ////////////////////////////////////////////////////////////////////////////////
+/// Set the space id
+void Entity::SetSpaceId( dSpaceID spaceid )
+{
+  this->spaceId = spaceid;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Return the space id
+dSpaceID Entity::GetSpaceId() const
+{
+  return this->spaceId;
+}
+
+////////////////////////////////////////////////////////////////////////////////
 /// Returns true if the entities are the same. Checks only the name
 bool Entity::operator==(const Entity &ent) const 
 {
   return ent.GetName() == this->GetName();
 }
+
+Pose3d Entity::GetPoseRelative() const
+{
+  if (this->parent)
+    return this->parent->GetPose() - this->GetPose();
+  else
+    return this->GetPose();
+}

Modified: code/branches/federation/gazebo/server/Entity.hh
===================================================================
--- code/branches/federation/gazebo/server/Entity.hh	2009-03-10 09:07:34 UTC (rev 7442)
+++ code/branches/federation/gazebo/server/Entity.hh	2009-03-10 09:37:04 UTC (rev 7443)
@@ -32,6 +32,7 @@
 #include <ode/ode.h>
 
 #include "Common.hh"
+#include "Pose3d.hh"
 #include "Param.hh"
 
 namespace gazebo
@@ -97,7 +98,17 @@
   
     /// \brief True if the entity is selected by the user
     public: bool IsSelected() const;
-  
+
+    /// \brief Set the space id
+    public: void SetSpaceId( dSpaceID spaceid );
+
+    /// \brief Return the space id
+    public: dSpaceID GetSpaceId() const;
+
+    /// \brief Get the pose of the entity
+    public: virtual Pose3d GetPose() const = 0;// { return Pose3d(); }
+    public: Pose3d GetPoseRelative() const;
+
     /// \brief Returns true if the entities are the same. Checks only the name
     public: bool operator==(const Entity &ent) const;
 
@@ -113,8 +124,8 @@
     /// \brief Visual stuff
     protected: OgreVisual *visualNode;
   
-    /// \brief ODE Stuff (should be go somewhere else)
-    public: dSpaceID spaceId;
+    /// \brief ODE Stuff 
+    protected: dSpaceID spaceId;
   
     private: bool selected;
   };

Modified: code/branches/federation/gazebo/server/Model.cc
===================================================================
--- code/branches/federation/gazebo/server/Model.cc	2009-03-10 09:07:34 UTC (rev 7442)
+++ code/branches/federation/gazebo/server/Model.cc	2009-03-10 09:37:04 UTC (rev 7443)
@@ -29,6 +29,8 @@
 #include <sstream>
 #include <iostream>
 
+#include "OgreVisual.hh"
+#include "GraphicsIfaceHandler.hh"
 #include "Global.hh"
 #include "GazeboError.hh"
 #include "GazeboMessage.hh"
@@ -80,6 +82,7 @@
 
   Param::End();
 
+  this->graphicsHandler = NULL;
   this->parentBodyNameP = NULL;
   this->myBodyNameP = NULL;
 }
@@ -92,6 +95,9 @@
   std::map< std::string, Joint* >::iterator jiter;
   std::map< std::string, Controller* >::iterator citer;
 
+  if (this->graphicsHandler)
+    delete this->graphicsHandler;
+
   for (biter=this->bodies.begin(); biter != this->bodies.end(); biter++)
   {
     GZ_DELETE(biter->second);
@@ -211,6 +217,10 @@
 
   this->SetCollideMode( **this->collideP );
 
+  // Create the graphics iface handler
+  this->graphicsHandler = new GraphicsIfaceHandler();
+  this->graphicsHandler->Load(this->GetName(), this);
+
   return 0;
 
   // Get the name of the python module
@@ -330,6 +340,8 @@
   std::map<std::string, Body* >::iterator biter;
   std::map<std::string, Controller* >::iterator contIter;
 
+  this->graphicsHandler->Init();
+
   for (biter = this->bodies.begin(); biter!=this->bodies.end(); biter++)
     biter->second->Init();
 
@@ -392,6 +404,12 @@
     this->rpyP->SetValue(this->pose.rot);
   }
 
+  if (this->parent)
+    this->visualNode->SetPose(this->GetPose() - this->parent->GetPose());
+  else
+    this->visualNode->SetPose(this->GetPose());
+
+  this->graphicsHandler->Update();
   return this->UpdateChild();
 }
 
@@ -413,6 +431,9 @@
     biter->second->Fini();
   }
 
+  if (this->graphicsHandler)
+    delete this->graphicsHandler;
+
   return this->FiniChild();
 }
 
@@ -509,6 +530,7 @@
       childModel->SetPose(newPose);
     }
   }
+  
 }
 
 ////////////////////////////////////////////////////////////////////////////////
@@ -592,11 +614,12 @@
 }
 ////////////////////////////////////////////////////////////////////////////////
 // Get the current pose
-const Pose3d &Model::GetPose() const
+Pose3d Model::GetPose() const
 {
   return this->pose;
 }
 
+
 ////////////////////////////////////////////////////////////////////////////////
 // Create and return a new body
 Body *Model::CreateBody()
@@ -798,13 +821,6 @@
   this->joint->SetAxis( Vector3(0,1,0) );
   this->joint->SetParam( dParamHiStop, 0);
   this->joint->SetParam( dParamLoStop, 0);
-
-  /*  if (this->spaceId)
-    {
-      dSpaceDestroy(this->spaceId);
-      this->spaceId = parentModel->spaceId;
-    }
-    */
 }
 
 ////////////////////////////////////////////////////////////////////////////////

Modified: code/branches/federation/gazebo/server/Model.hh
===================================================================
--- code/branches/federation/gazebo/server/Model.hh	2009-03-10 09:07:34 UTC (rev 7442)
+++ code/branches/federation/gazebo/server/Model.hh	2009-03-10 09:37:04 UTC (rev 7443)
@@ -48,6 +48,7 @@
   class Body;
   class Controller;
   class HingeJoint;
+  class GraphicsIfaceHandler;
 
   /// \addtogroup gazebo_server
   /// \brief A model
@@ -122,7 +123,7 @@
     public: void SetAngularAccel( const Vector3 &vel );
  
     /// \brief Get the current pose
-    public: const Pose3d &GetPose() const;
+    public: virtual Pose3d GetPose() const;
   
     /// \brief Create and return a new body
     /// \return Pointer to a new body.
@@ -222,6 +223,8 @@
     // Name of a light (if the model is renderable:light)
     private: std::string lightName;
 
+    private: GraphicsIfaceHandler *graphicsHandler;
+
   /*  private: PyObject *pName;
       private: PyObject *pModule;
       private: PyObject *pFuncUpdate;

Modified: code/branches/federation/gazebo/server/SConscript
===================================================================
--- code/branches/federation/gazebo/server/SConscript	2009-03-10 09:07:34 UTC (rev 7442)
+++ code/branches/federation/gazebo/server/SConscript	2009-03-10 09:37:04 UTC (rev 7443)
@@ -31,7 +31,8 @@
            'Model.cc',
            'Simulator.cc',
            'Angle.cc',
-           'Param.cc'
+           'Param.cc',
+           'GraphicsIfaceHandler.cc'
            ]
 
 headers.append( 
@@ -53,7 +54,8 @@
            '#/server/XMLConfig.hh',
            '#/server/GazeboConfig.hh',
            '#/server/Angle.hh',
-           '#/server/Param.hh'
+           '#/server/Param.hh',
+           '#/server/GraphicsIfaceHandler.hh'
            ] )
 
 #staticObjs.append( env.StaticObject(sources) )

Modified: code/branches/federation/gazebo/server/Simulator.cc
===================================================================
--- code/branches/federation/gazebo/server/Simulator.cc	2009-03-10 09:07:34 UTC (rev 7442)
+++ code/branches/federation/gazebo/server/Simulator.cc	2009-03-10 09:37:04 UTC (rev 7443)
@@ -28,6 +28,7 @@
 #include <fstream>
 #include <sys/time.h>
 
+#include "OgreVisualManager.hh"
 #include "Body.hh"
 #include "Geom.hh"
 #include "Model.hh"
@@ -330,6 +331,8 @@
       //this->prevRenderTime = this->GetRealTime();
     }
 
+    OgreVisualManager::Instance()->Update();
+
     // Update the gui
     if (this->gui)
     {

Modified: code/branches/federation/gazebo/server/World.cc
===================================================================
--- code/branches/federation/gazebo/server/World.cc	2009-03-10 09:07:34 UTC (rev 7442)
+++ code/branches/federation/gazebo/server/World.cc	2009-03-10 09:37:04 UTC (rev 7443)
@@ -28,6 +28,7 @@
 #include <sstream>
 #include <fstream>
 
+#include "GraphicsIfaceHandler.hh"
 #include "Global.hh"
 #include "GazeboError.hh"
 #include "GazeboMessage.hh"
@@ -60,6 +61,7 @@
   this->showPhysics = false;
   this->physicsEngine = NULL;
   this->server = NULL;
+  this->graphics = NULL;
 }
 
 ////////////////////////////////////////////////////////////////////////////////
@@ -74,7 +76,6 @@
 void World::Close()
 {
   std::vector< Model* >::iterator miter;
-
   for (miter = this->models.begin(); miter != this->models.end(); miter++)
   {
     if (*miter)
@@ -97,13 +98,13 @@
   {
     gzthrow(e);
   }
+
 }
 
 ////////////////////////////////////////////////////////////////////////////////
 // Load the world
 void World::Load(XMLConfigNode *rootNode, unsigned int serverId)
 {
-
   // Create the server object (needs to be done before models initialize)
   this->server = new Server();
 
@@ -116,7 +117,6 @@
     gzthrow (err);
   }
 
-
   // Create the simulator interface
   try
   {
@@ -128,6 +128,10 @@
     gzthrow(err);
   }
 
+  // Create the graphics iface handler
+  this->graphics = new GraphicsIfaceHandler();
+  this->graphics->Load("default");
+
 #ifdef HAVE_OPENAL
   // Load OpenAL audio 
   if (rootNode->GetChild("openal","audio"))
@@ -145,6 +149,7 @@
   }*/
 
   this->physicsEngine->Load(rootNode);
+
 }
 
 ////////////////////////////////////////////////////////////////////////////////
@@ -191,6 +196,8 @@
   this->toAddModels.clear();
   this->toDeleteModels.clear();
 
+  this->graphics->Init();
+
   return 0;
 }
 
@@ -236,6 +243,8 @@
     this->physicsEngine->Update();
   }
 
+  this->graphics->Update();
+
   return 0;
 }
 
@@ -251,6 +260,9 @@
 {
   std::vector< Model* >::iterator miter;
 
+  if (this->graphics)
+    delete this->graphics;
+
   // Finalize the models
   for (miter=this->models.begin(); miter!=this->models.end(); miter++)
   {
@@ -324,6 +336,7 @@
       return -1;
   }
 
+
   return 0;
 }
 
@@ -707,7 +720,6 @@
 
       case SimulationRequestData::GO:
         {
-
           this->simPauseTime = Simulator::Instance()->GetSimTime() 
                                   + req->runTime * 10e-6;
 

Modified: code/branches/federation/gazebo/server/World.hh
===================================================================
--- code/branches/federation/gazebo/server/World.hh	2009-03-10 09:07:34 UTC (rev 7442)
+++ code/branches/federation/gazebo/server/World.hh	2009-03-10 09:37:04 UTC (rev 7443)
@@ -46,6 +46,7 @@
   class Geom;
   class PhysicsEngine;
   class XMLConfigNode;
+  class GraphicsIfaceHandler;
    
 /// \brief The World
 /*
@@ -189,11 +190,11 @@
 
   /// Simulation interface
   private: SimulationIface *simIface;
+  private: GraphicsIfaceHandler *graphics;
 
   /// Length of time to run before receiving a "go" command
   private: double simPauseTime;
 
-
   private: friend class DestroyerT<World>;
   private: friend class SingletonT<World>;
 };

Modified: code/branches/federation/gazebo/server/controllers/SConscript
===================================================================
--- code/branches/federation/gazebo/server/controllers/SConscript	2009-03-10 09:07:34 UTC (rev 7442)
+++ code/branches/federation/gazebo/server/controllers/SConscript	2009-03-10 09:37:04 UTC (rev 7443)
@@ -3,7 +3,18 @@
 
 env.Append(CPPPATH = '#server/controllers')
 
-dirs = Split('position2d laser camera factory gripper actarray ptz opaque bumper imu irarray')
+dirs = ['position2d',
+        'laser',
+        'camera',
+        'factory',
+        'gripper',
+        'actarray',
+        'ptz',
+        'opaque',
+        'bumper',
+        'imu',
+        'irarray'
+        ]
 
 try:
   env["CCFLAGS"].index('-DHAVE_OPENAL')

Modified: code/branches/federation/gazebo/server/controllers/factory/Factory.hh
===================================================================
--- code/branches/federation/gazebo/server/controllers/factory/Factory.hh	2009-03-10 09:07:34 UTC (rev 7442)
+++ code/branches/federation/gazebo/server/controllers/factory/Factory.hh	2009-03-10 09:37:04 UTC (rev 7443)
@@ -24,8 +24,8 @@
  * Date: 29 July 2007
  * SVN: $Id$
  */
-#ifndef FACTORY_POSITION2D_HH
-#define FACTORY_POSITION2D_HH
+#ifndef FACTORY_HH
+#define FACTORY_HH
 
 #include "Controller.hh"
 #include "Entity.hh"

Modified: code/branches/federation/gazebo/server/controllers/position2d/differential/Differential_Position2d.cc
===================================================================
--- code/branches/federation/gazebo/server/controllers/position2d/differential/Differential_Position2d.cc	2009-03-10 09:07:34 UTC (rev 7442)
+++ code/branches/federation/gazebo/server/controllers/position2d/differential/Differential_Position2d.cc	2009-03-10 09:37:04 UTC (rev 7443)
@@ -197,6 +197,7 @@
     this->joints[LEFT]->SetParam( dParamFMax, **(this->torqueP) );
     this->joints[RIGHT]->SetParam( dParamFMax, **(this->torqueP) );
 
+    //printf("Set Speed[%f %f]\n",this->wheelSpeed[LEFT], this->wheelSpeed[RIGHT]);
   }
   /*else
   {

Modified: code/branches/federation/gazebo/server/main.cc
===================================================================
--- code/branches/federation/gazebo/server/main.cc	2009-03-10 09:07:34 UTC (rev 7442)
+++ code/branches/federation/gazebo/server/main.cc	2009-03-10 09:37:04 UTC (rev 7443)
@@ -277,7 +277,7 @@
   }
   catch (gazebo::GazeboError e)
   {
-    std::cerr << "Loading Gazebo" << std::endl;
+    std::cerr << "Erro Loading Gazebo" << std::endl;
     std::cerr << e << std::endl;
     return -1;
   }

Modified: code/branches/federation/gazebo/server/physics/Body.cc
===================================================================
--- code/branches/federation/gazebo/server/physics/Body.cc	2009-03-10 09:07:34 UTC (rev 7442)
+++ code/branches/federation/gazebo/server/physics/Body.cc	2009-03-10 09:37:04 UTC (rev 7443)
@@ -271,12 +271,12 @@
 
   this->UpdatePose();
 
-  if (!this->IsStatic())
+//  if (!this->IsStatic())
   {
-    // Set the pose of the scene node
-    this->visualNode->SetPose(this->pose);
+    this->visualNode->SetPose(this->GetPose() - this->parent->GetPose());
   }
 
+
   for (geomIter=this->geoms.begin();
        geomIter!=this->geoms.end(); geomIter++)
   {
@@ -327,16 +327,17 @@
 
 ////////////////////////////////////////////////////////////////////////////////
 // Set the pose of the body
-void Body::SetPose(const Pose3d &pose)
+void Body::SetPose(const Pose3d &_pose)
 {
-  this->pose = pose;
 
   if (this->IsStatic())
   {
     Pose3d oldPose = this->staticPose;
     Pose3d newPose;
-    this->staticPose = pose;
+    this->staticPose = _pose;
 
+    this->pose = this->staticPose;
+
     std::map<std::string, Geom*>::iterator iter;
 
     // This loop doesn't work properly when rotating objects
@@ -352,8 +353,9 @@
     Pose3d localPose;
 
     // Compute pose of CoM
-    localPose = this->comPose + this->pose;
+    localPose = this->comPose + _pose;
 
+    this->pose = localPose;
     this->SetPosition(localPose.pos);
     this->SetRotation(localPose.rot);
   }
@@ -363,9 +365,10 @@
 // Return the pose of the body
 Pose3d Body::GetPose() const
 {
-  if (this->IsStatic())
+  /*if (this->IsStatic())
     return this->staticPose;
   else
+  */
     return this->pose;
 }
 
@@ -386,8 +389,9 @@
   if (this->bodyId)
     dBodySetPosition(this->bodyId, pos.x, pos.y, pos.z);
 
+  // NATE
   // Set the position of the scene node
-  this->visualNode->SetPosition(pos);
+  //this->visualNode->SetPosition(pos);
 }
 
 ////////////////////////////////////////////////////////////////////////////////
@@ -407,8 +411,9 @@
     dBodySetQuaternion(this->bodyId, q);
   }
 
+  // NATE
   // Set the orientation of the scene node
-  this->visualNode->SetRotation(rot);
+  //this->visualNode->SetRotation(rot);
 }
 
 ////////////////////////////////////////////////////////////////////////////////

Modified: code/branches/federation/gazebo/server/physics/Body.hh
===================================================================
--- code/branches/federation/gazebo/server/physics/Body.hh	2009-03-10 09:07:34 UTC (rev 7442)
+++ code/branches/federation/gazebo/server/physics/Body.hh	2009-03-10 09:37:04 UTC (rev 7443)
@@ -83,7 +83,7 @@
 
     /// \brief Return the pose of the body
     /// \return Pose of the body
-    public: Pose3d GetPose() const;
+    public: virtual Pose3d GetPose() const;
 
     /// \brief Set the position of the body
     /// \param pos Vector position

Modified: code/branches/federation/gazebo/server/physics/ContactParams.cc
===================================================================
--- code/branches/federation/gazebo/server/physics/ContactParams.cc	2009-03-10 09:07:34 UTC (rev 7442)
+++ code/branches/federation/gazebo/server/physics/ContactParams.cc	2009-03-10 09:37:04 UTC (rev 7443)
@@ -45,8 +45,6 @@
   this->mu2 = dInfinity;
   this->slip1 = 0.1;
   this->slip2 = 0.1;
-
-  this->enabled = true;
 }
 
 //////////////////////////////////////////////////////////////////////////////

Modified: code/branches/federation/gazebo/server/physics/ContactParams.hh
===================================================================
--- code/branches/federation/gazebo/server/physics/ContactParams.hh	2009-03-10 09:07:34 UTC (rev 7442)
+++ code/branches/federation/gazebo/server/physics/ContactParams.hh	2009-03-10 09:37:04 UTC (rev 7443)
@@ -79,8 +79,6 @@
     /// \brief soft constraint force mixing
     public: double softCfm;
             
-    public: bool enabled;
-  
     public: boost::signal< void (Geom*, Geom*)> contactSignal;
   };
 }

Modified: code/branches/federation/gazebo/server/physics/Geom.cc
===================================================================
--- code/branches/federation/gazebo/server/physics/Geom.cc	2009-03-10 09:07:34 UTC (rev 7442)
+++ code/branches/federation/gazebo/server/physics/Geom.cc	2009-03-10 09:37:04 UTC (rev 7443)
@@ -26,6 +26,7 @@
 
 #include <sstream>
 
+#include "OgreVisualManager.hh"
 #include "OgreVisual.hh"
 #include "Global.hh"
 #include "GazeboMessage.hh"
@@ -46,7 +47,7 @@
   this->typeName = "unknown";
 
   this->body = body;
-  this->spaceId = this->body->spaceId;
+  this->SetSpaceId(this->body->GetSpaceId());
 
   // Create the contact parameters
   this->contact = new ContactParams();
@@ -89,11 +90,12 @@
   if (this->transId)
     dGeomDestroy(this->transId);
 
-  for (iter = this->visuals.begin(); iter != this->visuals.end(); iter++)
+  /*for (iter = this->visuals.begin(); iter != this->visuals.end(); iter++)
   {
     GZ_DELETE (*iter)
   }
   this->visuals.clear();
+  */
 
   delete this->massP;
   delete this->xyzP;
@@ -141,7 +143,12 @@
   childNode = node->GetChild("visual");
   while (childNode)
   {
-    OgreVisual *visual = new OgreVisual(this->visualNode, this);
+    std::ostringstream visname;
+    visname << this->GetName() << "_VISUAL_" << this->visuals.size();
+
+    OgreVisual *visual = OgreVisualManager::Instance()->CreateVisual(
+        visname.str(), this->visualNode, this);
+
     visual->Load(childNode);
     this->visuals.push_back(visual);
     childNode = childNode->GetNext("visual");
@@ -161,7 +168,11 @@
     Vector3 min(aabb[0], aabb[2], aabb[4]);
     Vector3 max(aabb[1], aabb[3], aabb[5]);
 
-    this->bbVisual = new OgreVisual(this->visualNode);
+    std::ostringstream visname;
+    visname << this->GetName() << "_BBVISUAL" ;
+
+    this->bbVisual = OgreVisualManager::Instance()->CreateVisual(
+        visname.str(), this->visualNode);
     this->bbVisual->AttachBoundingBox(min,max);
   }
 
@@ -258,8 +269,9 @@
 // Update
 void Geom::Update()
 {
+
+  this->visualNode->SetPose(this->GetPose());
   this->UpdateChild();
-
 }
 
 ////////////////////////////////////////////////////////////////////////////////
@@ -315,7 +327,8 @@
     dGeomSetPosition(this->geomId, localPose.pos.x, localPose.pos.y, localPose.pos.z);
     dGeomSetQuaternion(this->geomId, q);
 
-    this->visualNode->SetPose(newPose);
+    // NATE
+    //this->visualNode->SetPose(this->GetPoseRelative());
 
     if (updateCoM)
     {
@@ -353,9 +366,11 @@
     // Transform into body relative pose
     pose += this->body->GetCoMPose();
   }
-  else
+  /*else
     pose = this->body->GetPose();
+    */
 
+
   return pose;
 }
 
@@ -579,9 +594,20 @@
 /// Set the friction mode of the geom
 void Geom::SetFrictionMode( const bool &v )
 {
-  this->contact->mu1 = 0;
-  this->contact->mu2 = 0;
-  this->contact->slip1 = 0;
-  this->contact->slip2 = 0;
+  if (v == false)
+  {
+    this->contact->mu1 = 0;
+    this->contact->mu2 = 0;
+    this->contact->slip1 = 0;
+    this->contact->slip2 = 0;
+  }
+  else
+  {
+    this->contact->mu1 = dInfinity;
+    this->contact->mu2 = dInfinity;
+    this->contact->slip1 = 0.1;
+    this->contact->slip2 = 0.1;
+
+  }
 }
 

Modified: code/branches/federation/gazebo/server/physics/Geom.hh
===================================================================
--- code/branches/federation/gazebo/server/physics/Geom.hh	2009-03-10 09:07:34 UTC (rev 7442)
+++ code/branches/federation/gazebo/server/physics/Geom.hh	2009-03-10 09:37:04 UTC (rev 7443)
@@ -99,7 +99,7 @@
     public: void SetPose(const Pose3d &pose, bool updateCoM=true);
   
     /// \brief Return the pose of the geom
-    public: Pose3d GetPose() const;
+    public: virtual Pose3d GetPose() const;
   
     /// \brief Set the position
     /// \param pos Vector3 position
@@ -164,7 +164,6 @@
 
     ///  Contact parameters
     public: ContactParams *contact; 
-
   
     /// The body this geom belongs to
     protected: Body *body;
@@ -184,7 +183,7 @@
 
     /// mass of the body
     protected: dMass bodyMass;
- 
+
     private: ParamT<int> *laserFiducialIdP;
     private: ParamT<float> *laserRetroP;
 

Modified: code/branches/federation/gazebo/server/physics/Joint.cc
===================================================================
--- code/branches/federation/gazebo/server/physics/Joint.cc	2009-03-10 09:07:34 UTC (rev 7442)
+++ code/branches/federation/gazebo/server/physics/Joint.cc	2009-03-10 09:37:04 UTC (rev 7443)
@@ -24,6 +24,7 @@
  * CVS: $Id$
  */
 #include "OgreVisual.hh"
+#include "OgreVisualManager.hh"
 #include "OgreDynamicLines.hh"
 #include "Global.hh"
 #include "Body.hh"
@@ -115,8 +116,12 @@
 
   this->Attach(body1,body2);
 
+  std::ostringstream visname;
+  visname << this->GetName() << "_VISUAL";
+
   /// Add a renderable for the joint
-  this->visual = new OgreVisual(this->model->GetVisualNode());
+  this->visual = OgreVisualManager::Instance()->CreateVisual(
+      visname.str(), this->model->GetVisualNode());
   this->visual->AttachMesh("joint_anchor");
   this->visual->SetVisible(false);
 

Modified: code/branches/federation/gazebo/server/physics/ode/ODEPhysics.cc
===================================================================
--- code/branches/federation/gazebo/server/physics/ode/ODEPhysics.cc	2009-03-10 09:07:34 UTC (rev 7442)
+++ code/branches/federation/gazebo/server/physics/ode/ODEPhysics.cc	2009-03-10 09:37:04 UTC (rev 7443)
@@ -158,12 +158,11 @@
   // Only the top level parent should have a new space
   if (entity->GetParent() == NULL)
   {
-    //entity->spaceId = this->spaceId;
-    entity->spaceId = dSimpleSpaceCreate(this->spaceId);
+    entity->SetSpaceId( dSimpleSpaceCreate(this->spaceId) );
   }
   else
   {
-    entity->spaceId = entity->GetParent()->spaceId;
+    entity->SetSpaceId( entity->GetParent()->GetSpaceId() ) ;
   }
 }
 

Modified: code/branches/federation/gazebo/server/rendering/OgreAdaptor.cc
===================================================================
--- code/branches/federation/gazebo/server/rendering/OgreAdaptor.cc	2009-03-10 09:07:34 UTC (rev 7442)
+++ code/branches/federation/gazebo/server/rendering/OgreAdaptor.cc	2009-03-10 09:37:04 UTC (rev 7443)
@@ -489,9 +489,9 @@
     {
 
       OgreVisual *vis = dynamic_cast<OgreVisual*>(iter->movable->getUserObject());
-      if (vis && vis->GetEntity())
+      if (vis && vis->GetOwner())
       {
-        entity = vis->GetEntity();
+        entity = vis->GetOwner();
         entity->GetVisualNode()->ShowSelectionBox(true);
         Model *model = NULL;
         

Modified: code/branches/federation/gazebo/server/rendering/OgreCreator.cc
===================================================================
--- code/branches/federation/gazebo/server/rendering/OgreCreator.cc	2009-03-10 09:07:34 UTC (rev 7442)
+++ code/branches/federation/gazebo/server/rendering/OgreCreator.cc	2009-03-10 09:37:04 UTC (rev 7443)
@@ -611,3 +611,41 @@
 
   return window;
 }
+
+////////////////////////////////////////////////////////////////////////////////
+// Create a material from a color definition
+std::string OgreCreator::CreateMaterial(float r, float g, float b, float a)
+{
+  std::ostringstream matNameStream;
+
+  matNameStream << "Color[" << r << "," << g << "," << b << "," << a << "]";
+
+  if (!Ogre::MaterialManager::getSingleton().resourceExists(matNameStream.str()))
+  {
+    Ogre::MaterialPtr matPtr = Ogre::MaterialManager::getSingleton().create(
+        matNameStream.str(),"General");
+
+    matPtr->getTechnique(0)->setLightingEnabled(true);
+    matPtr->getTechnique(0)->getPass(0)->setDiffuse(r,g,b,a);
+    matPtr->getTechnique(0)->getPass(0)->setAmbient(r,g,b);
+    matPtr->getTechnique(0)->getPass(0)->setSelfIllumination(r,g,b);
+  //  matPtr->setReceiveShadows(false);
+  }
+
+  return matNameStream.str();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Create a material from a texture file
+std::string OgreCreator::CreateMaterialFromTexFile(const std::string &filename)
+{
+  if (!Ogre::MaterialManager::getSingleton().resourceExists(filename))
+  {
+    Ogre::MaterialPtr matPtr = Ogre::MaterialManager::getSingleton().create(
+        filename, "General");
+    matPtr->setLightingEnabled(true);
+    matPtr->getTechnique(0)->getPass(0)->createTextureUnitState(filename);
+  }
+
+  return filename;
+}

Modified: code/branches/federation/gazebo/server/rendering/OgreCreator.hh
===================================================================
--- code/branches/federation/gazebo/server/rendering/OgreCreator.hh	2009-03-10 09:07:34 UTC (rev 7442)
+++ code/branches/federation/gazebo/server/rendering/OgreCreator.hh	2009-03-10 09:37:04 UTC (rev 7443)
@@ -116,6 +116,14 @@
     /// \brief Remove a mesh by name
     public: static void RemoveMesh(const std::string &name);
 
+    /// \brief Create a material from a color definition
+    /// \return The name of the material
+    public: static std::string CreateMaterial(float r, float g, 
+                                              float b, float a);
+
+    /// \brief Create a material from a texture file
+    public: static std::string CreateMaterialFromTexFile(const std::string &filename);
+
     private: static unsigned int lightCounter;
     private: static unsigned int windowCounter;
 

Modified: code/branches/federation/gazebo/server/rendering/OgreVisual.cc
===================================================================
--- code/branches/federation/gazebo/server/rendering/OgreVisual.cc	2009-03-10 09:07:34 UTC (rev 7442)
+++ code/branches/federation/gazebo/server/rendering/OgreVisual.cc	2009-03-10 09:37:04 UTC (rev 7443)
@@ -39,7 +39,7 @@
 
 ////////////////////////////////////////////////////////////////////////////////
 // Constructor
-OgreVisual::OgreVisual(OgreVisual *node, Entity *owner)
+OgreVisual::OgreVisual(OgreVisual *node, Entity *_owner)
   : Common()
 {
   std::ostringstream stream;
@@ -58,7 +58,7 @@
   // Create the scene node
   this->sceneNode = this->parentNode->createChildSceneNode( stream.str() );
 
-  this->entity = owner;
+  this->owner = _owner;
 
   Param::Begin(&this->parameters);
   this->xyzP = new ParamT<Vector3>("xyz", Vector3(0,0,0), 0);
@@ -239,6 +239,27 @@
 }
 
 ////////////////////////////////////////////////////////////////////////////////
+/// Detach all objects
+void OgreVisual::DetachObjects()
+{
+  this->sceneNode->detachAllObjects();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Get the number of attached objects
+unsigned short OgreVisual::GetNumAttached()
+{
+  return this->sceneNode->numAttachedObjects();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Get an attached object
+Ogre::MovableObject *OgreVisual::GetAttached(unsigned short num)
+{
+  return this->sceneNode->getAttachedObject(num);
+}
+
+////////////////////////////////////////////////////////////////////////////////
 // Attach a static object
 void OgreVisual::MakeStatic()
 {
@@ -360,6 +381,9 @@
   }
 }
 
+
+
+
 ////////////////////////////////////////////////////////////////////////////////
 /// Set the transparency
 void OgreVisual::SetTransparency( float trans )
@@ -595,9 +619,9 @@
 
 ////////////////////////////////////////////////////////////////////////////////
 /// Get the entity that manages this visual
-Entity *OgreVisual::GetEntity() const
+Entity *OgreVisual::GetOwner() const
 {
-  return this->entity;
+  return this->owner;
 }
 
 ////////////////////////////////////////////////////////////////////////////////

Modified: code/branches/federation/gazebo/server/rendering/OgreVisual.hh
===================================================================
--- code/branches/federation/gazebo/server/rendering/OgreVisual.hh	2009-03-10 09:07:34 UTC (rev 7442)
+++ code/branches/federation/gazebo/server/rendering/OgreVisual.hh	2009-03-10 09:37:04 UTC (rev 7443)
@@ -57,6 +57,15 @@
     /// \brief Attach a renerable object to the visual
     public: void AttachObject( Ogre::MovableObject *obj);
 
+    /// \brief Detach all objects
+    public: void DetachObjects();
+
+    /// \brief Get the number of attached objects
+    public: unsigned short GetNumAttached();
+
+    /// \brief Get an attached object
+    public: Ogre::MovableObject *GetAttached(unsigned short num);
+
     /// \brief Attach a mesh to this visual by name
     public: void AttachMesh( const std::string &meshName );
 
@@ -118,7 +127,7 @@
     public: void MakeStatic();
 
     /// \brief Get the entity that manages this visual
-    public: Entity *GetEntity() const;
+    public: Entity *GetOwner() const;
 
     /// \brief Set to true to show a white bounding box, used to indicate 
     //         user selection
@@ -138,7 +147,7 @@
 
     private: static unsigned int visualCounter;
 
-    private: Entity *entity;
+    private: Entity *owner;
 
     private: ParamT<Vector3> *xyzP;
     private: ParamT<Quatern> *rpyP;

Modified: code/branches/federation/gazebo/server/rendering/SConscript
===================================================================
--- code/branches/federation/gazebo/server/rendering/SConscript	2009-03-10 09:07:34 UTC (rev 7442)
+++ code/branches/federation/gazebo/server/rendering/SConscript	2009-03-10 09:37:04 UTC (rev 7443)
@@ -13,6 +13,7 @@
            'OgreSimpleShape.cc',
            'OgreHUD.cc',
            'OgreVisual.cc',
+           'OgreVisualManager.cc',
            'OgreCamera.cc',
            'CameraManager.cc',
            'UserCamera.cc'
@@ -28,6 +29,7 @@
            '#/server/rendering/OgreHUD.hh',
            '#/server/rendering/OgreSimpleShape.hh',
            '#/server/rendering/OgreVisual.hh',
+           '#/server/rendering/OgreVisualManager.hh',
            '#/server/rendering/OgreCamera.hh',
            '#/server/rendering/CameraManager.hh',
            '#/server/rendering/UserCamera.hh'

Modified: code/branches/federation/gazebo/server/sensors/Sensor.cc
===================================================================
--- code/branches/federation/gazebo/server/sensors/Sensor.cc	2009-03-10 09:07:34 UTC (rev 7442)
+++ code/branches/federation/gazebo/server/sensors/Sensor.cc	2009-03-10 09:37:04 UTC (rev 7443)
@@ -190,7 +190,7 @@
 
 ////////////////////////////////////////////////////////////////////////////////
 /// Get the current pose
-const Pose3d Sensor::GetPose() const
+Pose3d Sensor::GetPose() const
 {
   return this->body->GetPose();
 }

Modified: code/branches/federation/gazebo/server/sensors/Sensor.hh
===================================================================
--- code/branches/federation/gazebo/server/sensors/Sensor.hh	2009-03-10 09:07:34 UTC (rev 7442)
+++ code/branches/federation/gazebo/server/sensors/Sensor.hh	2009-03-10 09:37:04 UTC (rev 7443)
@@ -70,7 +70,7 @@
     public: void Fini();
 
     /// \brief Get the current pose
-    public: const Pose3d GetPose() const;
+    public: virtual Pose3d GetPose() const;
  
     /// \brief Set whether the sensor is active or not
     public: void SetActive(bool value);

Modified: code/branches/federation/gazebo/server/sensors/ir/IRSensor.cc
===================================================================
--- code/branches/federation/gazebo/server/sensors/ir/IRSensor.cc	2009-03-10 09:07:34 UTC (rev 7442)
+++ code/branches/federation/gazebo/server/sensors/ir/IRSensor.cc	2009-03-10 09:37:04 UTC (rev 7443)
@@ -129,9 +129,7 @@
   dGeomSetCategoryBits((dGeomID) this->raySpaceId, GZ_SENSOR_COLLIDE);
   dGeomSetCollideBits((dGeomID) this->raySpaceId, ~GZ_SENSOR_COLLIDE);
 
-  //this->body->spaceId = this->superSpaceId;
-  this->body->spaceId = this->raySpaceId;
-
+  this->body->SetSpaceId( this->raySpaceId );
 }
 
 //////////////////////////////////////////////////////////////////////////////

Modified: code/branches/federation/gazebo/server/sensors/ray/RaySensor.cc
===================================================================
--- code/branches/federation/gazebo/server/sensors/ray/RaySensor.cc	2009-03-10 09:07:34 UTC (rev 7442)
+++ code/branches/federation/gazebo/server/sensors/ray/RaySensor.cc	2009-03-10 09:37:04 UTC (rev 7443)
@@ -111,9 +111,7 @@
   dGeomSetCategoryBits((dGeomID) this->raySpaceId, GZ_SENSOR_COLLIDE);
   dGeomSetCollideBits((dGeomID) this->raySpaceId, ~GZ_SENSOR_COLLIDE);
 
-  //this->body->spaceId = this->superSpaceId;
-  this->body->spaceId = this->raySpaceId;
-
+  this->body->SetSpaceId( this->raySpaceId );
 }
 
 //////////////////////////////////////////////////////////////////////////////

Modified: code/branches/federation/gazebo/worlds/pioneer2dx.world
===================================================================
--- code/branches/federation/gazebo/worlds/pioneer2dx.world	2009-03-10 09:07:34 UTC (rev 7442)
+++ code/branches/federation/gazebo/worlds/pioneer2dx.world	2009-03-10 09:37:04 UTC (rev 7443)
@@ -96,9 +96,7 @@
   <model:physical name="pioneer2dx_model1">
     <xyz>0 0 .145</xyz>
     <rpy>0.0 0.0 90.0</rpy>
-    <enableGravity>false</enableGravity>
-    <enableFriction>false</enableFriction>
-    <!--<collide>ghost</collide>-->
+    <collide>all</collide>
 
     <controller:differential_position2d name="controller1">
       <leftJoint>left_wheel_hinge</leftJoint>
@@ -109,10 +107,8 @@
       <interface:position name="position_iface_0"/>
     </controller:differential_position2d>
 
-    <model:physical name="laser">
+    <!--<model:physical name="laser">
       <xyz>0.15 0 0.18</xyz>
-      <enableGravity>false</enableGravity>
-      <!--      <collide>ghost</collide>-->
 
       <attach>
         <parentBody>chassis_body</parentBody>
@@ -123,32 +119,13 @@
         <xi:include href="models/sicklms200.model" />
       </include>
     </model:physical>
+    -->
 
     <include embedded="true">
       <xi:include href="models/pioneer2dx.model" />
     </include>
   </model:physical>
 
-  <model:physical name="box1_model">
-    <xyz>0 0.5 0.5</xyz>
-    <canonicalBody>box1_body</canonicalBody>
-    <static>false</static>
-
-    <body:box name="box1_body">
-
-      <geom:box name="box1_geom">
-        <size>1 .1 1</size>
-        <mass>0.1</mass>
-        <visual>
-          <size>1 .1 1</size>
-          <mesh>unit_box</mesh>
-          <material>Gazebo/Rocky</material>
-        </visual>
-      </geom:box>
-    </body:box>
-  </model:physical>
-
-
   <!-- White Point light -->
   <model:renderable name="point_white">
     <xyz>0 2 2</xyz>


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.