diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index 8760e51372..0979f82909 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -867,7 +868,7 @@ void Sensors::Configure(const Entity &/*_id*/, } ////////////////////////////////////////////////// -void Sensors::Reset(const UpdateInfo &_info, EntityComponentManager &) +void Sensors::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) { GZ_PROFILE("Sensors::Reset"); @@ -899,6 +900,17 @@ void Sensors::Reset(const UpdateInfo &_info, EntityComponentManager &) this->dataPtr->updateTimeToApply = _info.simTime; this->dataPtr->updateTimeApplied = _info.simTime; } + + { + 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); + } } ////////////////////////////////////////////////// @@ -1125,6 +1137,18 @@ std::string Sensors::CreateSensor(const Entity &_entity, return std::string(); } + // Bail if we already have the sensor + auto entityIt = this->dataPtr->entityToIdMap.find(_entity); + if (entityIt != this->dataPtr->entityToIdMap.end()) + { + sensors::Sensor *s = this->dataPtr->sensorManager.Sensor(entityIt->second); + // TODO(azeey): Don't warn if there has been a Reset. + if (s) { + gzwarn << "Sensor already exists: " << _entity << "\n"; + return s->Name(); + } + } + // Create within gz-sensors sensors::Sensor *sensor{nullptr}; if (_sdf.Type() == sdf::SensorType::CAMERA)