Publish performance sensor metrics.

include/ignition/sensors/Sensor.hh

@@ -180,6 +180,13 @@ 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().
+      ///        When different metrics need to be published this method
+      ///        could be overriden by subclasses.
+      /// \param[in] _simTime Current time.
+      public: virtual void PublishMetrics(const ignition::common::Time &_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 ignition::common::Time& _now);
+
   /// \brief id given to sensor when constructed
   public: SensorId id;
 
@@ -61,6 +66,18 @@ class ignition::sensors::SensorPrivate
   /// \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> lastClockTime;
+
+  /// \brief Last sim time at Update call.
+  public: ignition::common::Time 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;
 
@@ -196,6 +213,55 @@ bool SensorPrivate::SetTopic(const std::string &_topic)
   return true;
 }
 
+//////////////////////////////////////////////////
+void Sensor::PublishMetrics(const ignition::common::Time &_now)
+{
+  return this->dataPtr->PublishMetrics(_now);
+}
+
+void SensorPrivate::PublishMetrics(const ignition::common::Time &_now) {
+  if(!this->performanceSensorMetricsPub)
+  {
+    this->performanceSensorMetricsPub =
+      node.Advertise<msgs::PerformanceSensorMetrics>(
+          this->topic + "/performance_metrics");
+  }
+  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(this->lastUpdateTime > 0)
+  {
+    const double diffSimUpdate = _now.Double() -
+      this->lastUpdateTime.Double();
+    simUpdateRate = 1.0 / diffSimUpdate;
+    const double diffRealUpdate =
+      std::chrono::duration_cast<std::chrono::milliseconds>(
+        clockNow - this->lastClockTime).count() * 1e-3;
+    realUpdateRate = 1.0 / diffRealUpdate;
+  }
+
+  // Update last time values.
+  this->lastUpdateTime = _now;
+  this->lastClockTime = 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 +322,9 @@ bool Sensor::Update(const ignition::common::Time &_now,
   // Make the update happen
   result = this->Update(_now);
 
+  // Publish metrics
+  this->PublishMetrics(_now);
+
   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,72 @@ 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"};
+  // 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);
+  std::unique_ptr<Sensor> sensor = std::make_unique<TestSensor>();
+  sensor->Load(sdfSensor);
+  ASSERT_EQ(kSensorTopic, sensor->Topic());
+
+  // 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)
 {