Physics lockstep for all sensors

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/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/sensors/CameraSensor.cc

@@ -275,18 +275,7 @@ bool CameraSensor::NeedsUpdate()
 //////////////////////////////////////////////////
 void CameraSensor::Update(bool _force)
 {
-  if (this->useStrictRate)
-  {
-    if (this->IsActive() || _force)
-    {
-      if (this->UpdateImpl(_force))
-        this->updated();
-    }
-  }
-  else
-  {
-    Sensor::Update(_force);
-  }
+  Sensor::Update(_force);
 }
 
 //////////////////////////////////////////////////

gazebo/sensors/GpuRaySensor.cc

@@ -21,6 +21,7 @@
 #include "gazebo/physics/World.hh"
 #include "gazebo/physics/Entity.hh"
 #include "gazebo/physics/Model.hh"
+#include "gazebo/physics/PhysicsEngine.hh"
 
 #include "gazebo/common/Exception.hh"
 #include "gazebo/common/Events.hh"
@@ -79,12 +80,26 @@ std::string GpuRaySensor::Topic() const
 void GpuRaySensor::Load(const std::string &_worldName, sdf::ElementPtr _sdf)
 {
   Sensor::Load(_worldName, _sdf);
+  // useStrictRate is set in Sensor::Load()
+  if (this->useStrictRate)
+  {
+    this->connections.push_back(
+        event::Events::ConnectPreRenderEnded(
+          boost::bind(&GpuRaySensor::PrerenderEnded, this)));
+  }
 }
 
 //////////////////////////////////////////////////
 void GpuRaySensor::Load(const std::string &_worldName)
 {
   Sensor::Load(_worldName);
+  // useStrictRate is set in Sensor::Load()
+  if (this->useStrictRate)
+  {
+    this->connections.push_back(
+        event::Events::ConnectPreRenderEnded(
+          boost::bind(&GpuRaySensor::PrerenderEnded, this)));
+  }
 
   this->dataPtr->scanPub =
     this->node->Advertise<msgs::LaserScanStamped>(this->Topic(), 50);
@@ -346,13 +361,66 @@ void GpuRaySensor::Fini()
 //////////////////////////////////////////////////
 void GpuRaySensor::SetActive(bool _value)
 {
+  // If this sensor is reactivated
+  if (this->useStrictRate && _value && !this->IsActive())
+  {
+    // the next rendering time must be reset to ensure it is properly
+    // computed by GpuRaySensor::NeedsUpdate.
+    this->dataPtr->nextRenderingTime = std::numeric_limits<double>::quiet_NaN();
+  }
   Sensor::SetActive(_value);
 }
 
 //////////////////////////////////////////////////
 bool GpuRaySensor::NeedsUpdate()
 {
-  return Sensor::NeedsUpdate();
+  if (this->useStrictRate)
+  {
+    double simTime;
+    if (this->scene)
+      simTime = this->scene->SimTime().Double();
+    else
+      simTime = this->world->SimTime().Double();
+
+    if (simTime < this->lastMeasurementTime.Double())
+    {
+      // Rendering sensors also set the lastMeasurementTime variable in Render()
+      // and lastUpdateTime in Sensor::Update based on Scene::SimTime() which
+      // could be outdated when the world is reset. In this case reset
+      // the variables back to 0.
+      this->ResetLastUpdateTime();
+      return false;
+    }
+
+    double dt = this->world->Physics()->GetMaxStepSize();
+
+    // If next rendering time is not set yet
+    if (std::isnan(this->dataPtr->nextRenderingTime))
+    {
+      if (this->updatePeriod == 0
+          || (simTime > 0.0 &&
+          std::abs(std::fmod(simTime, this->updatePeriod.Double())) < dt))
+      {
+        this->dataPtr->nextRenderingTime = simTime;
+        return true;
+      }
+      else
+      {
+        return false;
+      }
+    }
+
+    if (simTime > this->dataPtr->nextRenderingTime + dt)
+      return true;
+
+    // Trigger on the tick the closest from the targeted rendering time
+    return (ignition::math::lessOrNearEqual(
+          std::abs(simTime - this->dataPtr->nextRenderingTime), dt / 2.0));
+  }
+  else
+  {
+    return Sensor::NeedsUpdate();
+  }
 }
 
 //////////////////////////////////////////////////
@@ -582,16 +650,47 @@ int GpuRaySensor::Fiducial(const unsigned int /*_index*/) const
   return -1;
 }
 
+//////////////////////////////////////////////////
+void GpuRaySensor::PrerenderEnded()
+{
+  if (this->useStrictRate && this->dataPtr->laserCam && this->IsActive() &&
+      this->NeedsUpdate())
+  {
+    // compute next rendering time, take care of the case where period is zero.
+    double dt;
+    if (this->updatePeriod <= 0.0)
+      dt = this->world->Physics()->GetMaxStepSize();
+    else
+      dt = this->updatePeriod.Double();
+    this->dataPtr->nextRenderingTime += dt;
+
+    this->dataPtr->renderNeeded = true;
+    this->lastMeasurementTime = this->scene->SimTime();
+  }
+}
+
 //////////////////////////////////////////////////
 void GpuRaySensor::Render()
 {
-  if (!this->dataPtr->laserCam || !this->IsActive() || !this->NeedsUpdate())
-    return;
+  if (this->useStrictRate)
+  {
+    if (!this->dataPtr->renderNeeded)
+      return;
 
-  this->lastMeasurementTime = this->scene->SimTime();
+    this->dataPtr->laserCam->Render();
+    this->dataPtr->rendered = true;
+    this->dataPtr->renderNeeded = false;
+  }
+  else
+  {
+    if (!this->dataPtr->laserCam || !this->IsActive() || !this->NeedsUpdate())
+      return;
+
+    this->lastMeasurementTime = this->scene->SimTime();
 
-  this->dataPtr->laserCam->Render();
-  this->dataPtr->rendered = true;
+    this->dataPtr->laserCam->Render();
+    this->dataPtr->rendered = true;
+  }
 }
 
 //////////////////////////////////////////////////
@@ -692,11 +791,23 @@ rendering::GpuLaserPtr GpuRaySensor::LaserCamera() const
 //////////////////////////////////////////////////
 double GpuRaySensor::NextRequiredTimestamp() const
 {
-  return Sensor::NextRequiredTimestamp();
+  if (this->useStrictRate)
+  {
+    if (!ignition::math::equal(this->updatePeriod.Double(), 0.0))
+      return this->dataPtr->nextRenderingTime;
+    else
+      return std::numeric_limits<double>::quiet_NaN();
+  }
+  else
+  {
+    return Sensor::NextRequiredTimestamp();
+  }
 }
 
 //////////////////////////////////////////////////
 void GpuRaySensor::ResetLastUpdateTime()
 {
   Sensor::ResetLastUpdateTime();
+  if (this->useStrictRate)
+    this->dataPtr->nextRenderingTime = std::numeric_limits<double>::quiet_NaN();
 }

gazebo/sensors/GpuRaySensor.hh

@@ -270,6 +270,9 @@ namespace gazebo
       /// brief Render the camera.
       private: void Render();
 
+      /// \brief Handle the prerenderEnded event.
+      private: void PrerenderEnded();
+
       /// \internal
       /// \brief Private data pointer.
       private: std::unique_ptr<GpuRaySensorPrivate> dataPtr;

gazebo/sensors/GpuRaySensorPrivate.hh

@@ -17,6 +17,7 @@
 #ifndef _GAZEBO_SENSORS_GPURAYENSOR_PRIVATE_HH_
 #define _GAZEBO_SENSORS_GPURAYENSOR_PRIVATE_HH_
 
+#include <limits>
 #include <mutex>
 #include <sdf/sdf.hh>
 
@@ -86,6 +87,13 @@ namespace gazebo
 
       /// \brief True if the sensor was rendered.
       public: bool rendered;
+
+      /// \brief True if the sensor needs a rendering
+      public: bool renderNeeded = false;
+
+      /// \brief Timestamp of the forthcoming rendering
+      public: double nextRenderingTime
+                           = std::numeric_limits<double>::quiet_NaN();
     };
   }
 }