diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index 3ce3f4b619..84769ca3f2 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -23,7 +23,6 @@ #include #include #include -#include #include #include @@ -41,6 +40,7 @@ #include #include #include +#include #include #include @@ -228,6 +228,9 @@ class gz::sim::systems::SensorsPrivate /// \brief Check if any of the sensors have connections public: bool SensorsHaveConnections(); + /// \brief Returns all sensors that have a pending trigger + public: std::unordered_set SensorsWithPendingTrigger(); + /// \brief Use to optionally set the background color. public: std::optional backgroundColor; @@ -745,11 +748,15 @@ void Sensors::PostUpdate(const UpdateInfo &_info, this->dataPtr->sensorsToUpdate, _info.simTime); } + std::unordered_set sensorsWithPendingTriggers = + this->dataPtr->SensorsWithPendingTrigger(); + // notify the render thread if updates are available if (hasRenderConnections || this->dataPtr->nextUpdateTime <= _info.simTime || this->dataPtr->renderUtil.PendingSensors() > 0 || - this->dataPtr->forceUpdate) + this->dataPtr->forceUpdate || + !sensorsWithPendingTriggers.empty()) { if (this->dataPtr->disableOnDrainedBattery) this->dataPtr->UpdateBatteryState(_ecm); @@ -769,6 +776,9 @@ void Sensors::PostUpdate(const UpdateInfo &_info, std::unique_lock lockSensors(this->dataPtr->sensorsMutex); this->dataPtr->activeSensors = std::move(this->dataPtr->sensorsToUpdate); + // Add all sensors that have pending triggers. + this->dataPtr->activeSensors.insert(sensorsWithPendingTriggers.begin(), + sensorsWithPendingTriggers.end()); } this->dataPtr->nextUpdateTime = this->dataPtr->NextUpdateTime( @@ -1075,6 +1085,27 @@ bool SensorsPrivate::SensorsHaveConnections() return false; } +////////////////////////////////////////////////// +std::unordered_set +SensorsPrivate::SensorsWithPendingTrigger() +{ + std::unordered_set sensorsWithPendingTrigger; + for (auto id : this->sensorIds) + { + sensors::Sensor *s = this->sensorManager.Sensor(id); + if (nullptr == s) + { + continue; + } + + if (s->HasPendingTrigger()) + { + sensorsWithPendingTrigger.insert(id); + } + } + return sensorsWithPendingTrigger; +} + GZ_ADD_PLUGIN(Sensors, System, Sensors::ISystemConfigure, Sensors::ISystemReset,