👩‍🌾 Print debug messages when sensors advertise topics (#151)

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

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

@@ -267,6 +267,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;
 
@@ -500,16 +503,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);
 }
 
 //////////////////////////////////////////////////
@@ -522,9 +516,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

@@ -124,6 +124,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;
 

src/ThermalCameraSensor.cc

@@ -214,6 +214,9 @@ bool ThermalCameraSensor::Load(const sdf::Sensor &_sdf)
     return false;
   }
 
+  igndbg << "Thermal images for [" << this->Name() << "] advertised on ["
+         << this->Topic() << "]" << std::endl;
+
   if (!this->AdvertiseInfo())
     return false;