Test ABI compatibility of physics-sensor 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,14 +600,21 @@ void Server::Run()
   // The server and sensor manager outlive worlds
   while (!this->dataPtr->stop)
   {
+    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();
 
-    common::Time::MSleep(1);
+    if (!this->dataPtr->lockstep)
+      common::Time::MSleep(1);
   }
 
   // Shutdown gazebo

gazebo/common/Events.cc

@@ -38,6 +38,7 @@ EventT<void ()> Events::worldReset;
 EventT<void ()> Events::timeReset;
 
 EventT<void ()> Events::preRender;
+EventT<void ()> Events::preRenderEnded;
 EventT<void ()> Events::render;
 EventT<void ()> Events::postRender;
 

gazebo/common/Events.hh

@@ -160,6 +160,14 @@ namespace gazebo
               static ConnectionPtr ConnectCreateSensor(T _subscriber)
               { return createSensor.Connect(_subscriber); }
 
+      //////////////////////////////////////////////////////////////////////////
+      /// \brief End of prerender phase signal
+      /// \param[in] _subscriber the subscriber to this event
+      /// \return a connection
+      public: template<typename T>
+              static ConnectionPtr ConnectPreRenderEnded(T _subscriber)
+              { return preRenderEnded.Connect(_subscriber); }
+
       //////////////////////////////////////////////////////////////////////////
       /// \brief Render start signal
       /// \param[in] _subscriber the subscriber to this event
@@ -254,6 +262,9 @@ namespace gazebo
       /// \brief Pre-render
       public: static EventT<void ()> preRender;
 
+      /// \brief End of Pre-render
+      public: static EventT<void ()> preRenderEnded;
+
       /// \brief Render
       public: static EventT<void ()> render;
 

gazebo/gazebo.1.ronn

@@ -49,6 +49,8 @@ Gazebo server runs simulation and handles commandline options, starts a Master,
  Load a server plugin.
 * -o, --profile arg :
  Physics preset profile name from the options in the world file.
+* --lockstep :
+ Lockstep simulation so sensor update rates are respected.
 
 
 ## AUTHOR

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/gzserver.1.ronn

@@ -19,6 +19,8 @@ Gazebo server runs simulation and handles commandline options, starts a Master,
  Produce this help message.
 * -u, --pause :
  Start the server in a paused state.
+* --lockstep :
+ Lockstep simulation so sensor update rates are respected.
 * -e, --physics arg :
  Specify a physics engine (ode|bullet|dart|simbody).
 * -p, --play arg :

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();
@@ -663,6 +665,12 @@ bool World::SensorsInitialized() const
   return this->dataPtr->sensorsInitialized;
 }
 
+/////////////////////////////////////////////////
+void World::SetSensorWaitFunc(std::function<void(double, double)> _func)
+{
+  this->dataPtr->waitForSensors = _func;
+}
+
 //////////////////////////////////////////////////
 void World::Step()
 {
@@ -685,6 +693,10 @@ void World::Step()
 
   DIAG_TIMER_LAP("World::Step", "publishWorldStats");
 
+  if (this->dataPtr->waitForSensors)
+    this->dataPtr->waitForSensors(this->dataPtr->simTime.Double(),
+        this->dataPtr->physicsEngine->GetMaxStepSize());
+
   double updatePeriod = this->dataPtr->physicsEngine->GetUpdatePeriod();
   // sleep here to get the correct update rate
   common::Time tmpTime = common::Time::GetWallTime();
@@ -2652,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()))
     {
@@ -2720,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/physics/World.hh

@@ -459,6 +459,10 @@ namespace gazebo
       /// \return Unique model name.
       public: std::string UniqueModelName(const std::string &_name);
 
+      /// \brief Set callback 'waitForSensors'
+      /// \param[in] function to be called
+      public: void SetSensorWaitFunc(std::function<void(double, double)> _func);
+
       /// \cond
       /// This is an internal function.
       /// \brief Get a model by id.

gazebo/physics/WorldPrivate.hh

@@ -377,6 +377,9 @@ namespace gazebo
       /// \brief Node for ignition transport communication.
       public: ignition::transport::Node ignNode;
 
+      /// \brief Wait until no sensors use the current step any more
+      public: std::function<void(double, double)> waitForSensors;
+
       /// \brief Callback function intended to call the scene with updated Poses
       public: UpdateScenePosesFunc updateScenePoses;
 

gazebo/rendering/RenderingIface.cc

@@ -24,6 +24,8 @@
 
 using namespace gazebo;
 
+bool g_lockstep = false;
+
 //////////////////////////////////////////////////
 bool rendering::load()
 {
@@ -109,6 +111,21 @@ void rendering::remove_scene(const std::string &_name)
   rendering::RenderEngine::Instance()->RemoveScene(_name);
 }
 
+//////////////////////////////////////////////////
+bool rendering::wait_for_render_request(const std::string &_name,
+                                        const double _timeoutsec)
+{
+  ScenePtr scene = get_scene(_name);
+
+  if (!scene)
+  {
+    common::Time::NSleep(_timeoutsec * 1e9);
+    return false;
+  }
+
+  return scene->WaitForRenderRequest(_timeoutsec);
+}
+
 //////////////////////////////////////////////////
 void rendering::update_scene_poses(const std::string &_name,
                                    const msgs::PosesStamped &_msg)
@@ -119,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,31 @@ 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
+    /// \return true if a render request occured, false in case
+    ///          we waited until the timeout
+    GZ_RENDERING_VISIBLE
+    bool wait_for_render_request(const std::string &_name,
+                                 const double _timeoutsec);
+
     /// \brief create rendering::Scene by name.
     /// \param[in] _name Name of the scene to create.
     /// \param[in] _enableVisualizations True enables visualization

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);
@@ -2890,6 +2892,24 @@ void Scene::UpdatePoses(const msgs::PosesStamped &_msg)
   this->dataPtr->newPoseCondition.notify_all();
 }
 
+/////////////////////////////////////////////////
+bool Scene::WaitForRenderRequest(double _timeoutsec)
+{
+  std::unique_lock<std::mutex> lck(this->dataPtr->newPoseMutex);
+
+  while (!this->dataPtr->newPoseAvailable)
+  {
+    auto ret = this->dataPtr->newPoseCondition.wait_for(lck,
+      std::chrono::duration<double>(_timeoutsec));
+
+    if (ret == std::cv_status::timeout)
+      return false;
+  }
+
+  this->dataPtr->newPoseAvailable = false;
+  return true;
+}
+
 /////////////////////////////////////////////////
 void Scene::OnSkeletonPoseMsg(ConstPoseAnimationPtr &_msg)
 {

gazebo/rendering/Scene.hh

@@ -127,6 +127,11 @@ namespace gazebo
       /// \brief Process all received messages.
       public: void PreRender();
 
+      /// \brief Wait until a render request occurs
+      /// \param[in] _timeoutsec timeout expressed in seconds
+      /// \return True if timeout was NOT met
+      public: bool WaitForRenderRequest(double _timeoutsec);
+
       /// \brief Get the OGRE scene manager.
       /// \return Pointer to the Ogre SceneManager.
       public: Ogre::SceneManager *OgreSceneManager() const;

gazebo/rendering/Visual.cc

@@ -820,8 +820,8 @@ void Visual::SetScale(const ignition::math::Vector3d &_scale)
   }
   else
   {
-    gzerr << Name() << ": Size of the collision contains one or several zeros." <<
-      " Collisions may not visualize properly." << std::endl;
+    gzerr << Name() << ": Size of the collision contains one or several zeros."
+      << " Collisions may not visualize properly." << std::endl;
   }
   // Scale selection object in case we have one attached. Other children were
   // scaled from UpdateGeomSize

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;