From 6aef65723d74dec3a77aa6fe69e8d11193827d4e Mon Sep 17 00:00:00 2001
From: Franco Cipollone <franco.c@ekumenlabs.com>
Date: Wed, 11 Aug 2021 23:47:12 -0300
Subject: [PATCH] Enable metrics from sdf.

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
---
 include/ignition/sensors/Sensor.hh |  8 ++++++++
 src/Sensor.cc                      | 23 ++++++++++++++++++++++-
 src/Sensor_TEST.cc                 |  4 ++++
 3 files changed, 34 insertions(+), 1 deletion(-)

diff --git a/include/ignition/sensors/Sensor.hh b/include/ignition/sensors/Sensor.hh
index 010469ba..c79cd8a3 100644
--- a/include/ignition/sensors/Sensor.hh
+++ b/include/ignition/sensors/Sensor.hh
@@ -147,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;
diff --git a/src/Sensor.cc b/src/Sensor.cc
index 175900d9..14df12fb 100644
--- a/src/Sensor.cc
+++ b/src/Sensor.cc
@@ -60,6 +60,9 @@ 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;
 
@@ -127,6 +130,8 @@ bool SensorPrivate::PopulateFromSDF(const sdf::Sensor &_sdf)
   }
 
   this->updateRate = _sdf.UpdateRate();
+
+  this->enableMetrics = _sdf.EnableMetrics();
   return true;
 }
 
@@ -213,6 +218,19 @@ 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)
 {
@@ -334,7 +352,10 @@ bool Sensor::Update(const ignition::common::Time &_now,
   result = this->Update(_now);
 
   // Publish metrics
-  this->PublishMetrics(std::chrono::duration<double>(_now.Double()));
+  if (this->EnableMetrics())
+  {
+    this->PublishMetrics(std::chrono::duration<double>(_now.Double()));
+  }
 
   if (!_force && this->dataPtr->updateRate > 0.0)
   {
diff --git a/src/Sensor_TEST.cc b/src/Sensor_TEST.cc
index 88f9414a..d9157ec8 100644
--- a/src/Sensor_TEST.cc
+++ b/src/Sensor_TEST.cc
@@ -162,6 +162,8 @@ class SensorUpdate : public ::testing::Test
   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"};
@@ -180,9 +182,11 @@ TEST_F(SensorUpdate, Update)
   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