Publish performance sensor metrics. (#146)

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>

Co-authored-by: Louise Poubel <louise@openrobotics.org>

include/ignition/sensors/Sensor.hh

@@ -27,6 +27,7 @@
 #pragma warning(pop)
 #endif
 
+#include <chrono>
 #include <memory>
 #include <string>
 
@@ -146,6 +147,14 @@ namespace ignition
       /// \return True if a valid topic was set.
       public: bool SetTopic(const std::string &_topic);
 
+      /// \brief Get flag state for enabling performance metrics publication.
+      /// \return True if performance metrics are enabled, false otherwise.
+      public: bool EnableMetrics() const;
+
+      /// \brief Set flag to enable publishing performance metrics
+      /// \param[in] _enableMetrics True to enable.
+      public: void SetEnableMetrics(bool _enableMetrics);
+
       /// \brief Get parent link of the sensor.
       /// \return Parent link of sensor.
       public: std::string Parent() const;
@@ -180,6 +189,12 @@ namespace ignition
       public: void AddSequence(ignition::msgs::Header *_msg,
                   const std::string &_seqKey = "default");
 
+      /// \brief Publishes information about the performance of the sensor.
+      ///        This method is called by Update().
+      /// \param[in] _now Current time.
+      public: void PublishMetrics(
+        const std::chrono::duration<double> &_now);
+
       IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING
       /// \internal
       /// \brief Data pointer for private data

src/Sensor.cc

@@ -16,17 +16,18 @@
 */
 
 #include "ignition/sensors/Sensor.hh"
+
+#include <chrono>
 #include <map>
 #include <vector>
+
 #include <ignition/common/Console.hh>
 #include <ignition/common/Profiler.hh>
+#include <ignition/transport/Node.hh>
 #include <ignition/transport/TopicUtils.hh>
 
-#include <ignition/sensors/Manager.hh>
-
 using namespace ignition::sensors;
 
-
 class ignition::sensors::SensorPrivate
 {
   /// \brief Populates fields from a <sensor> DOM
@@ -37,6 +38,10 @@ class ignition::sensors::SensorPrivate
   /// \return True if a valid topic was set.
   public: bool SetTopic(const std::string &_topic);
 
+  /// \brief Publishes information about the performance of the sensor.
+  /// \param[in] _now Current simulation time.
+  public: void PublishMetrics(const std::chrono::duration<double> &_now);
+
   /// \brief id given to sensor when constructed
   public: SensorId id;
 
@@ -55,12 +60,27 @@ class ignition::sensors::SensorPrivate
   /// \brief Pose of the sensor
   public: ignition::math::Pose3d pose;
 
+  /// \brief Flag to enable publishing performance metrics.
+  public: bool enableMetrics{false};
+
   /// \brief How many times the sensor will generate data per second
   public: double updateRate = 0.0;
 
   /// \brief What sim time should this sensor update at
   public: ignition::common::Time nextUpdateTime;
 
+  /// \brief Last steady clock time reading from last Update call.
+  public: std::chrono::time_point<std::chrono::steady_clock> lastRealTime;
+
+  /// \brief Last sim time at Update call.
+  public: std::chrono::duration<double> lastUpdateTime{0};
+
+  /// \brief Transport node.
+  public: ignition::transport::Node node;
+
+  /// \brief Publishes the PerformanceSensorMetrics message.
+  public: ignition::transport::Node::Publisher performanceSensorMetricsPub;
+
   /// \brief SDF element with sensor information.
   public: sdf::ElementPtr sdf = nullptr;
 
@@ -110,6 +130,8 @@ bool SensorPrivate::PopulateFromSDF(const sdf::Sensor &_sdf)
   }
 
   this->updateRate = _sdf.UpdateRate();
+
+  this->enableMetrics = _sdf.EnableMetrics();
   return true;
 }
 
@@ -196,6 +218,79 @@ bool SensorPrivate::SetTopic(const std::string &_topic)
   return true;
 }
 
+//////////////////////////////////////////////////
+bool Sensor::EnableMetrics() const
+{
+  return this->dataPtr->enableMetrics;
+}
+
+
+//////////////////////////////////////////////////
+void Sensor::SetEnableMetrics(bool _enableMetrics)
+{
+  this->dataPtr->enableMetrics = _enableMetrics;
+}
+
+//////////////////////////////////////////////////
+void Sensor::PublishMetrics(const std::chrono::duration<double> &_now)
+{
+  return this->dataPtr->PublishMetrics(_now);
+}
+
+//////////////////////////////////////////////////
+void SensorPrivate::PublishMetrics(const std::chrono::duration<double> &_now)
+{
+  if(!this->performanceSensorMetricsPub)
+  {
+    const auto validTopic = transport::TopicUtils::AsValidTopic(
+      this->topic + "/performance_metrics");
+    if (validTopic.empty())
+    {
+      ignerr << "Failed to set metrics sensor topic [" << topic << "]" <<
+        std::endl;
+      return;
+    }
+    this->performanceSensorMetricsPub =
+      node.Advertise<msgs::PerformanceSensorMetrics>(validTopic);
+  }
+  if (!performanceSensorMetricsPub ||
+      !performanceSensorMetricsPub.HasConnections())
+  {
+    return;
+  }
+
+  // Computes simulation update rate and real update rate.
+  double simUpdateRate;
+  double realUpdateRate;
+  const auto clockNow = std::chrono::steady_clock::now();
+  // If lastUpdateTime == 0 means it wasn't initialized yet.
+  if(this->lastUpdateTime.count() > 0)
+  {
+    const double diffSimUpdate = _now.count() -
+      this->lastUpdateTime.count();
+    simUpdateRate = 1.0 / diffSimUpdate;
+    const double diffRealUpdate =
+      std::chrono::duration_cast<std::chrono::duration<double>>(
+        clockNow - this->lastRealTime).count();
+    realUpdateRate = diffRealUpdate < std::numeric_limits<double>::epsilon() ?
+      std::numeric_limits<double>::infinity() : 1.0 / diffRealUpdate;
+  }
+
+  // Update last time values.
+  this->lastUpdateTime = _now;
+  this->lastRealTime = clockNow;
+
+  // Fill performance sensor metrics message.
+  msgs::PerformanceSensorMetrics performanceSensorMetricsMsg;
+  performanceSensorMetricsMsg.set_name(this->name);
+  performanceSensorMetricsMsg.set_real_update_rate(realUpdateRate);
+  performanceSensorMetricsMsg.set_sim_update_rate(simUpdateRate);
+  performanceSensorMetricsMsg.set_nominal_update_rate(this->updateRate);
+
+  // Publish data
+  performanceSensorMetricsPub.Publish(performanceSensorMetricsMsg);
+}
+
 //////////////////////////////////////////////////
 ignition::math::Pose3d Sensor::Pose() const
 {
@@ -256,6 +351,12 @@ bool Sensor::Update(const ignition::common::Time &_now,
   // Make the update happen
   result = this->Update(_now);
 
+  // Publish metrics
+  if (this->EnableMetrics())
+  {
+    this->PublishMetrics(std::chrono::duration<double>(_now.Double()));
+  }
+
   if (!_force && this->dataPtr->updateRate > 0.0)
   {
     // Update the time the plugin should be loaded

src/Sensor_TEST.cc

@@ -14,11 +14,27 @@
  * limitations under the License.
  *
 */
+#include <ignition/sensors/Sensor.hh>
+
+#include <atomic>
+#include <memory>
+
 #include <gtest/gtest.h>
 
+#ifdef _WIN32
+#pragma warning(push)
+#pragma warning(disable: 4005)
+#pragma warning(disable: 4251)
+#endif
+#include <ignition/msgs/performance_sensor_metrics.pb.h>
+#ifdef _WIN32
+#pragma warning(pop)
+#endif
+
 #include <ignition/common/Console.hh>
+#include <ignition/common/Time.hh>
 #include <ignition/sensors/Export.hh>
-#include <ignition/sensors/Sensor.hh>
+#include <ignition/transport/Node.hh>
 
 using namespace ignition;
 using namespace sensors;
@@ -124,6 +140,76 @@ TEST(Sensor_TEST, Topic)
   EXPECT_FALSE(sensor.SetTopic(""));
 }
 
+class SensorUpdate : public ::testing::Test
+{
+  // Documentation inherited
+  protected: void SetUp() override
+  {
+    ignition::common::Console::SetVerbosity(4);
+    node.Subscribe(kPerformanceMetricTopic,
+      &SensorUpdate::OnPerformanceMetric, this);
+  }
+
+  // Callback function for the performance metric topic.
+  protected: void OnPerformanceMetric(
+    const ignition::msgs::PerformanceSensorMetrics &_msg)
+  {
+    EXPECT_EQ(kSensorName, _msg.name());
+    performanceMetricsMsgsCount++;
+  }
+
+  // Sensor name.
+  protected: const std::string kSensorName{"sensor_test"};
+  // Sensor topic.
+  protected: const std::string kSensorTopic{"/sensor_test"};
+  // Enable metrics flag.
+  protected: const bool kEnableMetrics{true};
+  // Topic where performance metrics are published.
+  protected: const std::string kPerformanceMetricTopic{
+    kSensorTopic + "/performance_metrics"};
+  // Number of messages to be published.
+  protected: const unsigned int kNumberOfMessages{10};
+  // Counter for performance metrics messages published.
+  protected: std::atomic<unsigned int> performanceMetricsMsgsCount{0};
+  // Transport node.
+  protected: transport::Node node;
+};
+
+//////////////////////////////////////////////////
+TEST_F(SensorUpdate, Update)
+{
+  // Create sensor.
+  sdf::Sensor sdfSensor;
+  sdfSensor.SetName(kSensorName);
+  sdfSensor.SetTopic(kSensorTopic);
+  sdfSensor.SetEnableMetrics(kEnableMetrics);
+  std::unique_ptr<Sensor> sensor = std::make_unique<TestSensor>();
+  sensor->Load(sdfSensor);
+  ASSERT_EQ(kSensorTopic, sensor->Topic());
+  ASSERT_EQ(kEnableMetrics, sensor->EnableMetrics());
+
+  // Call Update() method kNumberOfMessages times.
+  // For each call there is also a call to Sensor::PublishMetrics() that
+  // publishes metrics in the correspondant topic.
+  for (int sec = 0 ; sec < static_cast<int>(kNumberOfMessages) ; ++sec)
+  {
+    common::Time now{sec, 0 /*nsec*/};
+    sensor->Update(now, true);
+  }
+
+  int sleep = 0;
+  const int maxSleep = 30;
+  while (performanceMetricsMsgsCount < kNumberOfMessages && sleep < maxSleep)
+  {
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    sleep++;
+  }
+  ASSERT_EQ(kNumberOfMessages, performanceMetricsMsgsCount);
+
+  auto testSensor = dynamic_cast<TestSensor*>(sensor.get());
+  EXPECT_EQ(testSensor->updateCount, performanceMetricsMsgsCount);
+}
+
 //////////////////////////////////////////////////
 int main(int argc, char **argv)
 {