Revision: 8969
          http://playerstage.svn.sourceforge.net/playerstage/?rev=8969&view=rev
Author:   natepak
Date:     2010-11-15 15:52:22 +0000 (Mon, 15 Nov 2010)

Log Message:
-----------
Fixed bugs with multi-world upgrade

Modified Paths:
--------------
    code/gazebo/branches/dev/cmake/SearchForStuff.cmake
    code/gazebo/branches/dev/libgazebo/CMakeLists.txt
    code/gazebo/branches/dev/server/CMakeLists.txt
    code/gazebo/branches/dev/server/Events.cc
    code/gazebo/branches/dev/server/FactoryIfaceHandler.cc
    code/gazebo/branches/dev/server/Model.cc
    code/gazebo/branches/dev/server/Model.hh
    code/gazebo/branches/dev/server/OgreLoader.hh
    code/gazebo/branches/dev/server/SimulationIfaceHandler.cc
    code/gazebo/branches/dev/server/Simulator.cc
    code/gazebo/branches/dev/server/Simulator.hh
    code/gazebo/branches/dev/server/World.cc
    code/gazebo/branches/dev/server/World.hh
    code/gazebo/branches/dev/server/controllers/Controller.cc
    code/gazebo/branches/dev/server/controllers/Controller.hh
    code/gazebo/branches/dev/server/controllers/audio/Audio.cc
    code/gazebo/branches/dev/server/main.cc
    code/gazebo/branches/dev/server/physics/Body.cc
    code/gazebo/branches/dev/server/physics/Geom.cc
    code/gazebo/branches/dev/server/physics/Geom.hh
    code/gazebo/branches/dev/server/physics/ode/ODEPhysics.cc
    code/gazebo/branches/dev/server/rendering/OgreAdaptor.cc
    code/gazebo/branches/dev/server/rendering/OgreAdaptor.hh
    code/gazebo/branches/dev/server/sensors/Sensor.cc
    code/gazebo/branches/dev/server/wx/BoxMaker.cc
    code/gazebo/branches/dev/server/wx/CylinderMaker.cc
    code/gazebo/branches/dev/server/wx/DirectionalLightMaker.cc
    code/gazebo/branches/dev/server/wx/PhysicsPage.cc
    code/gazebo/branches/dev/server/wx/PointLightMaker.cc
    code/gazebo/branches/dev/server/wx/RenderControl.cc
    code/gazebo/branches/dev/server/wx/SimulationApp.cc
    code/gazebo/branches/dev/server/wx/SimulationApp.hh
    code/gazebo/branches/dev/server/wx/SimulationFrame.cc
    code/gazebo/branches/dev/server/wx/SphereMaker.cc
    code/gazebo/branches/dev/server/wx/SpotLightMaker.cc
    code/gazebo/branches/dev/server/wx/TimePanel.cc
    code/gazebo/branches/dev/worlds/empty.world

Modified: code/gazebo/branches/dev/cmake/SearchForStuff.cmake
===================================================================
--- code/gazebo/branches/dev/cmake/SearchForStuff.cmake 2010-11-15 03:06:34 UTC 
(rev 8968)
+++ code/gazebo/branches/dev/cmake/SearchForStuff.cmake 2010-11-15 15:52:22 UTC 
(rev 8969)
@@ -423,11 +423,11 @@
 
 ########################################
 # Find profiler library, optional
-FIND_LIBRARY(PROFILER "profiler")
-IF (PROFILER)
-  SET (CMAKE_LINK_FLAGS_PROFILE "${CMAKE_LINK_FLAGS_PROFILE} -lprofiler" 
-       CACHE INTERNAL "Link flags for profile" FORCE)
-ENDIF (PROFILER)
+#FIND_LIBRARY(PROFILER "profiler")
+#IF (PROFILER)
+#  SET (CMAKE_LINK_FLAGS_PROFILE "${CMAKE_LINK_FLAGS_PROFILE} -lprofiler" 
+#       CACHE INTERNAL "Link flags for profile" FORCE)
+#ENDIF (PROFILER)
 
 ########################################
 # Find tcmalloc library, optional

Modified: code/gazebo/branches/dev/libgazebo/CMakeLists.txt
===================================================================
--- code/gazebo/branches/dev/libgazebo/CMakeLists.txt   2010-11-15 03:06:34 UTC 
(rev 8968)
+++ code/gazebo/branches/dev/libgazebo/CMakeLists.txt   2010-11-15 15:52:22 UTC 
(rev 8969)
@@ -28,7 +28,7 @@
 endif (CMAKE_LINK_FLAGS_${CMAKE_BUILD_TYPE})
 
 
-target_link_libraries( gazeboshm ${boost_libraries} profiler)
+target_link_libraries( gazeboshm ${boost_libraries})
 
 install (TARGETS gazeboshm DESTINATION ${CMAKE_INSTALL_PREFIX}/lib)
 install (FILES ${headers} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/gazebo)

Modified: code/gazebo/branches/dev/server/CMakeLists.txt
===================================================================
--- code/gazebo/branches/dev/server/CMakeLists.txt      2010-11-15 03:06:34 UTC 
(rev 8968)
+++ code/gazebo/branches/dev/server/CMakeLists.txt      2010-11-15 15:52:22 UTC 
(rev 8969)
@@ -96,7 +96,6 @@
              OgreLoader.cc
              AssimpLoader.cc
              STLLoader.cc
-             Logger.cc
              Plugin.cc
              Events.cc
 )
@@ -131,7 +130,6 @@
              OgreLoader.hh
              AssimpLoader.hh
              STLLoader.hh
-             Logger.hh
              Events.hh
              Plugin.hh
 )

Modified: code/gazebo/branches/dev/server/Events.cc
===================================================================
--- code/gazebo/branches/dev/server/Events.cc   2010-11-15 03:06:34 UTC (rev 
8968)
+++ code/gazebo/branches/dev/server/Events.cc   2010-11-15 15:52:22 UTC (rev 
8969)
@@ -3,7 +3,7 @@
 using namespace gazebo;
 
 boost::signal<void (bool)> Events::pauseSignal;
-boost::signal<void (bool)> Events::stepSignal;
+boost::signal<void ()> Events::stepSignal;
 boost::signal<void (bool)> Events::moveModeSignal;
 boost::signal<void (bool)> Events::manipModeSignal;
 

Modified: code/gazebo/branches/dev/server/FactoryIfaceHandler.cc
===================================================================
--- code/gazebo/branches/dev/server/FactoryIfaceHandler.cc      2010-11-15 
03:06:34 UTC (rev 8968)
+++ code/gazebo/branches/dev/server/FactoryIfaceHandler.cc      2010-11-15 
15:52:22 UTC (rev 8969)
@@ -51,7 +51,7 @@
   // Create the iface
   try
   {
-    this->factoryIface->Create(this->world->GetGzServer(), 
this->world->GetGzServer());
+    this->factoryIface->Create(this->world->GetGzServer(), 
this->world->GetGzServer()->serverName);
     this->factoryIface->Lock(1); // lock it right away to clear up data
     strcpy((char*)this->factoryIface->data->newModel,"");
     this->factoryIface->Unlock();

Modified: code/gazebo/branches/dev/server/Model.cc
===================================================================
--- code/gazebo/branches/dev/server/Model.cc    2010-11-15 03:06:34 UTC (rev 
8968)
+++ code/gazebo/branches/dev/server/Model.cc    2010-11-15 15:52:22 UTC (rev 
8969)
@@ -24,8 +24,6 @@
  * SVN: $Id$
  */
 
-//#include <boost/python.hpp>
-
 #include <tbb/parallel_for.h>
 #include <tbb/blocked_range.h>
 
@@ -225,8 +223,7 @@
   this->laserFiducialP->Load(node);
   this->laserRetroP->Load(node);
 
-  this->xmlNode = node;
-  this->modelType=node->GetName();
+  this->modelType = node->GetString("type", "physical", 1);
 
   this->SetStatic( **(this->staticP) );
 
@@ -255,11 +252,11 @@
   this->SetInitPose(pose);
 
   // Load controllers
-  childNode = node->GetChildByNSPrefix("controller");
+  childNode = node->GetChild("controller");
   while (childNode)
   {
     this->LoadController(childNode);
-    childNode = childNode->GetNextByNSPrefix("controller");
+    childNode = childNode->GetNext("controller");
   }
 
   // Create a default body if one does not exist in the XML file
@@ -308,26 +305,6 @@
   // Create the graphics iface handler
   this->graphicsHandler = new GraphicsIfaceHandler(this->GetWorld());
   this->graphicsHandler->Load(this->GetScopedName(), this);
-
-  // Get the name of the python module
-  
/*this->pName.reset(PyString_FromString(node->GetString("python","",0).c_str()));
-  //this->pName.reset(PyString_FromString("pioneer2dx"));
-
-  // Import the python module
-  if (this->pName)
-  {
-  this->pModule.reset(PyImport_Import(this->pName));
-  Py_DECREF(this->pName);
-  }
-
-  // Get the Update function from the module
-  if (this->pModule)
-  {
-  this->pFuncUpdate.reset(PyObject_GetAttrString(this->pModule, "Update"));
-  if (this->pFuncUpdate && !PyCallable_Check(this->pFuncUpdate))
-  this->pFuncUpdate = NULL;
-  }
-   */
 }
 
 
////////////////////////////////////////////////////////////////////////////////

Modified: code/gazebo/branches/dev/server/Model.hh
===================================================================
--- code/gazebo/branches/dev/server/Model.hh    2010-11-15 03:06:34 UTC (rev 
8968)
+++ code/gazebo/branches/dev/server/Model.hh    2010-11-15 15:52:22 UTC (rev 
8969)
@@ -257,9 +257,6 @@
     /// \brief Type of the model (such as Pioneer2DX, or SimpleSolid)
     private: std::string modelType;
   
-    /// \brief The node this model was loaded from
-    private: XMLConfigNode *xmlNode;
-  
     /// \brief Initial pose of the model
     private: Pose3d initPose;
  

Modified: code/gazebo/branches/dev/server/OgreLoader.hh
===================================================================
--- code/gazebo/branches/dev/server/OgreLoader.hh       2010-11-15 03:06:34 UTC 
(rev 8968)
+++ code/gazebo/branches/dev/server/OgreLoader.hh       2010-11-15 15:52:22 UTC 
(rev 8969)
@@ -33,6 +33,7 @@
 #include <string>
 #include <list>
 #include <vector>
+#include <stdint.h>
 
 #include "MeshLoader.hh"
 #include "GazeboError.hh"

Modified: code/gazebo/branches/dev/server/SimulationIfaceHandler.cc
===================================================================
--- code/gazebo/branches/dev/server/SimulationIfaceHandler.cc   2010-11-15 
03:06:34 UTC (rev 8968)
+++ code/gazebo/branches/dev/server/SimulationIfaceHandler.cc   2010-11-15 
15:52:22 UTC (rev 8969)
@@ -34,7 +34,8 @@
 #include "World.hh"
 #include "PhysicsEngine.hh"
 #include "Joint.hh"
-#include "Logger.hh"
+// NATY: put back in
+//#include "Logger.hh"
 #include "gz.h"
 
 #include "SimulationIfaceHandler.hh"
@@ -846,13 +847,15 @@
 
      case libgazebo::SimulationRequestData::START_LOG:
         {
-          Logger::Instance()->AddLog(req->name, req->strValue);
+          // NATY: put back in
+          //Logger::Instance()->AddLog(req->name, req->strValue);
           break;
         }
 
      case libgazebo::SimulationRequestData::STOP_LOG:
         {
-          Logger::Instance()->RemoveLog(req->name);
+          // NATY: Put back in
+          // Logger::Instance()->RemoveLog(req->name);
           break;
         }
 

Modified: code/gazebo/branches/dev/server/Simulator.cc
===================================================================
--- code/gazebo/branches/dev/server/Simulator.cc        2010-11-15 03:06:34 UTC 
(rev 8968)
+++ code/gazebo/branches/dev/server/Simulator.cc        2010-11-15 15:52:22 UTC 
(rev 8969)
@@ -122,12 +122,13 @@
   physicsQuit(false),
   guiEnabled(true),
   renderEngineEnabled(true),
-  physicsEnabled(true),
+  physicsEnabled(true)
 {
   PhysicsFactory::RegisterAll();
+  this->activeWorldIndex = 0;
 
-  this->render_mutex = new boost::recursive_mutex();
-  this->model_delete_mutex = new boost::recursive_mutex();
+  //this->render_mutex = new boost::recursive_mutex();
+  //this->model_delete_mutex = new boost::recursive_mutex();
   this->gazeboConfig=new gazebo::GazeboConfig();
 }
 
@@ -135,12 +136,14 @@
 // Destructor
 Simulator::~Simulator()
 {
+  this->Fini();
   if (this->gazeboConfig)
   {
     delete this->gazeboConfig;
     this->gazeboConfig = NULL;
   }
 
+  /* NATY
   if (this->render_mutex)
   {
     delete this->render_mutex;
@@ -152,10 +155,7 @@
     delete this->model_delete_mutex;
     this->model_delete_mutex = NULL;
   }
-
-  for (unsigned int i=0; i < this->worlds.size(); i++)
-    delete this->worlds[i];
-  this->worlds[i].clear();
+  */
 }
 
 
////////////////////////////////////////////////////////////////////////////////
@@ -172,9 +172,6 @@
   }
   this->worlds.clear();
 
-  if (this->renderEngineEnabled)
-    gazebo::OgreAdaptor::Instance()->Close();
-
   if (this->gui)
   {
     delete this->gui;
@@ -210,7 +207,7 @@
   }
 
   XMLConfigNode *rootNode(xmlFile->GetRootNode());
-  XMLConfigNode *confgNode = rootNode->GetChild("config");
+  XMLConfigNode *configNode = rootNode->GetChild("config");
 
   // Load the messaging system
   gazebo::GazeboMessage::Instance()->Load(configNode);
@@ -323,6 +320,7 @@
       pluginNode = pluginNode->GetNext("plugin");
     }
 
+    this->worlds.push_back(world);
     worldNode = worldNode->GetNext("world");
   }
 
@@ -362,8 +360,40 @@
   else
     return NULL;
 }
- 
+
 
////////////////////////////////////////////////////////////////////////////////
+/// Set the active world
+void Simulator::SetActiveWorld(unsigned int i)
+{
+  if (i < this->worlds.size())
+    this->activeWorldIndex = i;
+  else
+    gzerr(0) << "Invalid world index[" << i << "]\n";
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Set the active world
+void Simulator::SetActiveWorld(World *world)
+{
+  unsigned int i=0;
+  for (; i < this->worlds.size(); i++)
+    if (this->worlds[i]->GetName() == world->GetName())
+      break;
+
+  if (i < this->worlds.size())
+    this->activeWorldIndex = i;
+  else
+    gzerr(0) << "Invalid world [" << world->GetName() << "]\n";
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Get the currently active world
+World *Simulator::GetActiveWorld() const
+{
+  return this->worlds[this->activeWorldIndex];
+}
+
+////////////////////////////////////////////////////////////////////////////////
 /// Initialize the simulation
 void Simulator::Init()
 {
@@ -439,13 +469,12 @@
   {
     while (!this->userQuit)
     {
-      currTime = this->GetWallTime();
+      currTime = Time::GetWallTime();
       if ( currTime - lastTime > 1.0/freq)
       {
-        lastTime = this->GetWallTime();
-
+        lastTime = Time::GetWallTime();
         this->GraphicsUpdate();
-        currTime = this->GetWallTime();
+        currTime = Time::GetWallTime();
         if (currTime - lastTime < 1/freq)
         {
           Time sleepTime = ( Time(1.0/freq) - (currTime - lastTime));
@@ -471,17 +500,19 @@
 
 void Simulator::GraphicsUpdate()
 {
-  if (this->gui)
-    this->gui->Update();
-
   if (this->renderEngineEnabled)
   {
     OgreAdaptor::Instance()->UpdateScenes();
-    World::Instance()->GraphicsUpdate();
+    for (unsigned int i=0; i < this->worlds.size(); i++)
+      this->worlds[i]->GraphicsUpdate();
   }
 
-  World::Instance()->ProcessEntitiesToLoad();
-  World::Instance()->ProcessEntitiesToDelete();
+  for (unsigned int i=0; i < this->worlds.size(); i++)
+  {
+    this->worlds[i]->ProcessEntitiesToLoad();
+    this->worlds[i]->ProcessEntitiesToDelete();
+  }
+
 }
 
 
////////////////////////////////////////////////////////////////////////////////

Modified: code/gazebo/branches/dev/server/Simulator.hh
===================================================================
--- code/gazebo/branches/dev/server/Simulator.hh        2010-11-15 03:06:34 UTC 
(rev 8968)
+++ code/gazebo/branches/dev/server/Simulator.hh        2010-11-15 15:52:22 UTC 
(rev 8969)
@@ -97,6 +97,15 @@
       /// \brief Get a world by number
       public: World *GetWorld(unsigned int i) const;
 
+      /// \brief Set the active world
+      public: void SetActiveWorld(unsigned int i);
+
+      /// \brief Set the active world
+      public: void SetActiveWorld(World *world);
+
+      /// \brief Get the currently active world
+      public: World *GetActiveWorld() const;
+
       /// \brief Main simulation loop, when this loop ends the simulation 
finish
       public: void Run();
   
@@ -186,6 +195,7 @@
   
       private: static std::string defaultWorldXML;
       private: std::vector< World* > worlds;
+      private: unsigned int activeWorldIndex;
   
       private: std::vector<Plugin*> plugins;
   

Modified: code/gazebo/branches/dev/server/World.cc
===================================================================
--- code/gazebo/branches/dev/server/World.cc    2010-11-15 03:06:34 UTC (rev 
8968)
+++ code/gazebo/branches/dev/server/World.cc    2010-11-15 15:52:22 UTC (rev 
8969)
@@ -49,8 +49,10 @@
 #include "Simulator.hh"
 #include "gz.h"
 #include "World.hh"
-#include "Logger.hh"
 
+// NATY: put back in
+//#include "Logger.hh"
+
 #include "OpenAL.hh"
 
 #include "Geom.hh"
@@ -116,12 +118,10 @@
 // Load the world
 void World::Load(XMLConfigNode *rootNode)//, unsigned int serverId)
 {
-
   this->nameP->Load(rootNode);
   this->saveStateTimeoutP->Load(rootNode);
   this->saveStateBufferSizeP->Load(rootNode);
 
-
   // Create the server object (needs to be done before models initialize)
   if (this->server == NULL)
   {
@@ -162,15 +162,15 @@
   }
 
   // Load OpenAL audio 
-  if (rootNode && rootNode->GetChild("openal","audio"))
+  if (rootNode && rootNode->GetChild("audio"))
   {
     this->openAL = OpenAL::Instance();
-    this->openAL->Load(rootNode->GetChild("openal", "audio"));
+    this->openAL->Load(rootNode->GetChild("audio"));
   }
 
   XMLConfigNode *physicsNode = NULL;
   if (rootNode )
-    physicsNode = rootNode->GetChildByNSPrefix("physics");
+    physicsNode = rootNode->GetChild("physics");
 
   if (Simulator::Instance()->GetPhysicsEnabled() && physicsNode)
   {
@@ -179,8 +179,9 @@
       delete this->physicsEngine;
       this->physicsEngine = NULL;
     }
+    std::string type = physicsNode->GetString("type","ode",1);
 
-    this->physicsEngine = PhysicsFactory::NewPhysicsEngine( 
physicsNode->GetName(), this);
+    this->physicsEngine = PhysicsFactory::NewPhysicsEngine( type, this);
     if (this->physicsEngine == NULL)
       gzthrow("Unable to create physics engine\n");
   }
@@ -188,7 +189,7 @@
     this->physicsEngine = PhysicsFactory::NewPhysicsEngine("ode", this);
 
   // This should come before loading of entities
-  this->physicsEngine->Load(rootNode);
+  this->physicsEngine->Load(physicsNode);
   
   // last bool is initModel, init model is not needed as Init()
   // is called separately from main.cc
@@ -393,7 +394,8 @@
   // Process all incoming messages from simiface
   this->simIfaceHandler->Update();
 
-  Logger::Instance()->Update();
+  // NATY: put back in
+  //Logger::Instance()->Update();
 
   Events::worldUpdateEndSignal();
 }
@@ -516,10 +518,10 @@
   XMLConfigNode *cnode;
   Model *model = NULL;
 
-  if (node && node->GetNSPrefix() != "")
+  if (node)
   {
     // Check for model nodes
-    if (node->GetNSPrefix() == "model")
+    if (node->GetName() == "model")
     {
       model = this->LoadModel(node, parent, removeDuplicate, initModel);
       Events::addEntitySignal(model->GetCompleteScopedName());
@@ -642,6 +644,7 @@
 {
   Pose3d pose;
   Model *model = new Model(parent);
+  model->SetWorld(this);
 
   // Load the model
   model->Load( node, removeDuplicate );

Modified: code/gazebo/branches/dev/server/World.hh
===================================================================
--- code/gazebo/branches/dev/server/World.hh    2010-11-15 03:06:34 UTC (rev 
8968)
+++ code/gazebo/branches/dev/server/World.hh    2010-11-15 15:52:22 UTC (rev 
8969)
@@ -74,10 +74,10 @@
 class World 
 {
   /// Private constructor
-  private: World();
+  public: World();
 
   /// Private destructor
-  private: ~World();
+  public: ~World();
 
   /// Load the world
   /// \param node XMLConfig node point

Modified: code/gazebo/branches/dev/server/controllers/Controller.cc
===================================================================
--- code/gazebo/branches/dev/server/controllers/Controller.cc   2010-11-15 
03:06:34 UTC (rev 8968)
+++ code/gazebo/branches/dev/server/controllers/Controller.cc   2010-11-15 
15:52:22 UTC (rev 8969)
@@ -44,6 +44,7 @@
 Controller::Controller(Entity *entity )
 {
   Param::Begin(&this->parameters);
+  this->typeP = new ParamT<std::string>("type","",1);
   this->nameP = new ParamT<std::string>("name","",1);
   this->alwaysOnP = new ParamT<bool>("alwaysOn", false, 0);
   this->updatePeriodP = new ParamT<double>("updateRate", 10, 0);
@@ -61,6 +62,7 @@
 /// Destructor
 Controller::~Controller()
 {
+  delete this->typeP;
   delete this->nameP;
   delete this->alwaysOnP;
   delete this->updatePeriodP;
@@ -75,22 +77,16 @@
   if (!this->parent)
     gzthrow("Parent entity has not been set");
 
-  this->typeName = node->GetName();
-
+  this->typeP->Load(node);
   this->nameP->Load(node);
 
   this->alwaysOnP->Load(node);
 
   this->updatePeriodP->Load(node);
 
-  double updateRate  = this->updatePeriodP->GetValue();
-  if (updateRate == 0)
-    this->updatePeriod = 0.0; // no throttling if updateRate is 0
-  else
-    this->updatePeriod = 1.0 / updateRate;
   this->lastUpdate = this->parent->GetWorld()->GetSimTime();
 
-  childNode = node->GetChildByNSPrefix("interface");
+  childNode = node->GetChild("interface");
   
   // Create the interfaces
   while (childNode)
@@ -98,7 +94,7 @@
     libgazebo::Iface *iface=0;
 
     // Get the type of the interface (eg: laser)
-    std::string ifaceType = childNode->GetName();
+    std::string ifaceType = childNode->GetString("type","",1);
 
     // Get the name of the iface
     std::string ifaceName = childNode->GetString("name","",1);
@@ -121,7 +117,7 @@
     catch (...) //TODO: Show the exception text here (subclass exception?)
     {
       gzmsg(1) << "No libgazebo Iface for the interface[" << ifaceType << "] 
found. Disabled.\n";
-      childNode = childNode->GetNextByNSPrefix("interface");
+      childNode = childNode->GetNext("interface");
       continue;
     }
     
@@ -137,7 +133,7 @@
     
     this->ifaces.push_back(iface);
 
-    childNode = childNode->GetNextByNSPrefix("interface");
+    childNode = childNode->GetNext("interface");
   }
 
   this->LoadChild(node);
@@ -157,21 +153,21 @@
 {
   std::vector<libgazebo::Iface*>::iterator iter;
 
-  stream << prefix << "<controller:" << this->typeName << " name=\"" << 
this->nameP->GetValue() << "\">\n";
+  stream << prefix << "<controller type=\"" << **this->typeP << "\" name=\"" 
<< **this->nameP << "\">\n";
 
   stream << prefix << "  " << *(this->updatePeriodP) << "\n";
 
   // Ouptut the interfaces
   for (iter = this->ifaces.begin(); iter != this->ifaces.end(); iter++)
   {
-    stream << prefix << "  <interface:" << (*iter)->GetType() << " name=\"" << 
(*iter)->GetId() << "\"/>\n";
+    stream << prefix << "  <interface type=\"" << (*iter)->GetType() << "\" 
name=\"" << (*iter)->GetId() << "\"/>\n";
   }
 
   std::string p = prefix + "  ";
 
   this->SaveChild(p, stream);
 
-  stream << prefix << "</controller:" << this->typeName << ">\n";
+  stream << prefix << "</controller>\n";
 }
 
 
////////////////////////////////////////////////////////////////////////////////
@@ -195,7 +191,7 @@
     //timer.Start();
 
     Time simTime = this->parent->GetWorld()->GetSimTime();
-    if ((simTime-lastUpdate-updatePeriod)/physics_dt >= 0)
+    if ((simTime-lastUpdate - **this->updatePeriodP)/physics_dt >= 0)
     {
       this->UpdateChild();
       lastUpdate = this->parent->GetWorld()->GetSimTime();

Modified: code/gazebo/branches/dev/server/controllers/Controller.hh
===================================================================
--- code/gazebo/branches/dev/server/controllers/Controller.hh   2010-11-15 
03:06:34 UTC (rev 8968)
+++ code/gazebo/branches/dev/server/controllers/Controller.hh   2010-11-15 
15:52:22 UTC (rev 8969)
@@ -111,7 +111,10 @@
   /// \param number If several ifaces of the same type present, which one
   /// \return Iface, or exception if not found. 
   protected: libgazebo::Iface* GetIface(std::string type, bool mandatory=true, 
int number=0);
-  
+ 
+  /// The type of the controller
+  protected: ParamT<std::string> *typeP;
+
   /// \brief The controller's name
   protected: ParamT<std::string> *nameP;
 
@@ -122,11 +125,8 @@
   protected: ParamT<bool> *alwaysOnP;
 
   /// \brief Update period 
-  protected: double updatePeriod;
   protected: ParamT<double> *updatePeriodP;
 
-  private: std::string typeName;
-
   /// \brief Last update time
   protected: Time lastUpdate;
 

Modified: code/gazebo/branches/dev/server/controllers/audio/Audio.cc
===================================================================
--- code/gazebo/branches/dev/server/controllers/audio/Audio.cc  2010-11-15 
03:06:34 UTC (rev 8968)
+++ code/gazebo/branches/dev/server/controllers/audio/Audio.cc  2010-11-15 
15:52:22 UTC (rev 8969)
@@ -25,6 +25,7 @@
  */
 
 #include "Global.hh"
+#include "World.hh"
 #include "XMLConfig.hh"
 #include "Model.hh"
 #include "Simulator.hh"

Modified: code/gazebo/branches/dev/server/main.cc
===================================================================
--- code/gazebo/branches/dev/server/main.cc     2010-11-15 03:06:34 UTC (rev 
8968)
+++ code/gazebo/branches/dev/server/main.cc     2010-11-15 15:52:22 UTC (rev 
8969)
@@ -255,7 +255,8 @@
   try
   {
     gazebo::Simulator::Instance()->Load(worldFileName);
-    gazebo::Simulator::Instance()->SetTimeout(optTimeout);
+    // NATY: Put back in
+    //gazebo::Simulator::Instance()->SetTimeout(optTimeout);
     gazebo::Simulator::Instance()->SetPhysicsEnabled(optPhysicsEnabled);
 
     gazebo::Simulator::Instance()->CreateWorld(worldFileName);
@@ -271,7 +272,8 @@
   // Initialize the simulator
   try
   {
-    gazebo::Simulator::Instance()->SetPaused(optPaused);
+    // NATY: put back in
+    //gazebo::Simulator::Instance()->SetPaused(optPaused);
     gazebo::Simulator::Instance()->Init();
   }
   catch (gazebo::GazeboError e)

Modified: code/gazebo/branches/dev/server/physics/Body.cc
===================================================================
--- code/gazebo/branches/dev/server/physics/Body.cc     2010-11-15 03:06:34 UTC 
(rev 8968)
+++ code/gazebo/branches/dev/server/physics/Body.cc     2010-11-15 15:52:22 UTC 
(rev 8969)
@@ -199,24 +199,24 @@
   //   this body's pose - this body's offsetFromModelFrame
   this->initModelOffset = this->GetRelativePose().CoordPoseSolve(Pose3d());
 
-  childNode = node->GetChildByNSPrefix("geom");
+  childNode = node->GetChild("geom");
 
   // Load the geometries
   while (childNode)
   {
     // Create and Load a geom, which will belong to this body.
     this->LoadGeom(childNode);
-    childNode = childNode->GetNextByNSPrefix("geom");
+    childNode = childNode->GetNext("geom");
   }
 
-  childNode = node->GetChildByNSPrefix("sensor");
+  childNode = node->GetChild("sensor");
 
   // Load the sensors
   while (childNode)
   {
     // Create and Load a sensor, which will belong to this body.
     this->LoadSensor(childNode);
-    childNode = childNode->GetNextByNSPrefix("sensor");
+    childNode = childNode->GetNext("sensor");
   }
 
   this->SetKinematic(**this->kinematicP);
@@ -494,11 +494,12 @@
 void Body::LoadGeom(XMLConfigNode *node)
 {
   Geom *geom = NULL;
+  std::string type = node->GetString("type","",1);
 
-  if (node->GetName() == "heightmap" || node->GetName() == "map")
+  if (type == "heightmap" || type == "map")
     this->SetStatic(true);
 
-  geom = this->GetWorld()->GetPhysicsEngine()->CreateGeom(node->GetName(), 
this);
+  geom = this->GetWorld()->GetPhysicsEngine()->CreateGeom(type, this);
 
   if (!geom)
     gzthrow("Unknown Geometry Type["+
@@ -518,8 +519,10 @@
     gzthrow("Null node pointer. Invalid sensor in the world file.");
   }
 
-  sensor = SensorFactory::NewSensor(node->GetName(), this);
+  std::string type = node->GetString("type","",1);
 
+  sensor = SensorFactory::NewSensor(type, this);
+
   if (sensor)
   {
     sensor->Load(node);

Modified: code/gazebo/branches/dev/server/physics/Geom.cc
===================================================================
--- code/gazebo/branches/dev/server/physics/Geom.cc     2010-11-15 03:06:34 UTC 
(rev 8968)
+++ code/gazebo/branches/dev/server/physics/Geom.cc     2010-11-15 15:52:22 UTC 
(rev 8969)
@@ -51,8 +51,6 @@
 {
   this->AddType(GEOM);
 
-  this->typeName = "unknown";
-
   this->body = body;
 
   // Create the contact parameters
@@ -67,6 +65,8 @@
   this->contactsEnabled = false;
 
   Param::Begin(&this->parameters);
+  this->typeP = new ParamT<std::string>("type","unknown",1);
+
   this->massP = new ParamT<double>("mass",0.001,0);
   this->massP->Callback( &Geom::SetMass, this);
 
@@ -111,6 +111,7 @@
     this->bbVisual = NULL;
   }
 
+  delete this->typeP;
   delete this->massP;
   delete this->xyzP;
   delete this->rpyP;
@@ -144,10 +145,7 @@
 {
   XMLConfigNode *childNode = NULL;
 
-  this->xmlNode=node;
-
-  this->typeName = node->GetName();
-
+  this->typeP->Load(node);
   this->nameP->Load(node);
   this->SetName(this->nameP->GetValue());
   this->massP->Load(node);
@@ -236,7 +234,7 @@
   this->xyzP->SetValue( this->GetRelativePose().pos );
   this->rpyP->SetValue( this->GetRelativePose().rot );
 
-  stream << prefix << "<geom:" << this->typeName << " name=\"" 
+  stream << prefix << "<geom:" << **this->typeP << " name=\"" 
          << this->nameP->GetValue() << "\">\n";
 
   stream << prefix << "  " << *(this->xyzP) << "\n";
@@ -255,7 +253,7 @@
       (*iter)->Save(p, stream);
   }
 
-  stream << prefix << "</geom:" << this->typeName << ">\n";
+  stream << prefix << "</geom>\n";
 }
 
 
////////////////////////////////////////////////////////////////////////////////

Modified: code/gazebo/branches/dev/server/physics/Geom.hh
===================================================================
--- code/gazebo/branches/dev/server/physics/Geom.hh     2010-11-15 03:06:34 UTC 
(rev 8968)
+++ code/gazebo/branches/dev/server/physics/Geom.hh     2010-11-15 15:52:22 UTC 
(rev 8969)
@@ -218,6 +218,7 @@
 
     protected: Mass mass;
 
+    private: ParamT<std::string> *typeP;
     private: ParamT<int> *laserFiducialIdP;
     private: ParamT<float> *laserRetroP;
 
@@ -235,11 +236,6 @@
     /// All the visual apparence 
     private: std::vector<OgreVisual*> visuals;
 
-    ///our XML DATA
-    private: XMLConfigNode *xmlNode;
-
-    private: std::string typeName;
-
     protected: Shape *shape;
 
     private: bool contactsEnabled;

Modified: code/gazebo/branches/dev/server/physics/ode/ODEPhysics.cc
===================================================================
--- code/gazebo/branches/dev/server/physics/ode/ODEPhysics.cc   2010-11-15 
03:06:34 UTC (rev 8968)
+++ code/gazebo/branches/dev/server/physics/ode/ODEPhysics.cc   2010-11-15 
15:52:22 UTC (rev 8969)
@@ -202,50 +202,45 @@
 // Load the ODE engine
 void ODEPhysics::Load(XMLConfigNode *node)
 {
-  XMLConfigNode *cnode = NULL;
+  this->gravityP->Load(node);
+  this->stepTimeP->Load(node);
+  this->updateRateP->Load(node);
+  this->globalCFMP->Load(node);
+  this->globalERPP->Load(node);
+  this->stepTypeP->Load(node);
+  this->stepItersP->Load(node);
+  this->stepWP->Load(node);
+  this->contactMaxCorrectingVelP->Load(node);
+  this->contactSurfaceLayerP->Load(node);
+  this->autoDisableBodyP->Load(node);
+  this->contactFeedbacksP->Load(node);
+  this->maxContactsP->Load(node);
 
-  if (node)
-    cnode = node->GetChild("ode", "physics");
-
-  this->gravityP->Load(cnode);
-  this->stepTimeP->Load(cnode);
-  this->updateRateP->Load(cnode);
-  this->globalCFMP->Load(cnode);
-  this->globalERPP->Load(cnode);
-  this->stepTypeP->Load(cnode);
-  this->stepItersP->Load(cnode);
-  this->stepWP->Load(cnode);
-  this->contactMaxCorrectingVelP->Load(cnode);
-  this->contactSurfaceLayerP->Load(cnode);
-  this->autoDisableBodyP->Load(cnode);
-  this->contactFeedbacksP->Load(cnode);
-  this->maxContactsP->Load(cnode);
-
   // Help prevent "popping of deeply embedded object
-  dWorldSetContactMaxCorrectingVel(this->worldId, 
contactMaxCorrectingVelP->GetValue());
+  dWorldSetContactMaxCorrectingVel(this->worldId, **contactMaxCorrectingVelP);
 
   // This helps prevent jittering problems.
-  dWorldSetContactSurfaceLayer(this->worldId, 
contactSurfaceLayerP->GetValue());
+  dWorldSetContactSurfaceLayer(this->worldId, **contactSurfaceLayerP);
 
   // If auto-disable is active, then user interaction with the joints 
   // doesn't behave properly
   // disable autodisable by default
-  dWorldSetAutoDisableFlag(this->worldId, this->autoDisableBodyP->GetValue());
+  dWorldSetAutoDisableFlag(this->worldId, **this->autoDisableBodyP);
   dWorldSetAutoDisableTime(this->worldId, 2.0);
   dWorldSetAutoDisableLinearThreshold(this->worldId, 0.001);
   dWorldSetAutoDisableAngularThreshold(this->worldId, 0.001);
   dWorldSetAutoDisableSteps(this->worldId, 50);
 
-  this->contactFeedbacks.resize(this->contactFeedbacksP->GetValue());
+  this->contactFeedbacks.resize(**this->contactFeedbacksP);
 
   // Reset the contact pointer
   //this->contactFeedbackIter = this->contactFeedbacks.begin();
 
-  Vector3 g = this->gravityP->GetValue();
+  Vector3 g = **this->gravityP;
   dWorldSetGravity(this->worldId, g.x, g.y, g.z);
 
-  dWorldSetCFM(this->worldId, this->globalCFMP->GetValue());
-  dWorldSetERP(this->worldId, this->globalERPP->GetValue());
+  dWorldSetCFM(this->worldId, **this->globalCFMP);
+  dWorldSetERP(this->worldId, **this->globalERPP);
 
   dWorldSetQuickStepNumIterations(this->worldId, **this->stepItersP );
   dWorldSetQuickStepW(this->worldId, **this->stepWP );
@@ -255,7 +250,7 @@
 // Save the ODE engine
 void ODEPhysics::Save(std::string &prefix, std::ostream &stream)
 {
-  stream << prefix << "<physics:ode>\n";
+  stream << prefix << "<physics type=\"ode\">\n";
   stream << prefix << "  " << *(this->stepTimeP) << "\n";
   stream << prefix << "  " << *(this->gravityP) << "\n";
   stream << prefix << "  " << *(this->updateRateP) << "\n";
@@ -266,7 +261,7 @@
   stream << prefix << "  " << *(this->stepWP) << "\n";
   stream << prefix << "  " << *(this->contactMaxCorrectingVelP) << "\n";
   stream << prefix << "  " << *(this->contactSurfaceLayerP) << "\n";
-  stream << prefix << "</physics:ode>\n";
+  stream << prefix << "</physics>\n";
 }
 
 
////////////////////////////////////////////////////////////////////////////////

Modified: code/gazebo/branches/dev/server/rendering/OgreAdaptor.cc
===================================================================
--- code/gazebo/branches/dev/server/rendering/OgreAdaptor.cc    2010-11-15 
03:06:34 UTC (rev 8968)
+++ code/gazebo/branches/dev/server/rendering/OgreAdaptor.cc    2010-11-15 
15:52:22 UTC (rev 8969)
@@ -84,13 +84,6 @@
 }
 
 
////////////////////////////////////////////////////////////////////////////////
-// Closes and free
-void OgreAdaptor::Close()
-{
-  this->Fini();
-}
-
-////////////////////////////////////////////////////////////////////////////////
 /// Load the parameters for Ogre
 void OgreAdaptor::Load(XMLConfigNode *rootNode)
 {
@@ -120,7 +113,7 @@
   this->SetupResources();
 
   Scene *scene = new Scene("primary_scene");
-  scene->Load(rootNode->GetChild("ogre", "rendering"));
+  scene->Load(rootNode->GetChild("rendering"));
   scene->CreateGrid( 10, 1, 0.03, Color(1,1,1,1));
   this->scenes.push_back( scene );  
 
@@ -226,7 +219,6 @@
 
   if (this->HasGLSL())
     RTShaderSystem::Instance()->UpdateShaders();
-
 }
 
 

Modified: code/gazebo/branches/dev/server/rendering/OgreAdaptor.hh
===================================================================
--- code/gazebo/branches/dev/server/rendering/OgreAdaptor.hh    2010-11-15 
03:06:34 UTC (rev 8968)
+++ code/gazebo/branches/dev/server/rendering/OgreAdaptor.hh    2010-11-15 
15:52:22 UTC (rev 8969)
@@ -83,9 +83,6 @@
     /// \brief Destructor
     private: virtual ~OgreAdaptor();
   
-    /// \brief Closes the present simulation, frees the resources 
-    public: void Close();
-
     /// \brief Load the parameters for Ogre
     public: void Load(XMLConfigNode *rootNode);
 

Modified: code/gazebo/branches/dev/server/sensors/Sensor.cc
===================================================================
--- code/gazebo/branches/dev/server/sensors/Sensor.cc   2010-11-15 03:06:34 UTC 
(rev 8968)
+++ code/gazebo/branches/dev/server/sensors/Sensor.cc   2010-11-15 15:52:22 UTC 
(rev 8969)
@@ -77,7 +77,7 @@
   else
     this->updatePeriod = 1.0 / **(updateRateP);
 
-  this->LoadController( node->GetChildByNSPrefix("controller") );
+  this->LoadController( node->GetChild("controller") );
   this->LoadChild(node);
 
   double updateRate  = node->GetDouble("updateRate", 0, 0);
@@ -86,7 +86,6 @@
   else
     this->updatePeriod = 1.0 / updateRate;
   this->lastUpdate = this->GetWorld()->GetSimTime();
-
 }
 
 
////////////////////////////////////////////////////////////////////////////////

Modified: code/gazebo/branches/dev/server/wx/BoxMaker.cc
===================================================================
--- code/gazebo/branches/dev/server/wx/BoxMaker.cc      2010-11-15 03:06:34 UTC 
(rev 8968)
+++ code/gazebo/branches/dev/server/wx/BoxMaker.cc      2010-11-15 15:52:22 UTC 
(rev 8969)
@@ -1,5 +1,4 @@
 #include <iostream>
-#include <FL/Fl.H>
 
 #include "Events.hh"
 #include "MouseEvent.hh"
@@ -154,6 +153,6 @@
 
   OgreCreator::Instance()->DeleteVisual(this->visualName);
 
-  Simulator::Instance()->GetWorld(0)->InsertEntity(newModelStr.str());
+  Simulator::Instance()->GetActiveWorld()->InsertEntity(newModelStr.str());
 }
 

Modified: code/gazebo/branches/dev/server/wx/CylinderMaker.cc
===================================================================
--- code/gazebo/branches/dev/server/wx/CylinderMaker.cc 2010-11-15 03:06:34 UTC 
(rev 8968)
+++ code/gazebo/branches/dev/server/wx/CylinderMaker.cc 2010-11-15 15:52:22 UTC 
(rev 8969)
@@ -1,5 +1,4 @@
 #include <iostream>
-#include <FL/Fl.H>
 
 #include "Camera.hh"
 #include "Events.hh"
@@ -158,6 +157,6 @@
   newModelStr <<  "</gazebo:world>";
 
   OgreCreator::Instance()->DeleteVisual(this->visualName);
-  Simulator::Instance()->GetWorld(0)->InsertEntity(newModelStr.str());
+  Simulator::Instance()->GetActiveWorld()->InsertEntity(newModelStr.str());
 }
 

Modified: code/gazebo/branches/dev/server/wx/DirectionalLightMaker.cc
===================================================================
--- code/gazebo/branches/dev/server/wx/DirectionalLightMaker.cc 2010-11-15 
03:06:34 UTC (rev 8968)
+++ code/gazebo/branches/dev/server/wx/DirectionalLightMaker.cc 2010-11-15 
15:52:22 UTC (rev 8969)
@@ -1,5 +1,4 @@
 #include <iostream>
-#include <FL/Fl.H>
 
 #include "Camera.hh"
 #include "MouseEvent.hh"
@@ -96,5 +95,5 @@
 
   newModelStr <<  "</gazebo:world>";
 
-  Simulator::Instance()->GetWorld(0)->InsertEntity(newModelStr.str());
+  Simulator::Instance()->GetActiveWorld()->InsertEntity(newModelStr.str());
 }

Modified: code/gazebo/branches/dev/server/wx/PhysicsPage.cc
===================================================================
--- code/gazebo/branches/dev/server/wx/PhysicsPage.cc   2010-11-15 03:06:34 UTC 
(rev 8968)
+++ code/gazebo/branches/dev/server/wx/PhysicsPage.cc   2010-11-15 15:52:22 UTC 
(rev 8969)
@@ -16,7 +16,7 @@
 {
   this->SetName(wxT("physics"));
 
-  PhysicsEngine *engine = 
Simulator::Instance()->GetWorld(0)->GetPhysicsEngine();
+  PhysicsEngine *engine = 
Simulator::Instance()->GetActiveWorld()->GetPhysicsEngine();
 
   wxBoxSizer *boxSizer = new wxBoxSizer(wxVERTICAL);
 

Modified: code/gazebo/branches/dev/server/wx/PointLightMaker.cc
===================================================================
--- code/gazebo/branches/dev/server/wx/PointLightMaker.cc       2010-11-15 
03:06:34 UTC (rev 8968)
+++ code/gazebo/branches/dev/server/wx/PointLightMaker.cc       2010-11-15 
15:52:22 UTC (rev 8969)
@@ -1,5 +1,4 @@
 #include <iostream>
-#include <FL/Fl.H>
 
 #include "Camera.hh"
 #include "MouseEvent.hh"
@@ -95,5 +94,5 @@
 
   newModelStr <<  "</gazebo:world>";
 
-  Simulator::Instance()->GetWorld(0)->InsertEntity(newModelStr.str());
+  Simulator::Instance()->GetActiveWorld()->InsertEntity(newModelStr.str());
 }

Modified: code/gazebo/branches/dev/server/wx/RenderControl.cc
===================================================================
--- code/gazebo/branches/dev/server/wx/RenderControl.cc 2010-11-15 03:06:34 UTC 
(rev 8968)
+++ code/gazebo/branches/dev/server/wx/RenderControl.cc 2010-11-15 15:52:22 UTC 
(rev 8969)
@@ -177,7 +177,7 @@
   }
   else if (this->GetCursorState() == "manip")
   {
-    Entity *entity = Simulator::Instance()->GetWorld(0)->GetSelectedEntity();
+    Entity *entity = 
Simulator::Instance()->GetActiveWorld()->GetSelectedEntity();
 
     if (entity)
     {

Modified: code/gazebo/branches/dev/server/wx/SimulationApp.cc
===================================================================
--- code/gazebo/branches/dev/server/wx/SimulationApp.cc 2010-11-15 03:06:34 UTC 
(rev 8968)
+++ code/gazebo/branches/dev/server/wx/SimulationApp.cc 2010-11-15 15:52:22 UTC 
(rev 8969)
@@ -55,15 +55,10 @@
 
 void SimulationApp::OnIdle(wxTimerEvent &evt)
 {
+  this->frame->Update();
   Simulator::Instance()->GraphicsUpdate();
 }
 
-////////////////////////////////////////////////////////////////////////////////
-/// Initalize the gui
-void SimulationApp::Update()
-{
-  this->frame->Update();
-}
 
 
////////////////////////////////////////////////////////////////////////////////
 /// Save the gui params in xml format

Modified: code/gazebo/branches/dev/server/wx/SimulationApp.hh
===================================================================
--- code/gazebo/branches/dev/server/wx/SimulationApp.hh 2010-11-15 03:06:34 UTC 
(rev 8968)
+++ code/gazebo/branches/dev/server/wx/SimulationApp.hh 2010-11-15 15:52:22 UTC 
(rev 8969)
@@ -17,9 +17,6 @@
 
     public: bool OnInit();
 
-    /// \brief Update the gui
-    public: void Update();
-
     /// \brief Run the gui
     public: void Run();
 

Modified: code/gazebo/branches/dev/server/wx/SimulationFrame.cc
===================================================================
--- code/gazebo/branches/dev/server/wx/SimulationFrame.cc       2010-11-15 
03:06:34 UTC (rev 8968)
+++ code/gazebo/branches/dev/server/wx/SimulationFrame.cc       2010-11-15 
15:52:22 UTC (rev 8969)
@@ -121,7 +121,6 @@
 
   if (this->toolbar)
     this->auiManager->AddPane(this->toolbar, 
wxAuiPaneInfo().ToolbarPane().RightDockable(false).LeftDockable(false).MinSize(100,30).Top().Name(wxT("Tools")).Caption(wxT("Tools")));
-  this->auiManager->Update();
 
   Connect(wxEVT_AUI_PANE_CLOSE, 
wxAuiManagerEventHandler(SimulationFrame::OnPaneClosed), NULL, this);
 
@@ -162,7 +161,8 @@
 void SimulationFrame::Init()
 {
   this->renderPanel->Init();
-  this->OnPause(Simulator::Instance()->GetWorld(0)->IsPaused());
+  // NATY: put back in
+  //this->OnPause(Simulator::Instance()->GetActiveWorld()->IsPaused());
 
   EntityTreeItemData *data;
 
@@ -267,6 +267,7 @@
 // Update the frame
 void SimulationFrame::Update()
 {
+  this->auiManager->Update();
   this->timePanel->Update();
   this->renderPanel->Update();
 }
@@ -348,7 +349,7 @@
   // NATY
   // stop simulation when this is happening
   //boost::recursive_mutex::scoped_lock 
lock(*Simulator::Instance()->GetMRMutex());
-  Simulator::Instance()->GetWorld(0)->Reset();
+  Simulator::Instance()->GetActiveWorld()->Reset();
 }
 
 
////////////////////////////////////////////////////////////////////////////////
@@ -433,20 +434,20 @@
   {
     this->toolbar->ToggleTool(PAUSE, false);
     this->toolbar->ToggleTool(STEP, false);
-    Simulator::Instance()->GetWorld(0)->SetPaused(false);
+    Simulator::Instance()->GetActiveWorld()->SetPaused(false);
   }
   else if (id == PAUSE)
   {
     this->toolbar->ToggleTool(PLAY, false);
     this->toolbar->ToggleTool(STEP, false);
-    Simulator::Instance()->GetWorld(0)->SetPaused(true);
+    Simulator::Instance()->GetActiveWorld()->SetPaused(true);
   }
   else if (id == STEP)
   {
     this->toolbar->ToggleTool(PLAY, false);
     this->toolbar->ToggleTool(STEP, false);
     // NATY : put back in 
-    //Simulator::Instance()->GetWorld(0)->SetStepInc( true );
+    //Simulator::Instance()->GetActiveWorld()->SetStepInc( true );
   }
   else if (id == BOX)
   {
@@ -508,7 +509,7 @@
     Common *common = NULL;
 
     if (data->name == "World")
-      paramCount = Simulator::Instance()->GetWorld(0)->GetParamCount();
+      paramCount = Simulator::Instance()->GetActiveWorld()->GetParamCount();
     else 
     {
       common = Common::GetByName( data->name );
@@ -520,7 +521,7 @@
       Param *param = NULL;
 
       if (data->name == "World")
-        param = Simulator::Instance()->GetWorld(0)->GetParam(i);
+        param = Simulator::Instance()->GetActiveWorld()->GetParam(i);
       else
         param = common->GetParam(i);
 

Modified: code/gazebo/branches/dev/server/wx/SphereMaker.cc
===================================================================
--- code/gazebo/branches/dev/server/wx/SphereMaker.cc   2010-11-15 03:06:34 UTC 
(rev 8968)
+++ code/gazebo/branches/dev/server/wx/SphereMaker.cc   2010-11-15 15:52:22 UTC 
(rev 8969)
@@ -1,5 +1,4 @@
 #include <iostream>
-#include <FL/Fl.H>
 
 #include "Camera.hh"
 #include "Events.hh"
@@ -141,7 +140,7 @@
 
   newModelStr <<  "</gazebo:world>";
 
-  Simulator::Instance()->GetWorld(0)->InsertEntity(newModelStr.str());
+  Simulator::Instance()->GetActiveWorld()->InsertEntity(newModelStr.str());
 
   OgreCreator::Instance()->DeleteVisual(this->visualName);
 }

Modified: code/gazebo/branches/dev/server/wx/SpotLightMaker.cc
===================================================================
--- code/gazebo/branches/dev/server/wx/SpotLightMaker.cc        2010-11-15 
03:06:34 UTC (rev 8968)
+++ code/gazebo/branches/dev/server/wx/SpotLightMaker.cc        2010-11-15 
15:52:22 UTC (rev 8969)
@@ -1,5 +1,4 @@
 #include <iostream>
-#include <FL/Fl.H>
 
 #include "Camera.hh"
 #include "MouseEvent.hh"
@@ -99,5 +98,5 @@
   newModelStr <<  "</gazebo:world>";
 
   std::cout << "Make spot light[" << newModelStr << "]\n"; 
-  Simulator::Instance()->GetWorld(0)->InsertEntity(newModelStr.str());
+  Simulator::Instance()->GetActiveWorld()->InsertEntity(newModelStr.str());
 }

Modified: code/gazebo/branches/dev/server/wx/TimePanel.cc
===================================================================
--- code/gazebo/branches/dev/server/wx/TimePanel.cc     2010-11-15 03:06:34 UTC 
(rev 8968)
+++ code/gazebo/branches/dev/server/wx/TimePanel.cc     2010-11-15 15:52:22 UTC 
(rev 8969)
@@ -76,10 +76,12 @@
 // Update the time panel
 void TimePanel::Update()
 {
-  if (Simulator::Instance()->GetWorld(0)->GetRealTime() - this->lastUpdateTime 
> this->statusUpdatePeriod)
+  World *world = Simulator::Instance()->GetActiveWorld();
+
+  if (world->GetRealTime() - this->lastUpdateTime > this->statusUpdatePeriod)
   {
-    Time simTime = Simulator::Instance()->GetWorld(0)->GetSimTime();
-    Time realTime = Simulator::Instance()->GetWorld(0)->GetRealTime();
+    Time simTime = Simulator::Instance()->GetActiveWorld()->GetSimTime();
+    Time realTime = Simulator::Instance()->GetActiveWorld()->GetRealTime();
     Time percent;
     
     if (realTime < this->statusUpdatePeriod )
@@ -132,10 +134,10 @@
     this->simTimeCtrl->SetValue(simSuffix);
     this->realTimeCtrl->SetValue(realSuffix);
 
-    str.Printf(wxT("%f sec"), 
Simulator::Instance()->GetWorld(0)->GetPauseTime().Double());
+    str.Printf(wxT("%f sec"), 
Simulator::Instance()->GetActiveWorld()->GetPauseTime().Double());
     this->pauseTimeCtrl->SetValue(str);
 
-    this->lastUpdateTime = Simulator::Instance()->GetWorld(0)->GetRealTime();
+    this->lastUpdateTime = 
Simulator::Instance()->GetActiveWorld()->GetRealTime();
   }
 
 }

Modified: code/gazebo/branches/dev/worlds/empty.world
===================================================================
--- code/gazebo/branches/dev/worlds/empty.world 2010-11-15 03:06:34 UTC (rev 
8968)
+++ code/gazebo/branches/dev/worlds/empty.world 2010-11-15 15:52:22 UTC (rev 
8969)
@@ -1,76 +1,71 @@
 <?xml version="1.0"?> 
-<gazebo:world 
-  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: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: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"; 
>
+<gazebo>
+  <config>
+    <verbosity>4</verbosity>
 
-  <verbosity>4</verbosity>
+    <gui>
+      <type>fltk</type>
+      <size>800 600</size>
+      <pos>0 0</pos>
+    </gui>
 
-  <physics:ode>
-    <stepTime>0.001</stepTime>
-    <gravity>0 0 -9.8</gravity>
-    <cfm>0.0000000001</cfm>
-    <erp>0.2</erp>
+    <rendering>
+      <ambient>.2 .2 .2 1</ambient>
+      <shadows>true</shadows>
+      <grid>false</grid>
+    </rendering>
 
-    <stepType>quick</stepType>
-    <stepIters>10</stepIters>
-    <stepW>1.3</stepW>
-    <contactMaxCorrectingVel>100.0</contactMaxCorrectingVel>
-    <contactSurfaceLayer>0.0</contactSurfaceLayer>
-  </physics:ode>
+  </config>
 
-  <rendering:gui>
-    <type>fltk</type>
-    <size>800 600</size>
-    <pos>0 0</pos>
-  </rendering:gui>
+  <world name="default">
 
-  <rendering:ogre>
-    <ambient>.2 .2 .2 1</ambient>
-    <shadows>true</shadows>
-    <grid>false</grid>
-  </rendering:ogre>
+    <physics type="ode">
+      <stepTime>0.001</stepTime>
+      <gravity>0 0 -9.8</gravity>
+      <cfm>0.0000000001</cfm>
+      <erp>0.2</erp>
+  
+      <stepType>quick</stepType>
+      <stepIters>10</stepIters>
+      <stepW>1.3</stepW>
+      <contactMaxCorrectingVel>100.0</contactMaxCorrectingVel>
+      <contactSurfaceLayer>0.0</contactSurfaceLayer>
+    </physics>
+  
+    <!-- Ground Plane -->
+    <model type="physical" name="plane1_model">
+      <xyz>0 0 0</xyz>
+      <rpy>0 0 0</rpy>
+      <static>true</static>
+  
+      <body name="plane1_body">
+        <geom type="plane" name="plane1_geom">
+          <normal>0 0 1</normal>
+          <size>100 100</size>
+          <segments>1  1</segments>
+          <uvTile>100 100</uvTile>
+          <mu1>109999.0</mu1>
+          <mu2>1000.0</mu2>
+          <material>Gazebo/GrayGrid</material>
+        </geom>
+      </body>
+    </model>
+   
+    <!-- White Point light -->
+    <model type="renderable" name="point_white">
+      <xyz>0.0 0 10</xyz>
+      <rpy>0 0 0</rpy>
+      <static>true</static>
+      <light>
+        <type>point</type>
+        <diffuseColor>0.6 0.6 0.6 1.0</diffuseColor>
+        <specularColor>.1 .1 .1 1.0</specularColor>
+        <attenuation>.2 0.1 0.0</attenuation>
+        <range>20</range>
+        <direction>0 0 -1.0</direction>
+        <castShadows>true</castShadows>
+      </light>
+    </model>
+  </world>
 
-  <!-- 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>100 100</size>
-        <segments>1  1</segments>
-        <uvTile>100 100</uvTile>
-        <mu1>109999.0</mu1>
-        <mu2>1000.0</mu2>
-        <material>Gazebo/GrayGrid</material>
-      </geom:plane>
-    </body:plane>
-  </model:physical>
- 
-  <!-- White Point light -->
-  <model:renderable name="point_white">
-    <xyz>0.0 0 10</xyz>
-    <rpy>0 0 0</rpy>
-    <static>true</static>
-    <light>
-      <type>point</type>
-      <diffuseColor>0.6 0.6 0.6 1.0</diffuseColor>
-      <specularColor>.1 .1 .1 1.0</specularColor>
-      <attenuation>.2 0.1 0.0</attenuation>
-      <range>20</range>
-      <direction>0 0 -1.0</direction>
-      <castShadows>true</castShadows>
-    </light>
-  </model:renderable>
-
-</gazebo:world>
+</gazebo>


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

------------------------------------------------------------------------------
Centralized Desktop Delivery: Dell and VMware Reference Architecture
Simplifying enterprise desktop deployment and management using
Dell EqualLogic storage and VMware View: A highly scalable, end-to-end
client virtualization framework. Read more!
http://p.sf.net/sfu/dell-eql-dev2dev
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit

Reply via email to