diff --git a/src/AirPressureSensor.cc b/src/AirPressureSensor.cc index 7a0e733a..f56362d3 100644 --- a/src/AirPressureSensor.cc +++ b/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) { diff --git a/src/AltimeterSensor.cc b/src/AltimeterSensor.cc index c5d75890..0689c83c 100644 --- a/src/AltimeterSensor.cc +++ b/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) diff --git a/src/CameraSensor.cc b/src/CameraSensor.cc index c8f8a3f4..80968b0e 100644 --- a/src/CameraSensor.cc +++ b/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( - 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; } diff --git a/src/DepthCameraSensor.cc b/src/DepthCameraSensor.cc index 2f3a9c5b..06dadb8e 100644 --- a/src/DepthCameraSensor.cc +++ b/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 diff --git a/src/GpuLidarSensor.cc b/src/GpuLidarSensor.cc index dfcceb40..e1c2bedb 100644 --- a/src/GpuLidarSensor.cc +++ b/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; diff --git a/src/ImuSensor.cc b/src/ImuSensor.cc index 638ed22d..8427f717 100644 --- a/src/ImuSensor.cc +++ b/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 noises = { {ACCELEROMETER_X_NOISE_M_S_S, _sdf.ImuSensor()->LinearAccelerationXNoise()}, {ACCELEROMETER_Y_NOISE_M_S_S, _sdf.ImuSensor()->LinearAccelerationYNoise()}, diff --git a/src/Lidar.cc b/src/Lidar.cc index 84a49771..b34e649b 100644 --- a/src/Lidar.cc +++ b/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(); diff --git a/src/LogicalCameraSensor.cc b/src/LogicalCameraSensor.cc index 82579baf..ad647f91 100644 --- a/src/LogicalCameraSensor.cc +++ b/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; } diff --git a/src/MagnetometerSensor.cc b/src/MagnetometerSensor.cc index c227f1ad..5bc375b9 100644 --- a/src/MagnetometerSensor.cc +++ b/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) { diff --git a/src/RgbdCameraSensor.cc b/src/RgbdCameraSensor.cc index c2a766f9..3a04045f 100644 --- a/src/RgbdCameraSensor.cc +++ b/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( @@ -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( @@ -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; diff --git a/src/ThermalCameraSensor.cc b/src/ThermalCameraSensor.cc index 744b5d9b..a25da365 100644 --- a/src/ThermalCameraSensor.cc +++ b/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;