Physics CameraSensor lockstep

gazebo/Server.cc

@@ -100,6 +100,9 @@ namespace gazebo
 
     /// \brief Save argv for access by system plugins.
     char **systemPluginsArgv;
+
+    /// \brief Set whether to lockstep physics and rendering
+    bool lockstep = false;
   };
 }
 
@@ -149,6 +152,7 @@ bool Server::ParseArgs(int _argc, char **_argv)
     ("verbose", "Increase the messages written to the terminal.")
     ("help,h", "Produce this help message.")
     ("pause,u", "Start the server in a paused state.")
+    ("lockstep", "Lockstep simulation so sensor update rates are respected.")
     ("physics,e", po::value<std::string>(),
      "Specify a physics engine (ode|bullet|dart|simbody).")
     ("play,p", po::value<std::string>(), "Play a log file.")
@@ -282,6 +286,12 @@ bool Server::ParseArgs(int _argc, char **_argv)
     }
   }
 
+  if (this->dataPtr->vm.count("lockstep"))
+  {
+    this->dataPtr->lockstep = true;
+  }
+  rendering::set_lockstep_enabled(this->dataPtr->lockstep);
+
   if (!this->PreLoad())
   {
     gzerr << "Unable to load gazebo\n";
@@ -497,7 +507,11 @@ bool Server::LoadImpl(sdf::ElementPtr _elem,
       << " seconds for namespaces. Giving up.\n";
   }
 
-  physics::init_worlds(rendering::update_scene_poses);
+  if (this->dataPtr->lockstep)
+    physics::init_worlds(rendering::update_scene_poses);
+  else
+    physics::init_worlds(nullptr);
+
   this->dataPtr->stop = false;
 
   return true;
@@ -586,16 +600,21 @@ void Server::Run()
   // The server and sensor manager outlive worlds
   while (!this->dataPtr->stop)
   {
-    bool ret = rendering::wait_for_render_request("", 0.100);
-    if (ret == false)
-      gzerr << "time out reached!" << std::endl;
+    if (this->dataPtr->lockstep)
+      rendering::wait_for_render_request("", 0.100);
+    // bool ret = rendering::wait_for_render_request("", 0.100);
+    // if (ret == false)
+    //   gzerr << "time out reached!" << std::endl;
 
     this->ProcessControlMsgs();
 
     if (physics::worlds_running())
       sensors::run_once();
     else if (sensors::running())
       sensors::stop();
+
+    if (!this->dataPtr->lockstep)
+      common::Time::MSleep(1);
   }
 
   // Shutdown gazebo

gazebo/gazebo_main.cc

@@ -72,6 +72,8 @@ void help()
   << "  -o [ --profile ] arg          Physics preset profile name from the "
   << "options in\n"
   << "                                the world file.\n"
+  << "  --lockstep                    Lockstep simulation so sensor update "
+  <<                                  "rates are respected.\n"
   << "\n";
 }
 

gazebo/physics/World.cc

@@ -159,6 +159,8 @@ World::World(const std::string &_name)
      event::Events::ConnectPause(
        std::bind(&World::SetPaused, this, std::placeholders::_1)));
 
+  this->dataPtr->waitForSensors = nullptr;
+
   // Make sure dbs are initialized
   common::ModelDatabase::Instance();
   common::FuelModelDatabase::Instance();
@@ -2662,7 +2664,7 @@ void World::ProcessMessages()
     if ((this->dataPtr->posePub && this->dataPtr->posePub->HasConnections()) ||
       // When ready to use the direct API for updating scene poses from server,
       // uncomment the following line:
-      // this->dataPtr->updateScenePoses ||
+         this->dataPtr->updateScenePoses ||
         (this->dataPtr->poseLocalPub &&
          this->dataPtr->poseLocalPub->HasConnections()))
     {
@@ -2730,11 +2732,11 @@ void World::ProcessMessages()
 
       // When ready to use the direct API for updating scene poses from server,
       // uncomment the following lines:
-      // // Execute callback to export Pose msg
-      // if (this->dataPtr->updateScenePoses)
-      // {
-      //   this->dataPtr->updateScenePoses(this->Name(), msg);
-      // }
+      // Execute callback to export Pose msg
+      if (this->dataPtr->updateScenePoses)
+      {
+        this->dataPtr->updateScenePoses(this->Name(), msg);
+      }
     }
 
     this->dataPtr->publishModelPoses.clear();

gazebo/rendering/RenderingIface.cc

@@ -24,6 +24,8 @@
 
 using namespace gazebo;
 
+bool g_lockstep = false;
+
 //////////////////////////////////////////////////
 bool rendering::load()
 {
@@ -134,3 +136,15 @@ void rendering::update_scene_poses(const std::string &_name,
     scene->UpdatePoses(_msg);
   }
 }
+
+//////////////////////////////////////////////////
+void rendering::set_lockstep_enabled(bool _enable)
+{
+  g_lockstep = _enable;
+}
+
+//////////////////////////////////////////////////
+bool rendering::lockstep_enabled()
+{
+  return g_lockstep;
+}

gazebo/rendering/RenderingIface.hh

@@ -55,6 +55,22 @@ namespace gazebo
     void update_scene_poses(const std::string &_name,
                             const msgs::PosesStamped &_msg);
 
+    /// \brief Set whether to enable lockstepping for rendering and physics.
+    /// If enabled, the poses of objects in rendering will be updated via
+    /// direct API call instead of transport.
+    /// \param[in] _enable True to enable lockstepping, false to disable
+    /// \sa update_scene_poses
+    GZ_RENDERING_VISIBLE
+    void set_lockstep_enabled(bool _enable);
+
+    /// \brief Get whether or not lockstepping is enabled for rendering and
+    /// physics. If enabled, the poses of objects in rendering is updated via
+    /// direct API call instead of transport.
+    /// \return True if lockstepping is enabled, false if disabled
+    /// \sa lockstep_enabled
+    GZ_RENDERING_VISIBLE
+    bool lockstep_enabled();
+
     /// \brief wait until a render request occurs
     /// \param[in] _name Name of the scene to retrieve
     /// \param[in] _timeoutsec timeout expressed in seconds

gazebo/rendering/Scene.cc

@@ -62,6 +62,7 @@
 #include "gazebo/rendering/TransmitterVisual.hh"
 #include "gazebo/rendering/SelectionObj.hh"
 #include "gazebo/rendering/RayQuery.hh"
+#include "gazebo/rendering/RenderingIface.hh"
 
 #if OGRE_VERSION_MAJOR >= 1 && OGRE_VERSION_MINOR >= 8
 #include "gazebo/rendering/deferred_shading/SSAOLogic.hh"
@@ -157,15 +158,16 @@ Scene::Scene(const std::string &_name, const bool _enableVisualizations,
       &Scene::OnLightModifyMsg, this);
 
   this->dataPtr->isServer = _isServer;
-  if (_isServer)
+
+  if (_isServer && !rendering::lockstep_enabled())
   {
     this->dataPtr->poseSub = this->dataPtr->node->Subscribe("~/pose/local/info",
         &Scene::OnPoseMsg, this);
   }
-  else
+
   // When ready to use the direct API for updating scene poses from server,
   // uncomment the following line and delete the if and else directly above
-  // if (!_isServer)
+  if (!_isServer)
   {
     this->dataPtr->poseSub = this->dataPtr->node->Subscribe("~/pose/info",
         &Scene::OnPoseMsg, this);

gazebo/rendering/Visual.hh

@@ -549,7 +549,7 @@ namespace gazebo
       /// \param[in] _scene Pointer to the scene.
       public: void SetScene(ScenePtr _scene);
 
-      /// \brief Get current.
+      /// \brief Get current scene.
       /// \return Pointer to the scene.
       public: ScenePtr GetScene() const;