Adds enable_metrics flag to Sensor.

include/sdf/Sensor.hh

@@ -159,6 +159,14 @@ namespace sdf
     /// \param[in] _topic Topic for this sensor's data.
     public: void 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 the pose of the sensor. This is the pose of the sensor
     /// as specified in SDF (<sensor> <pose> ... </pose></sensor>), and is
     /// typically used to express the position and rotation of a sensor in a

sdf/1.7/sensor.sdf

@@ -51,6 +51,10 @@
     <description>Name of the topic on which data is published. This is necessary for visualization</description>
   </element>
 
+  <element name="enable_metrics" type="bool" default="false" required="0">
+    <description>If true, the sensor will publish performance metrics</description>
+  </element>
+
   <include filename="pose.sdf" required="0"/>
   <include filename="plugin.sdf" required="*"/>
   <include filename="air_pressure.sdf" required="0"/>

src/Sensor.cc

@@ -73,6 +73,7 @@ class sdf::SensorPrivate
             pose(_sensor.pose),
             poseRelativeTo(_sensor.poseRelativeTo),
             sdf(_sensor.sdf),
+            enableMetrics(_sensor.enableMetrics),
             updateRate(_sensor.updateRate)
 
   {
@@ -131,6 +132,9 @@ class sdf::SensorPrivate
   /// \brief The SDF element pointer used during load.
   public: sdf::ElementPtr sdf;
 
+  /// \brief Performance metrics flag.
+  public: bool enableMetrics{false};
+
   /// \brief Name of xml parent object.
   public: std::string xmlParentName;
 
@@ -214,6 +218,7 @@ bool Sensor::operator==(const Sensor &_sensor) const
       this->Topic() != _sensor.Topic() ||
       this->RawPose() != _sensor.RawPose() ||
       this->PoseRelativeTo() != _sensor.PoseRelativeTo() ||
+      this->EnableMetrics() != _sensor.EnableMetrics() ||
       !ignition::math::equal(this->UpdateRate(), _sensor.UpdateRate()))
   {
     return false;
@@ -299,6 +304,8 @@ Errors Sensor::Load(ElementPtr _sdf)
   if (this->dataPtr->topic == "__default__")
     this->dataPtr->topic = "";
 
+  this->dataPtr->enableMetrics = _sdf->Get<bool>("enable_metrics",
+      this->dataPtr->enableMetrics).first;
   std::string type = _sdf->Get<std::string>("type");
   if (type == "air_pressure")
   {
@@ -518,6 +525,18 @@ void Sensor::SetPoseRelativeToGraph(
   this->dataPtr->poseRelativeToGraph = _graph;
 }
 
+/////////////////////////////////////////////////
+bool Sensor::EnableMetrics() const
+{
+  return this->dataPtr->enableMetrics;
+}
+
+/////////////////////////////////////////////////
+void Sensor::SetEnableMetrics(bool _enableMetrics)
+{
+  this->dataPtr->enableMetrics = _enableMetrics;
+}
+
 /////////////////////////////////////////////////
 sdf::SemanticPose Sensor::SemanticPose() const
 {

src/Sensor_TEST.cc

@@ -18,6 +18,7 @@
 #include <gtest/gtest.h>
 #include "sdf/Noise.hh"
 #include "sdf/Magnetometer.hh"
+#include "sdf/sdf.hh"
 #include "sdf/Sensor.hh"
 
 /////////////////////////////////////////////////
@@ -302,3 +303,30 @@ TEST(DOMSensor, Type)
     EXPECT_EQ(typeStrs[i], sensor.TypeStr());
   }
 }
+
+/////////////////////////////////////////////////
+TEST(DOMSensor, EnableMetrics)
+{
+  sdf::Sensor sensor;
+  // Verify default value.
+  EXPECT_EQ(false, sensor.EnableMetrics());
+
+  // Set up a simple sdf to test enable metrics option
+  std::ostringstream stream;
+  stream << "<sdf version='1.5'>"
+         << "  <model name='test_model'>"
+         << "    <sensor name='test_sensor' type='none'>"
+         << "      <enable_metrics>true</enable_metrics>"
+         << "    </sensor>"
+         << "  </model>"
+         << "</sdf>";
+  sdf::SDF sdfParsed;
+  sdfParsed.SetFromString(stream.str());
+
+  const sdf::ElementPtr sensorElem = sdfParsed.Root()->
+    GetElement("model")->GetElement("sensor");
+  sensor.Load(sensorElem);
+  EXPECT_EQ(true, sensor.EnableMetrics());
+  sensor.SetEnableMetrics(false);
+  EXPECT_EQ(false, sensor.EnableMetrics());
+}

test/integration/link_dom.cc

@@ -256,6 +256,7 @@ TEST(DOMLink, Sensors)
   EXPECT_EQ("altimeter_sensor", altimeterSensor->Name());
   EXPECT_EQ(sdf::SensorType::ALTIMETER, altimeterSensor->Type());
   EXPECT_EQ(ignition::math::Pose3d::Zero, altimeterSensor->RawPose());
+  EXPECT_FALSE(altimeterSensor->EnableMetrics());
   const sdf::Altimeter *altSensor = altimeterSensor->AltimeterSensor();
   ASSERT_NE(nullptr, altSensor);
   EXPECT_DOUBLE_EQ(0.1, altSensor->VerticalPositionNoise().Mean());
@@ -271,6 +272,7 @@ TEST(DOMLink, Sensors)
   EXPECT_EQ("camera_sensor", cameraSensor->Name());
   EXPECT_EQ(sdf::SensorType::CAMERA, cameraSensor->Type());
   EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, 0), cameraSensor->RawPose());
+  EXPECT_FALSE(cameraSensor->EnableMetrics());
   const sdf::Camera *camSensor = cameraSensor->CameraSensor();
   ASSERT_NE(nullptr, camSensor);
   EXPECT_EQ("my_camera", camSensor->Name());
@@ -319,13 +321,15 @@ TEST(DOMLink, Sensors)
   EXPECT_EQ(ignition::math::Pose3d(4, 5, 6, 0, 0, 0), pose);
   EXPECT_TRUE(contactSensor->SemanticPose().Resolve(pose).empty());
   EXPECT_EQ(ignition::math::Pose3d(4, 5, 3, 0, 0, 0), pose);
+  EXPECT_TRUE(contactSensor->EnableMetrics());
 
   // Get the depth sensor
   const sdf::Sensor *depthSensor = link->SensorByName("depth_sensor");
   ASSERT_NE(nullptr, depthSensor);
   EXPECT_EQ("depth_sensor", depthSensor->Name());
   EXPECT_EQ(sdf::SensorType::DEPTH_CAMERA, depthSensor->Type());
   EXPECT_EQ(ignition::math::Pose3d(7, 8, 9, 0, 0, 0), depthSensor->RawPose());
+  EXPECT_TRUE(depthSensor->EnableMetrics());
   const sdf::Camera *depthCamSensor = depthSensor->CameraSensor();
   ASSERT_NE(nullptr, depthCamSensor);
   EXPECT_EQ("my_depth_camera", depthCamSensor->Name());
@@ -336,6 +340,7 @@ TEST(DOMLink, Sensors)
   EXPECT_EQ("rgbd_sensor", rgbdSensor->Name());
   EXPECT_EQ(sdf::SensorType::RGBD_CAMERA, rgbdSensor->Type());
   EXPECT_EQ(ignition::math::Pose3d(37, 38, 39, 0, 0, 0), rgbdSensor->RawPose());
+  EXPECT_FALSE(rgbdSensor->EnableMetrics());
   const sdf::Camera *rgbdCamSensor = rgbdSensor->CameraSensor();
   ASSERT_NE(nullptr, rgbdCamSensor);
   EXPECT_EQ("my_rgbd_camera", rgbdCamSensor->Name());
@@ -347,6 +352,7 @@ TEST(DOMLink, Sensors)
   EXPECT_EQ(sdf::SensorType::THERMAL_CAMERA, thermalSensor->Type());
   EXPECT_EQ(ignition::math::Pose3d(37, 38, 39, 0, 0, 0),
             thermalSensor->RawPose());
+  EXPECT_FALSE(thermalSensor->EnableMetrics());
   const sdf::Camera *thermalCamSensor = thermalSensor->CameraSensor();
   ASSERT_NE(nullptr, thermalCamSensor);
   EXPECT_EQ("my_thermal_camera", thermalCamSensor->Name());
@@ -359,6 +365,7 @@ TEST(DOMLink, Sensors)
   EXPECT_EQ(sdf::SensorType::FORCE_TORQUE, forceTorqueSensor->Type());
   EXPECT_EQ(ignition::math::Pose3d(10, 11, 12, 0, 0, 0),
       forceTorqueSensor->RawPose());
+  EXPECT_FALSE(forceTorqueSensor->EnableMetrics());
 
   // Get the navsat sensor
   const sdf::Sensor *navSatSensor = link->SensorByName("navsat_sensor");
@@ -367,6 +374,7 @@ TEST(DOMLink, Sensors)
   EXPECT_EQ(sdf::SensorType::NAVSAT, navSatSensor->Type());
   EXPECT_EQ(ignition::math::Pose3d(13, 14, 15, 0, 0, 0),
       navSatSensor->RawPose());
+  EXPECT_FALSE(navSatSensor->EnableMetrics());
   const sdf::NavSat *navSatSensorObj = navSatSensor->NavSatSensor();
   ASSERT_NE(nullptr, navSatSensorObj);
 
@@ -385,6 +393,7 @@ TEST(DOMLink, Sensors)
   EXPECT_EQ("gpu_ray_sensor", gpuRaySensor->Name());
   EXPECT_EQ(sdf::SensorType::GPU_LIDAR, gpuRaySensor->Type());
   EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, 0), gpuRaySensor->RawPose());
+  EXPECT_FALSE(gpuRaySensor->EnableMetrics());
   const sdf::Lidar *gpuRay = gpuRaySensor->LidarSensor();
   ASSERT_NE(nullptr, gpuRay);
 
@@ -395,6 +404,7 @@ TEST(DOMLink, Sensors)
   EXPECT_EQ(sdf::SensorType::GPU_LIDAR, gpuLidarSensor->Type());
   EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, 0),
             gpuLidarSensor->RawPose());
+  EXPECT_FALSE(gpuLidarSensor->EnableMetrics());
   const sdf::Lidar *gpuLidar = gpuLidarSensor->LidarSensor();
   ASSERT_NE(nullptr, gpuLidar);
 
@@ -404,6 +414,7 @@ TEST(DOMLink, Sensors)
   EXPECT_EQ("imu_sensor", imuSensor->Name());
   EXPECT_EQ(sdf::SensorType::IMU, imuSensor->Type());
   EXPECT_EQ(ignition::math::Pose3d(4, 5, 6, 0, 0, 0), imuSensor->RawPose());
+  EXPECT_FALSE(imuSensor->EnableMetrics());
   const sdf::Imu *imuSensorObj = imuSensor->ImuSensor();
   ASSERT_NE(nullptr, imuSensorObj);
 
@@ -465,6 +476,7 @@ TEST(DOMLink, Sensors)
   EXPECT_EQ(sdf::SensorType::LOGICAL_CAMERA, logicalCameraSensor->Type());
   EXPECT_EQ(ignition::math::Pose3d(7, 8, 9, 0, 0, 0),
       logicalCameraSensor->RawPose());
+  EXPECT_FALSE(logicalCameraSensor->EnableMetrics());
 
   // Get the magnetometer sensor
   const sdf::Sensor *magnetometerSensor =
@@ -474,6 +486,7 @@ TEST(DOMLink, Sensors)
   EXPECT_EQ(sdf::SensorType::MAGNETOMETER, magnetometerSensor->Type());
   EXPECT_EQ(ignition::math::Pose3d(10, 11, 12, 0, 0, 0),
       magnetometerSensor->RawPose());
+  EXPECT_FALSE(magnetometerSensor->EnableMetrics());
   const sdf::Magnetometer *magSensor = magnetometerSensor->MagnetometerSensor();
   ASSERT_NE(nullptr, magSensor);
   EXPECT_DOUBLE_EQ(0.1, magSensor->XNoise().Mean());
@@ -491,13 +504,15 @@ TEST(DOMLink, Sensors)
   EXPECT_EQ(sdf::SensorType::MULTICAMERA, multicameraSensor->Type());
   EXPECT_EQ(ignition::math::Pose3d(13, 14, 15, 0, 0, 0),
       multicameraSensor->RawPose());
+  EXPECT_FALSE(multicameraSensor->EnableMetrics());
 
   // Get the ray sensor
   const sdf::Sensor *raySensor = link->SensorByName("ray_sensor");
   ASSERT_NE(nullptr, raySensor);
   EXPECT_EQ("ray_sensor", raySensor->Name());
   EXPECT_EQ(sdf::SensorType::LIDAR, raySensor->Type());
   EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, 0), raySensor->RawPose());
+  EXPECT_FALSE(raySensor->EnableMetrics());
   const sdf::Lidar *ray = raySensor->LidarSensor();
   ASSERT_NE(nullptr, ray);
   EXPECT_EQ(320u, ray->HorizontalScanSamples());
@@ -520,6 +535,7 @@ TEST(DOMLink, Sensors)
   EXPECT_EQ("lidar_sensor", lidarSensor->Name());
   EXPECT_EQ(sdf::SensorType::LIDAR, lidarSensor->Type());
   EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, 0), lidarSensor->RawPose());
+  EXPECT_TRUE(lidarSensor->EnableMetrics());
   const sdf::Lidar *lidar = lidarSensor->LidarSensor();
   ASSERT_NE(nullptr, lidar);
   EXPECT_EQ(320u, lidar->HorizontalScanSamples());
@@ -542,13 +558,15 @@ TEST(DOMLink, Sensors)
   EXPECT_EQ("rfid_sensor", rfidSensor->Name());
   EXPECT_EQ(sdf::SensorType::RFID, rfidSensor->Type());
   EXPECT_EQ(ignition::math::Pose3d(4, 5, 6, 0, 0, 0), rfidSensor->RawPose());
+  EXPECT_FALSE(rfidSensor->EnableMetrics());
 
   // Get the rfid tag
   const sdf::Sensor *rfidTag = link->SensorByName("rfid_tag");
   ASSERT_NE(nullptr, rfidTag);
   EXPECT_EQ("rfid_tag", rfidTag->Name());
   EXPECT_EQ(sdf::SensorType::RFIDTAG, rfidTag->Type());
   EXPECT_EQ(ignition::math::Pose3d(7, 8, 9, 0, 0, 0), rfidTag->RawPose());
+  EXPECT_FALSE(rfidTag->EnableMetrics());
 
   // Get the sonar sensor
   const sdf::Sensor *sonarSensor = link->SensorByName("sonar_sensor");
@@ -557,6 +575,7 @@ TEST(DOMLink, Sensors)
   EXPECT_EQ(sdf::SensorType::SONAR, sonarSensor->Type());
   EXPECT_EQ(ignition::math::Pose3d(10, 11, 12, 0, 0, 0),
             sonarSensor->RawPose());
+  EXPECT_FALSE(sonarSensor->EnableMetrics());
 
   // Get the wireless receiver
   const sdf::Sensor *wirelessReceiver = link->SensorByName("wireless_receiver");
@@ -565,6 +584,7 @@ TEST(DOMLink, Sensors)
   EXPECT_EQ(sdf::SensorType::WIRELESS_RECEIVER, wirelessReceiver->Type());
   EXPECT_EQ(ignition::math::Pose3d(13, 14, 15, 0, 0, 0),
       wirelessReceiver->RawPose());
+  EXPECT_FALSE(wirelessReceiver->EnableMetrics());
 
   // Get the wireless transmitter
   const sdf::Sensor *wirelessTransmitter =
@@ -574,6 +594,7 @@ TEST(DOMLink, Sensors)
   EXPECT_EQ(sdf::SensorType::WIRELESS_TRANSMITTER, wirelessTransmitter->Type());
   EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, 0),
       wirelessTransmitter->RawPose());
+  EXPECT_FALSE(wirelessTransmitter->EnableMetrics());
 
   // Get the air_pressure sensor
   const sdf::Sensor *airPressureSensor = link->SensorByName(
@@ -583,6 +604,7 @@ TEST(DOMLink, Sensors)
   EXPECT_EQ(sdf::SensorType::AIR_PRESSURE, airPressureSensor->Type());
   EXPECT_EQ(ignition::math::Pose3d(10, 20, 30, 0, 0, 0),
       airPressureSensor->RawPose());
+  EXPECT_FALSE(airPressureSensor->EnableMetrics());
   const sdf::AirPressure *airSensor = airPressureSensor->AirPressureSensor();
   ASSERT_NE(nullptr, airSensor);
   EXPECT_DOUBLE_EQ(3.4, airSensor->PressureNoise().Mean());

test/sdf/sensors.sdf

@@ -22,6 +22,7 @@
 
       <sensor name="camera_sensor" type="camera">
         <pose>1 2 3 0 0 0</pose>
+        <enable_metrics>false</enable_metrics>
         <camera name="my_camera">
           <pose>0.1 0.2 0.3 0 0 0</pose>
           <horizontal_fov>.75</horizontal_fov>
@@ -78,10 +79,12 @@
 
       <sensor name="contact_sensor" type="contact">
         <pose relative_to="__model__">4 5 6 0 0 0</pose>
+        <enable_metrics>true</enable_metrics>
       </sensor>
 
       <sensor name="depth_sensor" type="depth">
         <pose>7 8 9 0 0 0</pose>
+        <enable_metrics>true</enable_metrics>
         <camera name="my_depth_camera">
           <pose>0.1 0.2 0.3 0 0 0</pose>
           <horizontal_fov>.75</horizontal_fov>
@@ -310,6 +313,7 @@
 
       <sensor name="lidar_sensor" type="lidar">
         <pose>1 2 3 0 0 0</pose>
+        <enable_metrics>true</enable_metrics>
         <lidar>
           <scan>
             <horizontal>