[Forward Port Gazebo 11] Publish performance metrics (#2819) (#2848)

Signed-off-by: ahcorde <ahcorde@gmail.com>
Signed-off-by: Ian Chen <ichen@osrfoundation.org>
Co-authored-by: Ian Chen <ichen@osrfoundation.org>

gazebo/msgs/CMakeLists.txt

@@ -8,9 +8,9 @@ set (msgs
   axis.proto
   battery.proto
   boxgeom.proto
-  camerasensor.proto
   camera_cmd.proto
   camera_lens.proto
+  camerasensor.proto
   cessna.proto
   collision.proto
   color.proto
@@ -29,16 +29,16 @@ set (msgs
   geometry.proto
   gps.proto
   gps_sensor.proto
-  gui_camera.proto
   gui.proto
+  gui_camera.proto
   gz_string.proto
   gz_string_v.proto
   header.proto
   heightmapgeom.proto
   hydra.proto
-  imagegeom.proto
   image.proto
   image_stamped.proto
+  imagegeom.proto
   images_stamped.proto
   imu.proto
   imu_sensor.proto
@@ -68,11 +68,12 @@ set (msgs
   model_configuration.proto
   model_v.proto
   packet.proto
-  physics.proto
   param.proto
   param_v.proto
-  planegeom.proto
+  performance_metrics.proto
+  physics.proto
   pid.proto
+  planegeom.proto
   plugin.proto
   pointcloud.proto
   polylinegeom.proto
@@ -83,20 +84,18 @@ set (msgs
   pose_v.proto
   poses_stamped.proto
   projector.proto
-  propagation_particle.proto
   propagation_grid.proto
-  publishers.proto
+  propagation_particle.proto
   publish.proto
+  publishers.proto
   quaternion.proto
-  sonar.proto
-  sonar_stamped.proto
   raysensor.proto
   request.proto
   response.proto
-  rest_response.proto
   rest_login.proto
   rest_logout.proto
   rest_post.proto
+  rest_response.proto
   road.proto
   scene.proto
   selection.proto
@@ -106,6 +105,8 @@ set (msgs
   shadows.proto
   sim_event.proto
   sky.proto
+  sonar.proto
+  sonar_stamped.proto
   spheregeom.proto
   spherical_coordinates.proto
   subscribe.proto
@@ -126,9 +127,9 @@ set (msgs
   wireless_node.proto
   wireless_nodes.proto
   world_control.proto
+  world_modify.proto
   world_reset.proto
   world_stats.proto
-  world_modify.proto
   wrench.proto
   wrench_stamped.proto
 )

gazebo/msgs/performance_metrics.proto

@@ -0,0 +1,37 @@
+syntax = "proto2";
+package gazebo.msgs;
+
+/// \ingroup gazebo_msgs
+/// \interface PerformanceMetrics
+/// \brief A message for run-time simulation performance metrics
+
+message PerformanceMetrics
+{
+  /// \brief This message contains information about the performance of
+  /// each sensor in the world.
+  /// If the sensor is a camera then it will publish the frame per second (fps).
+  message PerformanceSensorMetrics
+  {
+    /// \brief Sensor name
+    required string name             = 1;
+
+    /// \brief The update rate of the sensor in real time.
+    required double real_update_rate = 2;
+
+    /// \brief The update rate of the sensor in simulation time.
+    required double sim_update_rate  = 3;
+
+    /// \brief If the sensor is a camera then this field should be filled
+    /// with average fps in real time.
+    optional double fps                     = 4;
+  }
+
+  /// max_step_size x real_time_update_rate sets an upper bound of
+  /// real_time_factor. If real_time_factor < 1 the simulation is
+  /// slower than real time.
+  required double real_time_factor         = 1[default=0.0];
+
+  /// Each sensor in the world will create a PerformanceSensorMetrics
+  /// message publishing information about the performance.
+  repeated PerformanceSensorMetrics sensor = 2;
+}

gazebo/sensors/SensorManager.cc

@@ -17,16 +17,20 @@
 
 #include <functional>
 #include <boost/bind.hpp>
-#include "gazebo/common/Assert.hh"
-#include "gazebo/common/Time.hh"
 
+#include "gazebo/physics/Link.hh"
+#include "gazebo/physics/Model.hh"
 #include "gazebo/physics/PhysicsIface.hh"
 #include "gazebo/physics/PhysicsEngine.hh"
+#include "gazebo/physics/PhysicsIface.hh"
 #include "gazebo/physics/World.hh"
+#include "gazebo/rendering/Camera.hh"
+#include "gazebo/sensors/CameraSensor.hh"
 #include "gazebo/sensors/Sensor.hh"
-#include "gazebo/sensors/SensorsIface.hh"
 #include "gazebo/sensors/SensorFactory.hh"
 #include "gazebo/sensors/SensorManager.hh"
+#include "gazebo/sensors/SensorsIface.hh"
+#include "gazebo/transport/transport.hh"
 #include "gazebo/util/LogPlay.hh"
 
 #include "ignition/common/Profiler.hh"
@@ -42,6 +46,46 @@ boost::mutex g_sensorTimingMutex;
 /// max update rate needs to be recalculated
 bool g_sensorsDirty = true;
 
+/// 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. The difference
+  /// between sensorAvgFps and sensorRealUpdateRte is that sensorAvgFps is
+  /// for rendering sensors only and the rate is averaged over a fixed
+  /// window size, whereas the sensorSimUpdateRate stores the instantaneous
+  /// update rate and it is filled by all sensors.
+  double sensorAvgFPS;
+};
+
+/// \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)
@@ -129,6 +173,126 @@ void SensorManager::WaitForSensors(double _clk, double _dt)
   }
 }
 
+void PublishPerformanceMetrics()
+{
+  if (node == nullptr)
+  {
+    auto world = physics::get_world();
+    // Transport
+    node = transport::NodePtr(new transport::Node());
+    node->Init(world->Name());
+    performanceMetricsPub =
+     node->Advertise<msgs::PerformanceMetrics>(
+         "/gazebo/performance_metrics", 10, 5);
+  }
+
+  if (!performanceMetricsPub || !performanceMetricsPub->HasConnections())
+  {
+    return;
+  }
+
+  physics::WorldPtr world = physics::get_world(
+    gazebo::physics::get_world()->Name());
+
+  /// Outgoing run-time simulation performance metrics.
+  msgs::PerformanceMetrics performanceMetricsMsg;
+
+  // Real time factor
+  common::Time realTime = world->RealTime();
+  common::Time diffRealtime = realTime - lastRealTime;
+  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
+    realTimeFactor = diffSimTime / diffRealtime;
+
+  performanceMetricsMsg.set_real_time_factor(realTimeFactor.Double());
+
+  lastRealTime = realTime;
+  lastSimTime = simTime;
+
+  /// update sim time for sensors
+  for (auto model: world->Models())
+  {
+    for (auto link: model->GetLinks())
+    {
+      for (unsigned int i = 0; i < link->GetSensorCount(); i++)
+      {
+        std::string name = link->GetSensorName(i);
+        sensors::SensorPtr sensor = sensors::get_sensor(name);
+
+        auto ret = sensorsLastMeasurementTime.insert(
+            std::pair<std::string, gazebo::common::Time>(name, 0));
+        worldLastMeasurementTime.insert(
+                std::pair<std::string, gazebo::common::Time>(name, 0));
+        if (ret.second == false)
+        {
+          double updateSimRate =
+            (sensor->LastMeasurementTime() - sensorsLastMeasurementTime[name])
+            .Double();
+          if (updateSimRate > 0.0)
+          {
+            sensorsLastMeasurementTime[name] = sensor->LastMeasurementTime();
+            double updateRealRate =
+              (world->RealTime() - worldLastMeasurementTime[name]).Double();
+
+            struct sensorPerformanceMetricsType emptySensorPerfomanceMetrics;
+            auto ret2 = sensorPerformanceMetrics.insert(
+                std::pair<std::string, struct sensorPerformanceMetricsType>
+                  (name, emptySensorPerfomanceMetrics));
+            if (ret2.second == false)
+            {
+              ret2.first->second.sensorSimUpdateRate =
+                  1.0/updateSimRate;
+              ret2.first->second.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)
+              {
+                ret2.first->second.sensorAvgFPS =
+                    cameraSensor->Camera()->AvgFPS();
+              }
+              else
+              {
+                ret2.first->second.sensorAvgFPS = -1;
+              }
+            }
+          }
+        }
+      }
+    }
+  }
+
+  for (auto sensorPerformanceMetric: sensorPerformanceMetrics)
+  {
+    msgs::PerformanceMetrics::PerformanceSensorMetrics
+        *performanceSensorMetricsMsg = performanceMetricsMsg.add_sensor();
+    performanceSensorMetricsMsg->set_name(sensorPerformanceMetric.first);
+    performanceSensorMetricsMsg->set_real_update_rate(
+      sensorPerformanceMetric.second.sensorRealUpdateRate);
+    performanceSensorMetricsMsg->set_sim_update_rate(
+      sensorPerformanceMetric.second.sensorSimUpdateRate);
+    if (sensorPerformanceMetric.second.sensorAvgFPS >= 0.0)
+    {
+      performanceSensorMetricsMsg->set_fps(
+        sensorPerformanceMetric.second.sensorAvgFPS);
+    }
+  }
+
+  // Publish data
+  performanceMetricsPub->Publish(performanceMetricsMsg);
+}
+
 //////////////////////////////////////////////////
 void SensorManager::Update(bool _force)
 {
@@ -686,6 +850,8 @@ void SensorManager::SensorContainer::Update(bool _force)
 {
   boost::recursive_mutex::scoped_lock lock(this->mutex);
 
+  PublishPerformanceMetrics();
+
   if (this->sensors.empty())
     gzlog << "Updating a sensor container without any sensors.\n";