Optimize sensor updates (#1480)

Signed-off-by: Ian Chen <[email protected]> Co-authored-by: Michael Carroll <[email protected]> Co-authored-by: Alejandro Hernández Cordero <[email protected]>
gazebosim · Jun 1, 2022 · b26a573 · b26a573
1 parent 67c41c9
commit b26a573
Show file tree

Hide file tree

Showing 8 changed files with 193 additions and 0 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -20,6 +20,7 @@
 #include <map>
 #include <set>
 #include <unordered_map>
+#include <unordered_set>
 #include <utility>
 #include <vector>
 
@@ -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<math::Color> 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<sensors::RenderingSensor *> tmpDisabledSensors;
+ this->sensorMaskMutex.lock();
+ for (auto id : this->sensorIds)
+ {
+ sensors::Sensor *s = this->sensorManager.Sensor(id);
+ auto rs = dynamic_cast<sensors::RenderingSensor *>(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<sensors::DepthCameraSensor *>(_sensor);
+ if (s)
+ return s->HasConnections();
+ }
+ {
+ auto s = dynamic_cast<sensors::GpuLidarSensor *>(_sensor);
+ if (s)
+ return s->HasConnections();
+ }
+ {
+ auto s = dynamic_cast<sensors::SegmentationCameraSensor *>(_sensor);
+ if (s)
+ return s->HasConnections();
+ }
+ {
+ auto s = dynamic_cast<sensors::ThermalCameraSensor *>(_sensor);
+ if (s)
+ return s->HasConnections();
+ }
+ {
+ auto s = dynamic_cast<sensors::CameraSensor *>(_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,