Backport sensors system fixes

src/systems/sensors/Sensors.cc

@@ -123,19 +123,35 @@ class ignition::gazebo::systems::SensorsPrivate
   /// \brief Mutex to protect rendering data
   public: std::mutex renderMutex;
 
+  /// \brief Mutex to protect renderUtil changes
+  public: std::mutex renderUtilMutex;
+
   /// \brief Condition variable to signal rendering thread
   ///
   /// This variable is used to block/unblock operations in the rendering
   /// thread.  For a more detailed explanation on the flow refer to the
   /// documentation on RenderThread.
   public: std::condition_variable renderCv;
 
+  /// \brief Condition variable to signal update time applied
+  ///
+  /// This variable is used to block/unblock operations in PostUpdate thread
+  /// to make sure renderUtil's ECM updates are applied to the scene first
+  /// before they are overriden by PostUpdate
+  public: std::condition_variable updateTimeCv;
+
   /// \brief Connection to events::Stop event, used to stop thread
   public: common::ConnectionPtr stopConn;
 
   /// \brief Update time for the next rendering iteration
   public: std::chrono::steady_clock::duration updateTime;
 
+  /// \brief Update time applied in the rendering thread
+  public: std::chrono::steady_clock::duration updateTimeApplied;
+
+  /// \brief Update time to be appplied in the rendering thread
+  public: std::chrono::steady_clock::duration updateTimeToApply;
+
   /// \brief Next sensors update time
   public: std::chrono::steady_clock::duration nextUpdateTime;
 
@@ -148,10 +164,6 @@ class ignition::gazebo::systems::SensorsPrivate
   /// \brief Mutex to protect sensorMask
   public: std::mutex sensorsMutex;
 
-  /// \brief Mask sensor updates for sensors currently being rendered
-  public: std::unordered_map<sensors::SensorId,
-    std::chrono::steady_clock::duration> sensorMask;
-
   /// \brief Pointer to the event manager
   public: EventManager *eventManager{nullptr};
 
@@ -292,13 +304,13 @@ void SensorsPrivate::RunOnce()
   if (!this->scene)
     return;
 
-  std::chrono::steady_clock::duration updateTimeApplied;
   IGN_PROFILE("SensorsPrivate::RunOnce");
   {
     IGN_PROFILE("Update");
-    std::unique_lock<std::mutex> timeLock(this->renderMutex);
+    std::unique_lock<std::mutex> timeLock(this->renderUtilMutex);
     this->renderUtil.Update();
-    updateTimeApplied = this->updateTime;
+    this->updateTimeApplied = this->updateTime;
+    this->updateTimeCv.notify_one();
   }
 
   bool activeSensorsEmpty = true;
@@ -329,7 +341,7 @@ void SensorsPrivate::RunOnce()
     {
       IGN_PROFILE("PreRender");
       this->eventManager->Emit<events::PreRender>();
-      this->scene->SetTime(updateTimeApplied);
+      this->scene->SetTime(this->updateTimeApplied);
       // Update the scene graph manually to improve performance
       // We only need to do this once per frame It is important to call
       // sensors::RenderingSensor::SetManualSceneUpdate and set it to true
@@ -356,11 +368,11 @@ void SensorsPrivate::RunOnce()
     // safety check to see if reset occurred while we're rendering
     // avoid publishing outdated data if reset occurred
     std::unique_lock<std::mutex> timeLock(this->renderMutex);
-    if (updateTimeApplied <= this->updateTime)
+    if (this->updateTimeApplied <= this->updateTime)
     {
       // publish data
       IGN_PROFILE("RunOnce");
-      this->sensorManager.RunOnce(updateTimeApplied);
+      this->sensorManager.RunOnce(this->updateTimeApplied);
     }
 
     // re-enble sensors
@@ -379,12 +391,16 @@ void SensorsPrivate::RunOnce()
       this->eventManager->Emit<events::PostRender>();
     }
 
+    std::unique_lock<std::mutex> lk(this->sensorsMutex);
     this->activeSensors.clear();
   }
 
-  this->updateAvailable = false;
   this->forceUpdate = false;
-  this->renderCv.notify_one();
+  {
+    std::unique_lock<std::mutex> cvLock(this->renderMutex);
+    this->updateAvailable = false;
+    this->renderCv.notify_one();
+  }
 }
 
 //////////////////////////////////////////////////
@@ -600,7 +616,19 @@ void Sensors::PostUpdate(const UpdateInfo &_info,
   if (this->dataPtr->running && this->dataPtr->initialized)
   {
     {
-      std::unique_lock<std::mutex> lock(this->dataPtr->renderMutex);
+      IGN_PROFILE("UpdateFromECM");
+      // Make sure we do not override the state in renderUtil if there are
+      // still ECM changes that still need to be propagated to the scene,
+      // i.e. wait until renderUtil.Update(), has taken place in the
+      // rendering thread
+      std::unique_lock<std::mutex> lock(this->dataPtr->renderUtilMutex);
+      this->dataPtr->updateTimeCv.wait(lock, [this]()
+      {
+        return !this->dataPtr->updateAvailable ||
+               (this->dataPtr->updateTimeToApply ==
+               this->dataPtr->updateTimeApplied);
+      });
+
       this->dataPtr->renderUtil.UpdateFromECM(_info, _ecm);
       this->dataPtr->updateTime = _info.simTime;
     }
@@ -649,24 +677,31 @@ void Sensors::PostUpdate(const UpdateInfo &_info,
         std::unique_lock<std::mutex> lockSensors(this->dataPtr->sensorsMutex);
         this->dataPtr->activeSensors =
             std::move(this->dataPtr->sensorsToUpdate);
+      }
+
+      this->dataPtr->nextUpdateTime = this->dataPtr->NextUpdateTime(
+          this->dataPtr->sensorsToUpdate, _info.simTime);
 
-        this->dataPtr->nextUpdateTime = this->dataPtr->NextUpdateTime(
-            this->dataPtr->sensorsToUpdate, _info.simTime);
+      // Force scene tree update if there are sensors to be created or
+      // subscribes to the render events. This does not necessary force
+      // sensors to update. Only active sensors will be updated
+      this->dataPtr->forceUpdate =
+          (this->dataPtr->renderUtil.PendingSensors() > 0) ||
+          hasRenderConnections;
 
-        // Force scene tree update if there are sensors to be created or
-        // subscribes to the render events. This does not necessary force
-        // sensors to update. Only active sensors will be updated
-        this->dataPtr->forceUpdate =
-            (this->dataPtr->renderUtil.PendingSensors() > 0) ||
-            hasRenderConnections;
+      {
+        std::unique_lock<std::mutex> timeLock(this->dataPtr->renderUtilMutex);
+        this->dataPtr->updateTimeToApply = this->dataPtr->updateTime;
+      }
+      {
+        std::unique_lock<std::mutex> cvLock(this->dataPtr->renderMutex);
         this->dataPtr->updateAvailable = true;
+        this->dataPtr->renderCv.notify_one();
       }
-      this->dataPtr->renderCv.notify_one();
     }
   }
 }
 
-
 //////////////////////////////////////////////////
 void SensorsPrivate::UpdateBatteryState(const EntityComponentManager &_ecm)
 {