From e7bc54e836baee81d42ec797ac62453a590924fd Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 16 Oct 2023 14:08:35 -0700 Subject: [PATCH] merge and fix conflicts Signed-off-by: Ian Chen --- src/systems/sensors/Sensors.cc | 69 ++++++++++++++++++++++++---------- 1 file changed, 49 insertions(+), 20 deletions(-) diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index fcb921bdbb..cc16c1ab46 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -123,6 +123,9 @@ 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 @@ -130,12 +133,25 @@ class ignition::gazebo::systems::SensorsPrivate /// 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 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 timeLock(this->renderMutex); + std::unique_lock 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(); - 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 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,6 +391,7 @@ void SensorsPrivate::RunOnce() this->eventManager->Emit(); } + std::unique_lock lk(this->sensorsMutex); this->activeSensors.clear(); } @@ -603,7 +616,19 @@ void Sensors::PostUpdate(const UpdateInfo &_info, if (this->dataPtr->running && this->dataPtr->initialized) { { - std::unique_lock 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 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; } @@ -652,16 +677,21 @@ void Sensors::PostUpdate(const UpdateInfo &_info, std::unique_lock 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 timeLock(this->dataPtr->renderUtilMutex); + this->dataPtr->updateTimeToApply = this->dataPtr->updateTime; } { std::unique_lock cvLock(this->dataPtr->renderMutex); @@ -672,7 +702,6 @@ void Sensors::PostUpdate(const UpdateInfo &_info, } } - ////////////////////////////////////////////////// void SensorsPrivate::UpdateBatteryState(const EntityComponentManager &_ecm) {