style fix, add comment to var, protect PublishPerformanceMetrics with…

… mutex Signed-off-by: Ian Chen <ichen@osrfoundation.org>
gazebosim · Sep 2, 2020 · fd995c1 · fd995c1
1 parent 66426aa
commit fd995c1
Showing 1 changed file with 55 additions and 25 deletions.
diff --git a/gazebo/sensors/SensorManager.cc b/gazebo/sensors/SensorManager.cc
@@ -50,6 +50,42 @@ bool g_sensorsDirty = true;
 std::condition_variable
     SensorManager::ImageSensorContainer::conditionPrerendered;
 
+/// Performance metrics variables
+/// \brief last sensor measurement sim time
+std::map<std::string, gazebo::common::Time> sensorsLastMeasurementTime;
+
+/// \brief last sensor measurement real time
+std::map<std::string, gazebo::common::Time> worldLastMeasurementTime;
+
+/// \brief Data structure for storing metric data to be published
+struct sensorPerformanceMetricsType
+{
+  /// \brief sensor real time update rate
+  double sensorRealUpdateRate;
+
+  /// \brief sensor sim time update rate
+  double sensorSimUpdateRate;
+
+  /// \brief Rendering sensor average real time update rate
+  double sensorFPS;
+};
+
+/// \brief A map of sensor name to its performance metrics data
+std::map<std::string, struct sensorPerformanceMetricsType>
+    sensorPerformanceMetrics;
+
+/// \brief Publisher for run-time simulation performance metrics.
+transport::PublisherPtr performanceMetricsPub;
+
+/// \brief Node for publishing performance metrics
+transport::NodePtr node = nullptr;
+
+/// \brief Last sim time measured for performance metrics
+common::Time lastSimTime;
+
+/// \brief Last real time measured for performance metrics
+common::Time lastRealTime;
+
 //////////////////////////////////////////////////
 SensorManager::SensorManager()
   : initialized(false), removeAllSensors(false)
@@ -136,25 +172,11 @@ void SensorManager::WaitForSensors(double _clk, double _dt)
   }
 }
 
-std::map<std::string, gazebo::common::Time> sensorsLastMeasurementTime;
-std::map<std::string, gazebo::common::Time> worldLastMeasurementTime;
-struct sensorPerformanceMetricsType
-{
-  double sensorRealUpdateRate;
-  double sensorSimUpdateRate;
-  double sensorFPS;
-};
-std::map<std::string, struct sensorPerformanceMetricsType> sensorPerformanceMetrics;
-/// \brief Publisher for run-time simulation performance metrics.
-transport::PublisherPtr performanceMetricsPub;
-transport::NodePtr node = nullptr;
-
-common::Time lastSimTime;
-common::Time lastRealTime;
-
+//////////////////////////////////////////////////
 void PublishPerformanceMetrics()
 {
-  if (node == nullptr){
+  if (node == nullptr)
+  {
     auto world = physics::get_world();
     // Transport
     node = transport::NodePtr(new transport::Node());
@@ -181,6 +203,10 @@ void PublishPerformanceMetrics()
   common::Time simTime = world->SimTime();
   common::Time diffSimTime = simTime - lastSimTime;
   common::Time realTimeFactor;
+
+  if (ignition::math::equal(diffSimTime.Double(), 0.0))
+    return;
+
   if (realTime == 0)
     realTimeFactor = 0;
   else
@@ -211,7 +237,8 @@ void PublishPerformanceMetrics()
         if (ret.second == false)
         {
           double updateSimRate =
-            (sensor->LastMeasurementTime() - sensorsLastMeasurementTime[name]).Double();
+            (sensor->LastMeasurementTime() - sensorsLastMeasurementTime[name])
+            .Double();
           if (updateSimRate > 0.0)
           {
             sensorsLastMeasurementTime[name] = sensor->LastMeasurementTime();
@@ -224,16 +251,19 @@ void PublishPerformanceMetrics()
                   (name, emptySensorPerfomanceMetrics));
             if (ret2.second == false)
             {
-              sensorPerformanceMetrics[name].sensorSimUpdateRate = 1.0/updateSimRate;
-              sensorPerformanceMetrics[name].sensorRealUpdateRate = 1.0/updateRealRate;
+              sensorPerformanceMetrics[name].sensorSimUpdateRate =
+                  1.0/updateSimRate;
+              sensorPerformanceMetrics[name].sensorRealUpdateRate =
+                  1.0/updateRealRate;
               worldLastMeasurementTime[name] = world->RealTime();
 
               // Special case for stereo cameras
               sensors::CameraSensorPtr cameraSensor =
                 std::dynamic_pointer_cast<sensors::CameraSensor>(sensor);
               if (nullptr != cameraSensor)
               {
-                sensorPerformanceMetrics[name].sensorFPS = cameraSensor->Camera()->AvgFPS();
+                sensorPerformanceMetrics[name].sensorFPS =
+                    cameraSensor->Camera()->AvgFPS();
               }
               else
               {
@@ -248,8 +278,8 @@ void PublishPerformanceMetrics()
 
   for (auto sensorPerformanceMetric: sensorPerformanceMetrics)
   {
-    msgs::PerformanceMetrics::PerformanceSensorMetrics * performanceSensorMetricsMsg =
-      performanceMetricsMsg.add_sensor();
+    msgs::PerformanceMetrics::PerformanceSensorMetrics
+        *performanceSensorMetricsMsg = performanceMetricsMsg.add_sensor();
     performanceSensorMetricsMsg->set_sensor_name(sensorPerformanceMetric.first);
     performanceSensorMetricsMsg->set_real_sensor_update_rate(
       sensorPerformanceMetric.second.sensorRealUpdateRate);
@@ -821,10 +851,10 @@ void SensorManager::SensorContainer::RunLoop()
 //////////////////////////////////////////////////
 void SensorManager::SensorContainer::Update(bool _force)
 {
-  PublishPerformanceMetrics();
-
   boost::recursive_mutex::scoped_lock lock(this->mutex);
 
+  PublishPerformanceMetrics();
+
   if (this->sensors.empty())
     gzlog << "Updating a sensor container without any sensors.\n";