3 ➡️ 4

Signed-off-by: Louise Poubel <[email protected]>

include/ignition/sensors/Sensor.hh

@@ -27,6 +27,7 @@
 #pragma warning(pop)
 #endif
 
+#include <chrono>
 #include <memory>
 #include <string>
 
@@ -188,6 +189,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;
@@ -222,6 +231,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/AirPressureSensor.cc

@@ -122,6 +122,9 @@ bool AirPressureSensor::Load(const sdf::Sensor &_sdf)
     return false;
   }
 
+  igndbg << "Air pressure for [" << this->Name() << "] advertised on ["
+         << this->Topic() << "]" << std::endl;
+
   // Load the noise parameters
   if (_sdf.AirPressureSensor()->PressureNoise().Type() != sdf::NoiseType::NONE)
   {

src/AltimeterSensor.cc

@@ -100,6 +100,9 @@ bool AltimeterSensor::Load(const sdf::Sensor &_sdf)
     return false;
   }
 
+  igndbg << "Altimeter data for [" << this->Name() << "] advertised on ["
+         << this->Topic() << "]" << std::endl;
+
   // Load the noise parameters
   if (_sdf.AltimeterSensor()->VerticalPositionNoise().Type()
       != sdf::NoiseType::NONE)

src/CameraSensor.cc

@@ -268,6 +268,9 @@ bool CameraSensor::Load(const sdf::Sensor &_sdf)
     return false;
   }
 
+  igndbg << "Camera images for [" << this->Name() << "] advertised on ["
+         << this->Topic() << "]" << std::endl;
+
   if (!this->AdvertiseInfo())
     return false;
 
@@ -507,16 +510,7 @@ bool CameraSensor::AdvertiseInfo()
   }
   this->dataPtr->infoTopic += "/camera_info";
 
-  this->dataPtr->infoPub =
-      this->dataPtr->node.Advertise<ignition::msgs::CameraInfo>(
-      this->dataPtr->infoTopic);
-  if (!this->dataPtr->infoPub)
-  {
-    ignerr << "Unable to create publisher on topic["
-      << this->dataPtr->infoTopic << "].\n";
-  }
-
-  return this->dataPtr->infoPub;
+  return this->AdvertiseInfo(this->dataPtr->infoTopic);
 }
 
 //////////////////////////////////////////////////
@@ -529,9 +523,14 @@ bool CameraSensor::AdvertiseInfo(const std::string &_topic)
       this->dataPtr->infoTopic);
   if (!this->dataPtr->infoPub)
   {
-    ignerr << "Unable to create publisher on topic["
+    ignerr << "Unable to create publisher on topic ["
       << this->dataPtr->infoTopic << "].\n";
   }
+  else
+  {
+    igndbg << "Camera info for [" << this->Name() << "] advertised on ["
+           << this->dataPtr->infoTopic << "]" << std::endl;
+  }
 
   return this->dataPtr->infoPub;
 }

src/DepthCameraSensor.cc

@@ -284,6 +284,9 @@ bool DepthCameraSensor::Load(const sdf::Sensor &_sdf)
     return false;
   }
 
+  igndbg << "Depth images for [" << this->Name() << "] advertised on ["
+         << this->Topic() << "]" << std::endl;
+
   if (!this->AdvertiseInfo())
     return false;
 
@@ -298,6 +301,9 @@ bool DepthCameraSensor::Load(const sdf::Sensor &_sdf)
     return false;
   }
 
+  igndbg << "Points for [" << this->Name() << "] advertised on ["
+         << this->Topic() << "/points]" << std::endl;
+
   // Initialize the point message.
   // \todo(anyone) The true value in the following function call forces
   // the xyz and rgb fields to be aligned to memory boundaries. This is need

src/GpuLidarSensor.cc

@@ -146,6 +146,9 @@ bool GpuLidarSensor::Load(const sdf::Sensor &_sdf)
     return false;
   }
 
+  igndbg << "Lidar points for [" << this->Name() << "] advertised on ["
+         << this->Topic() << "]" << std::endl;
+
   this->initialized = true;
 
   return true;

src/ImuSensor.cc

@@ -128,6 +128,9 @@ bool ImuSensor::Load(const sdf::Sensor &_sdf)
     return false;
   }
 
+  igndbg << "IMU data for [" << this->Name() << "] advertised on ["
+         << this->Topic() << "]" << std::endl;
+
   const std::map<SensorNoiseType, sdf::Noise> noises = {
     {ACCELEROMETER_X_NOISE_M_S_S, _sdf.ImuSensor()->LinearAccelerationXNoise()},
     {ACCELEROMETER_Y_NOISE_M_S_S, _sdf.ImuSensor()->LinearAccelerationYNoise()},

src/Lidar.cc

@@ -118,7 +118,9 @@ bool Lidar::Load(const sdf::Sensor &_sdf)
       << this->Topic() << "].\n";
     return false;
   }
-  ignmsg << "Publishing laser scans on [" << this->Topic() << "]" << std::endl;
+
+  igndbg << "Laser scans for [" << this->Name() << "] advertised on ["
+         << this->Topic() << "]" << std::endl;
 
   // Load ray atributes
   this->dataPtr->sdfLidar = *_sdf.LidarSensor();

src/LogicalCameraSensor.cc

@@ -117,6 +117,9 @@ bool LogicalCameraSensor::Load(sdf::ElementPtr _sdf)
     return false;
   }
 
+  igndbg << "Logical images for [" << this->Name() << "] advertised on ["
+         << this->Topic() << "]" << std::endl;
+
   this->dataPtr->initialized = true;
   return true;
 }

src/MagnetometerSensor.cc

@@ -114,6 +114,9 @@ bool MagnetometerSensor::Load(const sdf::Sensor &_sdf)
     return false;
   }
 
+  igndbg << "Magnetometer data for [" << this->Name() << "] advertised on ["
+         << this->Topic() << "]" << std::endl;
+
   // Load the noise parameters
   if (_sdf.MagnetometerSensor()->XNoise().Type() != sdf::NoiseType::NONE)
   {

src/RgbdCameraSensor.cc

@@ -211,6 +211,9 @@ bool RgbdCameraSensor::Load(const sdf::Sensor &_sdf)
     return false;
   }
 
+  igndbg << "RGB images for [" << this->Name() << "] advertised on ["
+         << this->Topic() << "/image]" << std::endl;
+
   // Create the depth image publisher
   this->dataPtr->depthPub =
       this->dataPtr->node.Advertise<ignition::msgs::Image>(
@@ -222,6 +225,9 @@ bool RgbdCameraSensor::Load(const sdf::Sensor &_sdf)
     return false;
   }
 
+  igndbg << "Depth images for [" << this->Name() << "] advertised on ["
+         << this->Topic() << "/depth_image]" << std::endl;
+
   // Create the point cloud publisher
   this->dataPtr->pointPub =
       this->dataPtr->node.Advertise<ignition::msgs::PointCloudPacked>(
@@ -233,6 +239,9 @@ bool RgbdCameraSensor::Load(const sdf::Sensor &_sdf)
     return false;
   }
 
+  igndbg << "Points for [" << this->Name() << "] advertised on ["
+         << this->Topic() << "/points]" << std::endl;
+
   if (!this->AdvertiseInfo(this->Topic() + "/camera_info"))
     return false;