diff --git a/src/systems/air_pressure/AirPressure.cc b/src/systems/air_pressure/AirPressure.cc index 6f4188ede7..a2fcb5704a 100644 --- a/src/systems/air_pressure/AirPressure.cc +++ b/src/systems/air_pressure/AirPressure.cc @@ -140,6 +140,24 @@ void AirPressure::PostUpdate(const UpdateInfo &_info, if (!_info.paused) { + // check to see if update is necessary + // we only update if there is at least one sensor that needs data + // and that sensor has subscribers. + // note: ign-sensors does its own throttling. Here the check is mainly + // to avoid doing work in the AirPressurePrivate::UpdatePressures function + bool needsUpdate = false; + for (auto &it : this->dataPtr->entitySensorMap) + { + if (it.second->NextDataUpdateTime() <= _info.simTime && + it.second->HasConnections()) + { + needsUpdate = true; + break; + } + } + if (!needsUpdate) + return; + this->dataPtr->UpdateAirPressures(_ecm); for (auto &it : this->dataPtr->entitySensorMap) diff --git a/src/systems/altimeter/Altimeter.cc b/src/systems/altimeter/Altimeter.cc index d72ca9847a..3152c0c7e7 100644 --- a/src/systems/altimeter/Altimeter.cc +++ b/src/systems/altimeter/Altimeter.cc @@ -141,6 +141,24 @@ void Altimeter::PostUpdate(const UpdateInfo &_info, // Only update and publish if not paused. if (!_info.paused) { + // check to see if update is necessary + // we only update if there is at least one sensor that needs data + // and that sensor has subscribers. + // note: ign-sensors does its own throttling. Here the check is mainly + // to avoid doing work in the AltimeterPrivate::UpdateAltimeters function + bool needsUpdate = false; + for (auto &it : this->dataPtr->entitySensorMap) + { + if (it.second->NextDataUpdateTime() <= _info.simTime && + it.second->HasConnections()) + { + needsUpdate = true; + break; + } + } + if (!needsUpdate) + return; + this->dataPtr->UpdateAltimeters(_ecm); for (auto &it : this->dataPtr->entitySensorMap) diff --git a/src/systems/force_torque/ForceTorque.cc b/src/systems/force_torque/ForceTorque.cc index e1668fbf71..7ebd91c3d2 100644 --- a/src/systems/force_torque/ForceTorque.cc +++ b/src/systems/force_torque/ForceTorque.cc @@ -132,6 +132,24 @@ void ForceTorque::PostUpdate(const UpdateInfo &_info, // Only update and publish if not paused. if (!_info.paused) { + // check to see if update is necessary + // we only update if there is at least one sensor that needs data + // and that sensor has subscribers. + // note: ign-sensors does its own throttling. Here the check is mainly + // to avoid doing work in the ForceTorquePrivate::Update function + bool needsUpdate = false; + for (auto &it : this->dataPtr->entitySensorMap) + { + if (it.second->NextDataUpdateTime() <= _info.simTime && + it.second->HasConnections()) + { + needsUpdate = true; + break; + } + } + if (!needsUpdate) + return; + this->dataPtr->Update(_ecm); for (auto &it : this->dataPtr->entitySensorMap) diff --git a/src/systems/imu/Imu.cc b/src/systems/imu/Imu.cc index 2199854e2e..5afbd67ae0 100644 --- a/src/systems/imu/Imu.cc +++ b/src/systems/imu/Imu.cc @@ -145,6 +145,24 @@ void Imu::PostUpdate(const UpdateInfo &_info, // Only update and publish if not paused. if (!_info.paused) { + // check to see if update is necessary + // we only update if there is at least one sensor that needs data + // and that sensor has subscribers. + // note: ign-sensors does its own throttling. Here the check is mainly + // to avoid doing work in the ImuPrivate::Update function + bool needsUpdate = false; + for (auto &it : this->dataPtr->entitySensorMap) + { + if (it.second->NextDataUpdateTime() <= _info.simTime && + it.second->HasConnections()) + { + needsUpdate = true; + break; + } + } + if (!needsUpdate) + return; + this->dataPtr->Update(_ecm); for (auto &it : this->dataPtr->entitySensorMap) diff --git a/src/systems/logical_camera/LogicalCamera.cc b/src/systems/logical_camera/LogicalCamera.cc index b573f5214b..5c21813f25 100644 --- a/src/systems/logical_camera/LogicalCamera.cc +++ b/src/systems/logical_camera/LogicalCamera.cc @@ -143,6 +143,25 @@ void LogicalCamera::PostUpdate(const UpdateInfo &_info, // Only update and publish if not paused. if (!_info.paused) { + // check to see if update is necessary + // we only update if there is at least one sensor that needs data + // and that sensor has subscribers. + // note: ign-sensors does its own throttling. Here the check is mainly + // to avoid doing work in the LogicalCameraPrivate::UpdateLogicalCameras + // function + bool needsUpdate = false; + for (auto &it : this->dataPtr->entitySensorMap) + { + if (it.second->NextDataUpdateTime() <= _info.simTime && + it.second->HasConnections()) + { + needsUpdate = true; + break; + } + } + if (!needsUpdate) + return; + this->dataPtr->UpdateLogicalCameras(_ecm); for (auto &it : this->dataPtr->entitySensorMap) diff --git a/src/systems/magnetometer/Magnetometer.cc b/src/systems/magnetometer/Magnetometer.cc index b46b77a424..2ecad15692 100644 --- a/src/systems/magnetometer/Magnetometer.cc +++ b/src/systems/magnetometer/Magnetometer.cc @@ -142,6 +142,24 @@ void Magnetometer::PostUpdate(const UpdateInfo &_info, // Only update and publish if not paused. if (!_info.paused) { + // check to see if update is necessary + // we only update if there is at least one sensor that needs data + // and that sensor has subscribers. + // note: ign-sensors does its own throttling. Here the check is mainly + // to avoid doing work in the MagnetometerPrivate::Update function + bool needsUpdate = false; + for (auto &it : this->dataPtr->entitySensorMap) + { + if (it.second->NextDataUpdateTime() <= _info.simTime && + it.second->HasConnections()) + { + needsUpdate = true; + break; + } + } + if (!needsUpdate) + return; + this->dataPtr->Update(_ecm); for (auto &it : this->dataPtr->entitySensorMap) diff --git a/src/systems/navsat/NavSat.cc b/src/systems/navsat/NavSat.cc index faa8f4f38f..2eea382d9e 100644 --- a/src/systems/navsat/NavSat.cc +++ b/src/systems/navsat/NavSat.cc @@ -137,6 +137,24 @@ void NavSat::PostUpdate(const UpdateInfo &_info, // Only update and publish if not paused. if (!_info.paused) { + // check to see if update is necessary + // we only update if there is at least one sensor that needs data + // and that sensor has subscribers. + // note: ign-sensors does its own throttling. Here the check is mainly + // to avoid doing work in the NavSat::Implementation::Update function + bool needsUpdate = false; + for (auto &it : this->dataPtr->entitySensorMap) + { + if (it.second->NextDataUpdateTime() <= _info.simTime && + it.second->HasConnections()) + { + needsUpdate = true; + break; + } + } + if (!needsUpdate) + return; + this->dataPtr->Update(_ecm); for (auto &it : this->dataPtr->entitySensorMap) diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index e83d053e30..69222d2209 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -186,6 +187,11 @@ class ignition::gazebo::systems::SensorsPrivate /// \param[in] _ecm Entity component manager public: void UpdateBatteryState(const EntityComponentManager &_ecm); + /// \brief Check if sensor has subscribers + /// \param[in] _sensor Sensor to check + /// \return True if the sensor has subscribers, false otherwise + public: bool HasConnections(sensors::RenderingSensor *_sensor) const; + /// \brief Use to optionally set the background color. public: std::optional backgroundColor; @@ -316,12 +322,34 @@ void SensorsPrivate::RunOnce() this->scene->PreRender(); } + // disable sensors that have no subscribers to prevent doing unnecessary + // work + std::unordered_set tmpDisabledSensors; + this->sensorMaskMutex.lock(); + for (auto id : this->sensorIds) + { + sensors::Sensor *s = this->sensorManager.Sensor(id); + auto rs = dynamic_cast(s); + if (rs->IsActive() && !this->HasConnections(rs)) + { + rs->SetActive(false); + tmpDisabledSensors.insert(rs); + } + } + this->sensorMaskMutex.unlock(); + { // publish data IGN_PROFILE("RunOnce"); this->sensorManager.RunOnce(this->updateTime); } + // re-enble sensors + for (auto &rs : tmpDisabledSensors) + { + rs->SetActive(true); + } + { IGN_PROFILE("PostRender"); // Update the scene graph manually to improve performance @@ -805,6 +833,44 @@ std::string Sensors::CreateSensor(const Entity &_entity, return sensor->Name(); } +////////////////////////////////////////////////// +bool SensorsPrivate::HasConnections(sensors::RenderingSensor *_sensor) const +{ + if (!_sensor) + return true; + + // \todo(iche033) Remove this function once a virtual + // sensors::RenderingSensor::HasConnections function is available + { + auto s = dynamic_cast(_sensor); + if (s) + return s->HasConnections(); + } + { + auto s = dynamic_cast(_sensor); + if (s) + return s->HasConnections(); + } + { + auto s = dynamic_cast(_sensor); + if (s) + return s->HasConnections(); + } + { + auto s = dynamic_cast(_sensor); + if (s) + return s->HasConnections(); + } + { + auto s = dynamic_cast(_sensor); + if (s) + return s->HasConnections(); + } + ignwarn << "Unable to check connection count for sensor: " << _sensor->Name() + << ". Unknown sensor type." << std::endl; + return true; +} + IGNITION_ADD_PLUGIN(Sensors, System, Sensors::ISystemConfigure, Sensors::ISystemUpdate,