diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index d161215b31..d70dd6590a 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -361,16 +361,13 @@ 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) { - std::unique_lock timeLock(this->renderMutex); - if (updateTimeApplied <= this->updateTime) - { - timeLock.unlock(); - // publish data - GZ_PROFILE("RunOnce"); - this->sensorManager.RunOnce(updateTimeApplied); - this->eventManager->Emit(); - } + // publish data + GZ_PROFILE("RunOnce"); + this->sensorManager.RunOnce(updateTimeApplied); + this->eventManager->Emit(); } // re-enble sensors @@ -742,17 +739,17 @@ 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 cvLock(this->dataPtr->renderMutex); this->dataPtr->updateAvailable = true;