From 9f77eaa0a654a4aba420fa53bee9e8c62a7214a8 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 9 Aug 2022 19:50:15 -0700 Subject: [PATCH 1/9] Remove redundant namespace references (#258) Signed-off-by: methylDragon --- src/AirPressureSensor.cc | 6 +- src/AltimeterSensor.cc | 4 +- src/CameraSensor.cc | 46 +++++------ src/DepthCameraSensor.cc | 30 +++---- src/GaussianNoiseModel.cc | 12 +-- src/ImuSensor.cc | 18 ++--- src/ImuSensor_TEST.cc | 12 +-- src/LogicalCameraSensor.cc | 12 +-- src/MagnetometerSensor.cc | 10 +-- src/Manager.cc | 10 +-- src/Noise_TEST.cc | 4 +- src/RgbdCameraSensor.cc | 18 ++--- src/SensorFactory.cc | 2 +- src/SensorTypes.cc | 6 +- src/Sensor_TEST.cc | 10 +-- src/ThermalCameraSensor.cc | 18 ++--- test/integration/rgbd_camera_plugin.cc | 106 ++++++++++++------------- 17 files changed, 162 insertions(+), 162 deletions(-) diff --git a/src/AirPressureSensor.cc b/src/AirPressureSensor.cc index baafc46f..c91df95d 100644 --- a/src/AirPressureSensor.cc +++ b/src/AirPressureSensor.cc @@ -113,7 +113,7 @@ bool AirPressureSensor::Load(const sdf::Sensor &_sdf) this->SetTopic("/air_pressure"); this->dataPtr->pub = - this->dataPtr->node.Advertise( + this->dataPtr->node.Advertise( this->Topic()); if (!this->dataPtr->pub) @@ -145,7 +145,7 @@ bool AirPressureSensor::Load(sdf::ElementPtr _sdf) } ////////////////////////////////////////////////// -bool AirPressureSensor::Update(const ignition::common::Time &_now) +bool AirPressureSensor::Update(const common::Time &_now) { IGN_PROFILE("AirPressureSensor::Update"); if (!this->dataPtr->initialized) @@ -193,7 +193,7 @@ bool AirPressureSensor::Update(const ignition::common::Time &_now) NoiseType::GAUSSIAN) { GaussianNoiseModelPtr gaussian = - std::dynamic_pointer_cast( + std::dynamic_pointer_cast( this->dataPtr->noises[AIR_PRESSURE_NOISE_PASCALS]); msg.set_variance(sqrt(gaussian->StdDev())); } diff --git a/src/AltimeterSensor.cc b/src/AltimeterSensor.cc index 2297263c..9d8e067b 100644 --- a/src/AltimeterSensor.cc +++ b/src/AltimeterSensor.cc @@ -92,7 +92,7 @@ bool AltimeterSensor::Load(const sdf::Sensor &_sdf) this->SetTopic("/altimeter"); this->dataPtr->pub = - this->dataPtr->node.Advertise(this->Topic()); + this->dataPtr->node.Advertise(this->Topic()); if (!this->dataPtr->pub) { @@ -133,7 +133,7 @@ bool AltimeterSensor::Load(sdf::ElementPtr _sdf) } ////////////////////////////////////////////////// -bool AltimeterSensor::Update(const ignition::common::Time &_now) +bool AltimeterSensor::Update(const common::Time &_now) { IGN_PROFILE("AltimeterSensor::Update"); if (!this->dataPtr->initialized) diff --git a/src/CameraSensor.cc b/src/CameraSensor.cc index 779e8f56..edb1f4e2 100644 --- a/src/CameraSensor.cc +++ b/src/CameraSensor.cc @@ -59,7 +59,7 @@ class ignition::sensors::CameraSensorPrivate /// of the path was not possible. /// \sa ImageSaver public: bool SaveImage(const unsigned char *_data, unsigned int _width, - unsigned int _height, ignition::common::Image::PixelFormatType _format); + unsigned int _height, common::Image::PixelFormatType _format); /// \brief node to create publisher public: transport::Node node; @@ -74,21 +74,21 @@ class ignition::sensors::CameraSensorPrivate public: bool initialized = false; /// \brief Rendering camera - public: ignition::rendering::CameraPtr camera; + public: rendering::CameraPtr camera; /// \brief Pointer to an image to be published - public: ignition::rendering::Image image; + public: rendering::Image image; /// \brief Noise added to sensor data public: std::map noises; /// \brief Event that is used to trigger callbacks when a new image /// is generated - public: ignition::common::EventT< - void(const ignition::msgs::Image &)> imageEvent; + public: common::EventT< + void(const msgs::Image &)> imageEvent; /// \brief Connection to the Manager's scene change event. - public: ignition::common::ConnectionPtr sceneChangeConnection; + public: common::ConnectionPtr sceneChangeConnection; /// \brief Just a mutex for thread safety public: std::mutex mutex; @@ -188,7 +188,7 @@ bool CameraSensor::CreateCamera() switch (pixelFormat) { case sdf::PixelFormatType::RGB_INT8: - this->dataPtr->camera->SetImageFormat(ignition::rendering::PF_R8G8B8); + this->dataPtr->camera->SetImageFormat(rendering::PF_R8G8B8); break; default: ignerr << "Unsupported pixel format [" @@ -258,7 +258,7 @@ bool CameraSensor::Load(const sdf::Sensor &_sdf) this->SetTopic("/camera"); this->dataPtr->pub = - this->dataPtr->node.Advertise( + this->dataPtr->node.Advertise( this->Topic()); if (!this->dataPtr->pub) { @@ -293,14 +293,14 @@ bool CameraSensor::Load(sdf::ElementPtr _sdf) } ///////////////////////////////////////////////// -ignition::common::ConnectionPtr CameraSensor::ConnectImageCallback( - std::function _callback) +common::ConnectionPtr CameraSensor::ConnectImageCallback( + std::function _callback) { return this->dataPtr->imageEvent.Connect(_callback); } ///////////////////////////////////////////////// -void CameraSensor::SetScene(ignition::rendering::ScenePtr _scene) +void CameraSensor::SetScene(rendering::ScenePtr _scene) { std::lock_guard lock(this->dataPtr->mutex); // APIs make it possible for the scene pointer to change @@ -315,7 +315,7 @@ void CameraSensor::SetScene(ignition::rendering::ScenePtr _scene) } ////////////////////////////////////////////////// -bool CameraSensor::Update(const ignition::common::Time &_now) +bool CameraSensor::Update(const common::Time &_now) { IGN_PROFILE("CameraSensor::Update"); if (!this->dataPtr->initialized) @@ -370,15 +370,15 @@ bool CameraSensor::Update(const ignition::common::Time &_now) unsigned int height = this->dataPtr->camera->ImageHeight(); unsigned char *data = this->dataPtr->image.Data(); - ignition::common::Image::PixelFormatType + common::Image::PixelFormatType format{common::Image::UNKNOWN_PIXEL_FORMAT}; msgs::PixelFormatType msgsPixelFormat = msgs::PixelFormatType::UNKNOWN_PIXEL_FORMAT; switch (this->dataPtr->camera->ImageFormat()) { - case ignition::rendering::PF_R8G8B8: - format = ignition::common::Image::RGB_INT8; + case rendering::PF_R8G8B8: + format = common::Image::RGB_INT8; msgsPixelFormat = msgs::PixelFormatType::RGB_INT8; break; default: @@ -388,7 +388,7 @@ bool CameraSensor::Update(const ignition::common::Time &_now) } // create message - ignition::msgs::Image msg; + msgs::Image msg; { IGN_PROFILE("CameraSensor::Update Message"); msg.set_width(width); @@ -439,12 +439,12 @@ bool CameraSensor::Update(const ignition::common::Time &_now) ////////////////////////////////////////////////// bool CameraSensorPrivate::SaveImage(const unsigned char *_data, unsigned int _width, unsigned int _height, - ignition::common::Image::PixelFormatType _format) + common::Image::PixelFormatType _format) { // Attempt to create the directory if it doesn't exist - if (!ignition::common::isDirectory(this->saveImagePath)) + if (!common::isDirectory(this->saveImagePath)) { - if (!ignition::common::createDirectories(this->saveImagePath)) + if (!common::createDirectories(this->saveImagePath)) return false; } @@ -452,11 +452,11 @@ bool CameraSensorPrivate::SaveImage(const unsigned char *_data, std::to_string(this->saveImageCounter) + ".png"; ++this->saveImageCounter; - ignition::common::Image localImage; + common::Image localImage; localImage.SetFromData(_data, _width, _height, _format); localImage.SavePNG( - ignition::common::joinPaths(this->saveImagePath, filename)); + common::joinPaths(this->saveImagePath, filename)); return true; } @@ -512,7 +512,7 @@ bool CameraSensor::AdvertiseInfo(const std::string &_topic) this->dataPtr->infoTopic = _topic; this->dataPtr->infoPub = - this->dataPtr->node.Advertise( + this->dataPtr->node.Advertise( this->dataPtr->infoTopic); if (!this->dataPtr->infoPub) { @@ -529,7 +529,7 @@ bool CameraSensor::AdvertiseInfo(const std::string &_topic) } ////////////////////////////////////////////////// -void CameraSensor::PublishInfo(const ignition::common::Time &_now) +void CameraSensor::PublishInfo(const common::Time &_now) { this->dataPtr->infoMsg.mutable_header()->mutable_stamp()->set_sec(_now.sec); this->dataPtr->infoMsg.mutable_header()->mutable_stamp()->set_nsec( diff --git a/src/DepthCameraSensor.cc b/src/DepthCameraSensor.cc index 33caedb5..8365317e 100644 --- a/src/DepthCameraSensor.cc +++ b/src/DepthCameraSensor.cc @@ -178,19 +178,19 @@ bool DepthCameraSensorPrivate::ConvertDepthToImage( ////////////////////////////////////////////////// bool DepthCameraSensorPrivate::SaveImage(const float *_data, unsigned int _width, unsigned int _height, - ignition::common::Image::PixelFormatType /*_format*/) + common::Image::PixelFormatType /*_format*/) { // Attempt to create the directory if it doesn't exist - if (!ignition::common::isDirectory(this->saveImagePath)) + if (!common::isDirectory(this->saveImagePath)) { - if (!ignition::common::createDirectories(this->saveImagePath)) + if (!common::createDirectories(this->saveImagePath)) return false; } if (_width == 0 || _height == 0) return false; - ignition::common::Image localImage; + common::Image localImage; unsigned int depthSamples = _width * _height; unsigned int depthBufferSize = depthSamples * 3; @@ -206,7 +206,7 @@ bool DepthCameraSensorPrivate::SaveImage(const float *_data, localImage.SetFromData(imgDepthBuffer, _width, _height, common::Image::RGB_INT8); localImage.SavePNG( - ignition::common::joinPaths(this->saveImagePath, filename)); + common::joinPaths(this->saveImagePath, filename)); delete[] imgDepthBuffer; return true; @@ -275,7 +275,7 @@ bool DepthCameraSensor::Load(const sdf::Sensor &_sdf) this->SetTopic("/camera/depth"); this->dataPtr->pub = - this->dataPtr->node.Advertise( + this->dataPtr->node.Advertise( this->Topic()); if (!this->dataPtr->pub) { @@ -292,7 +292,7 @@ bool DepthCameraSensor::Load(const sdf::Sensor &_sdf) // Create the point cloud publisher this->dataPtr->pointPub = - this->dataPtr->node.Advertise( + this->dataPtr->node.Advertise( this->Topic() + "/points"); if (!this->dataPtr->pointPub) { @@ -445,8 +445,8 @@ void DepthCameraSensor::OnNewDepthFrame(const float *_scan, unsigned int depthSamples = _width * _height; unsigned int depthBufferSize = depthSamples * sizeof(float); - ignition::common::Image::PixelFormatType format = - ignition::common::Image::ConvertPixelFormat(_format); + common::Image::PixelFormatType format = + common::Image::ConvertPixelFormat(_format); if (!this->dataPtr->depthBuffer) this->dataPtr->depthBuffer = new float[depthSamples]; @@ -480,20 +480,20 @@ void DepthCameraSensor::OnNewRgbPointCloud(const float *_scan, } ///////////////////////////////////////////////// -ignition::rendering::DepthCameraPtr DepthCameraSensor::DepthCamera() +rendering::DepthCameraPtr DepthCameraSensor::DepthCamera() { return this->dataPtr->depthCamera; } ///////////////////////////////////////////////// -ignition::common::ConnectionPtr DepthCameraSensor::ConnectImageCallback( - std::function _callback) +common::ConnectionPtr DepthCameraSensor::ConnectImageCallback( + std::function _callback) { return this->dataPtr->imageEvent.Connect(_callback); } ///////////////////////////////////////////////// -void DepthCameraSensor::SetScene(ignition::rendering::ScenePtr _scene) +void DepthCameraSensor::SetScene(rendering::ScenePtr _scene) { std::lock_guard lock(this->dataPtr->mutex); // APIs make it possible for the scene pointer to change @@ -509,7 +509,7 @@ void DepthCameraSensor::SetScene(ignition::rendering::ScenePtr _scene) } ////////////////////////////////////////////////// -bool DepthCameraSensor::Update(const ignition::common::Time &_now) +bool DepthCameraSensor::Update(const common::Time &_now) { IGN_PROFILE("DepthCameraSensor::Update"); if (!this->dataPtr->initialized) @@ -533,7 +533,7 @@ bool DepthCameraSensor::Update(const ignition::common::Time &_now) auto msgsFormat = msgs::PixelFormatType::R_FLOAT32; // create message - ignition::msgs::Image msg; + msgs::Image msg; msg.set_width(width); msg.set_height(height); msg.set_step(width * rendering::PixelUtil::BytesPerPixel( diff --git a/src/GaussianNoiseModel.cc b/src/GaussianNoiseModel.cc index 5e8b9667..f3343022 100644 --- a/src/GaussianNoiseModel.cc +++ b/src/GaussianNoiseModel.cc @@ -87,12 +87,12 @@ void GaussianNoiseModel::Load(const sdf::Noise &_sdf) double biasStdDev = 0; biasMean = _sdf.BiasMean(); biasStdDev = _sdf.BiasStdDev(); - this->dataPtr->bias = ignition::math::Rand::DblNormal(biasMean, biasStdDev); + this->dataPtr->bias = math::Rand::DblNormal(biasMean, biasStdDev); // With equal probability, we pick a negative bias (by convention, // rateBiasMean should be positive, though it would work fine if // negative). - if (ignition::math::Rand::DblUniform() < 0.5) + if (math::Rand::DblUniform() < 0.5) this->dataPtr->bias = -this->dataPtr->bias; this->Print(out); @@ -100,7 +100,7 @@ void GaussianNoiseModel::Load(const sdf::Noise &_sdf) this->dataPtr->precision = _sdf.Precision(); if (this->dataPtr->precision < 0) ignerr << "Noise precision cannot be less than 0" << std::endl; - else if (!ignition::math::equal(this->dataPtr->precision, 0.0, 1e-6)) + else if (!math::equal(this->dataPtr->precision, 0.0, 1e-6)) this->dataPtr->quantized = true; } @@ -108,7 +108,7 @@ void GaussianNoiseModel::Load(const sdf::Noise &_sdf) double GaussianNoiseModel::ApplyImpl(double _in, double _dt) { // Generate independent (uncorrelated) Gaussian noise to each input value. - double whiteNoise = ignition::math::Rand::DblNormal( + double whiteNoise = math::Rand::DblNormal( this->dataPtr->mean, this->dataPtr->stdDev); // Generate varying (correlated) bias to each input value. @@ -132,7 +132,7 @@ double GaussianNoiseModel::ApplyImpl(double _in, double _dt) tau / 2 * expm1(-2 * _dt / tau)); double phi_d = exp(-_dt / tau); this->dataPtr->bias = phi_d * this->dataPtr->bias + - ignition::math::Rand::DblNormal(0, sigma_b_d); + math::Rand::DblNormal(0, sigma_b_d); } double output = _in + this->dataPtr->bias + whiteNoise; @@ -140,7 +140,7 @@ double GaussianNoiseModel::ApplyImpl(double _in, double _dt) if (this->dataPtr->quantized) { // Apply this->dataPtr->precision - if (!ignition::math::equal(this->dataPtr->precision, 0.0, 1e-6)) + if (!math::equal(this->dataPtr->precision, 0.0, 1e-6)) { output = std::round(output / this->dataPtr->precision) * this->dataPtr->precision; diff --git a/src/ImuSensor.cc b/src/ImuSensor.cc index f2d066d3..702220f6 100644 --- a/src/ImuSensor.cc +++ b/src/ImuSensor.cc @@ -48,28 +48,28 @@ class ignition::sensors::ImuSensorPrivate public: bool initialized = false; /// \brief Noise free linear acceleration - public: ignition::math::Vector3d linearAcc; + public: math::Vector3d linearAcc; /// \brief Noise free angular velocity. - public: ignition::math::Vector3d angularVel; + public: math::Vector3d angularVel; /// \brief transform to Imu orientation reference frame. - public: ignition::math::Quaterniond orientationReference; + public: math::Quaterniond orientationReference; /// \brief transform to Imu frame from Imu reference frame. - public: ignition::math::Quaterniond orientation; + public: math::Quaterniond orientation; /// \brief store gravity vector to be added to the IMU output. - public: ignition::math::Vector3d gravity; + public: math::Vector3d gravity; /// \brief World pose of the imu sensor - public: ignition::math::Pose3d worldPose; + public: math::Pose3d worldPose; /// \brief Flag for if time has been initialized public: bool timeInitialized = false; /// \brief Previous update time step. - public: ignition::common::Time prevStep { ignition::common::Time::Zero }; + public: common::Time prevStep { common::Time::Zero }; /// \brief Noise added to sensor data public: std::map noises; @@ -116,7 +116,7 @@ bool ImuSensor::Load(const sdf::Sensor &_sdf) this->SetTopic("/imu"); this->dataPtr->pub = - this->dataPtr->node.Advertise(this->Topic()); + this->dataPtr->node.Advertise(this->Topic()); if (!this->dataPtr->pub) { @@ -157,7 +157,7 @@ bool ImuSensor::Load(sdf::ElementPtr _sdf) } ////////////////////////////////////////////////// -bool ImuSensor::Update(const ignition::common::Time &_now) +bool ImuSensor::Update(const common::Time &_now) { IGN_PROFILE("ImuSensor::Update"); if (!this->dataPtr->initialized) diff --git a/src/ImuSensor_TEST.cc b/src/ImuSensor_TEST.cc index e4db50e5..5eda3801 100644 --- a/src/ImuSensor_TEST.cc +++ b/src/ImuSensor_TEST.cc @@ -187,7 +187,7 @@ class ImuSensor_TEST : public ::testing::Test // Documentation inherited protected: void SetUp() override { - ignition::common::Console::SetVerbosity(4); + common::Console::SetVerbosity(4); } }; @@ -195,7 +195,7 @@ class ImuSensor_TEST : public ::testing::Test TEST(ImuSensor_TEST, CreateImuSensor) { // Create a sensor manager - ignition::sensors::Manager mgr; + sensors::Manager mgr; const std::string name = "TestImu"; const std::string topic = "/ignition/sensors/test/imu"; @@ -209,7 +209,7 @@ TEST(ImuSensor_TEST, CreateImuSensor) accelNoise, gyroNoise, always_on, visualize); // Create an ImuSensor - auto sensor = mgr.CreateSensor(imuSDF); + auto sensor = mgr.CreateSensor(imuSDF); // Make sure the above dynamic cast worked. EXPECT_TRUE(sensor != nullptr); @@ -219,7 +219,7 @@ TEST(ImuSensor_TEST, CreateImuSensor) TEST(ImuSensor_TEST, ComputeNoise) { // Create a sensor manager - ignition::sensors::Manager mgr; + sensors::Manager mgr; sdf::ElementPtr imuSDF, imuSDF_truth; @@ -253,9 +253,9 @@ TEST(ImuSensor_TEST, ComputeNoise) } // Create an ImuSensor - auto sensor_truth = mgr.CreateSensor( + auto sensor_truth = mgr.CreateSensor( imuSDF_truth); - auto sensor = mgr.CreateSensor(imuSDF); + auto sensor = mgr.CreateSensor(imuSDF); // Make sure the above dynamic cast worked. ASSERT_NE(nullptr, sensor); diff --git a/src/LogicalCameraSensor.cc b/src/LogicalCameraSensor.cc index 7fa1af39..a99fc548 100644 --- a/src/LogicalCameraSensor.cc +++ b/src/LogicalCameraSensor.cc @@ -47,7 +47,7 @@ class ignition::sensors::LogicalCameraSensorPrivate public: std::mutex mutex; /// \brief Camera frustum. - public: ignition::math::Frustum frustum; + public: math::Frustum frustum; /// \brief Set world pose. public: math::Pose3d worldPose; @@ -56,7 +56,7 @@ class ignition::sensors::LogicalCameraSensorPrivate public: std::map models; /// \brief Msg containg info on models detected by logical camera - ignition::msgs::LogicalCameraImage msg; + msgs::LogicalCameraImage msg; }; ////////////////////////////////////////////////// @@ -87,7 +87,7 @@ bool LogicalCameraSensor::Load(sdf::ElementPtr _sdf) if (!_sdf->HasElement("logical_camera")) { ignerr << " SDF element not found while attempting to " - << "load a ignition::sensors::LogicalCameraSensor\n"; + << "load a LogicalCameraSensor\n"; return false; } cameraSdf = _sdf->GetElement("logical_camera"); @@ -108,7 +108,7 @@ bool LogicalCameraSensor::Load(sdf::ElementPtr _sdf) this->SetTopic("/logical_camera"); this->dataPtr->pub = - this->dataPtr->node.Advertise( + this->dataPtr->node.Advertise( this->Topic()); if (!this->dataPtr->pub) @@ -132,7 +132,7 @@ void LogicalCameraSensor::SetModelPoses( } ////////////////////////////////////////////////// -bool LogicalCameraSensor::Update(const ignition::common::Time &_now) +bool LogicalCameraSensor::Update(const common::Time &_now) { IGN_PROFILE("LogicalCameraSensor::Update"); if (!this->dataPtr->initialized) @@ -188,7 +188,7 @@ double LogicalCameraSensor::Far() const } ////////////////////////////////////////////////// -ignition::math::Angle LogicalCameraSensor::HorizontalFOV() const +math::Angle LogicalCameraSensor::HorizontalFOV() const { return this->dataPtr->frustum.FOV(); } diff --git a/src/MagnetometerSensor.cc b/src/MagnetometerSensor.cc index b736df75..3fbd4c72 100644 --- a/src/MagnetometerSensor.cc +++ b/src/MagnetometerSensor.cc @@ -51,14 +51,14 @@ class ignition::sensors::MagnetometerSensorPrivate /// \brief The latest field reading from the sensor, based on the world /// field and the sensor's current pose. - public: ignition::math::Vector3d localField; + public: math::Vector3d localField; /// \brief Store world magnetic field vector. We assume it is uniform /// everywhere in the world, and that it doesn't change during the simulation. - public: ignition::math::Vector3d worldField; + public: math::Vector3d worldField; /// \brief World pose of the magnetometer - public: ignition::math::Pose3d worldPose; + public: math::Pose3d worldPose; /// \brief Noise added to sensor data public: std::map noises; @@ -105,7 +105,7 @@ bool MagnetometerSensor::Load(const sdf::Sensor &_sdf) this->SetTopic("/magnetometer"); this->dataPtr->pub = - this->dataPtr->node.Advertise( + this->dataPtr->node.Advertise( this->Topic()); if (!this->dataPtr->pub) @@ -149,7 +149,7 @@ bool MagnetometerSensor::Load(sdf::ElementPtr _sdf) } ////////////////////////////////////////////////// -bool MagnetometerSensor::Update(const ignition::common::Time &_now) +bool MagnetometerSensor::Update(const common::Time &_now) { IGN_PROFILE("MagnetometerSensor::Update"); if (!this->dataPtr->initialized) diff --git a/src/Manager.cc b/src/Manager.cc index 9e49eb24..1c9962c7 100644 --- a/src/Manager.cc +++ b/src/Manager.cc @@ -72,8 +72,8 @@ bool Manager::Init() } ////////////////////////////////////////////////// -ignition::sensors::Sensor *Manager::Sensor( - ignition::sensors::SensorId _id) +Sensor *Manager::Sensor( + SensorId _id) { auto iter = this->dataPtr->sensors.find(_id); return iter != this->dataPtr->sensors.end() ? iter->second.get() : nullptr; @@ -86,7 +86,7 @@ void Manager::AddPluginPaths(const std::string &_paths) } ////////////////////////////////////////////////// -bool Manager::Remove(const ignition::sensors::SensorId _id) +bool Manager::Remove(const SensorId _id) { return this->dataPtr->sensors.erase(_id) > 0; } @@ -102,7 +102,7 @@ void Manager::RunOnce(const ignition::common::Time &_time, bool _force) } ///////////////////////////////////////////////// -ignition::sensors::SensorId Manager::CreateSensor(const sdf::Sensor &_sdf) +SensorId Manager::CreateSensor(const sdf::Sensor &_sdf) { auto sensor = this->dataPtr->sensorFactory.CreateSensor(_sdf); if (!sensor) @@ -114,7 +114,7 @@ ignition::sensors::SensorId Manager::CreateSensor(const sdf::Sensor &_sdf) } ///////////////////////////////////////////////// -ignition::sensors::SensorId Manager::CreateSensor(sdf::ElementPtr _sdf) +SensorId Manager::CreateSensor(sdf::ElementPtr _sdf) { auto sensor = this->dataPtr->sensorFactory.CreateSensor(_sdf); if (!sensor) diff --git a/src/Noise_TEST.cc b/src/Noise_TEST.cc index 735f3f1c..1019f22c 100644 --- a/src/Noise_TEST.cc +++ b/src/Noise_TEST.cc @@ -61,7 +61,7 @@ class NoiseTest : public ::testing::Test // Documentation inherited protected: void SetUp() override { - ignition::common::Console::SetVerbosity(4); + common::Console::SetVerbosity(4); } }; @@ -132,7 +132,7 @@ void NoNoise(sensors::NoisePtr _noise, unsigned int _count) // Expect no change in input value for (unsigned int i = 0; i < _count; ++i) { - double x = ignition::math::Rand::DblUniform(-1e6, 1e6); + double x = math::Rand::DblUniform(-1e6, 1e6); EXPECT_NEAR(x, _noise->Apply(x), 1e-6); } } diff --git a/src/RgbdCameraSensor.cc b/src/RgbdCameraSensor.cc index 2afd1bc5..2e0b19da 100644 --- a/src/RgbdCameraSensor.cc +++ b/src/RgbdCameraSensor.cc @@ -202,7 +202,7 @@ bool RgbdCameraSensor::Load(const sdf::Sensor &_sdf) // Create the 2d image publisher this->dataPtr->imagePub = - this->dataPtr->node.Advertise( + this->dataPtr->node.Advertise( this->Topic() + "/image"); if (!this->dataPtr->imagePub) { @@ -216,7 +216,7 @@ bool RgbdCameraSensor::Load(const sdf::Sensor &_sdf) // Create the depth image publisher this->dataPtr->depthPub = - this->dataPtr->node.Advertise( + this->dataPtr->node.Advertise( this->Topic() + "/depth_image"); if (!this->dataPtr->depthPub) { @@ -230,7 +230,7 @@ bool RgbdCameraSensor::Load(const sdf::Sensor &_sdf) // Create the point cloud publisher this->dataPtr->pointPub = - this->dataPtr->node.Advertise( + this->dataPtr->node.Advertise( this->Topic() + "/points"); if (!this->dataPtr->pointPub) { @@ -381,7 +381,7 @@ bool RgbdCameraSensor::CreateCameras() } ///////////////////////////////////////////////// -void RgbdCameraSensor::SetScene(ignition::rendering::ScenePtr _scene) +void RgbdCameraSensor::SetScene(rendering::ScenePtr _scene) { std::lock_guard lock(this->dataPtr->mutex); // APIs make it possible for the scene pointer to change @@ -433,7 +433,7 @@ void RgbdCameraSensorPrivate::OnNewRgbPointCloud(const float *_scan, } ////////////////////////////////////////////////// -bool RgbdCameraSensor::Update(const ignition::common::Time &_now) +bool RgbdCameraSensor::Update(const common::Time &_now) { IGN_PROFILE("RgbdCameraSensor::Update"); if (!this->dataPtr->initialized) @@ -458,7 +458,7 @@ bool RgbdCameraSensor::Update(const ignition::common::Time &_now) // create and publish the depthmessage if (this->dataPtr->depthPub.HasConnections()) { - ignition::msgs::Image msg; + msgs::Image msg; msg.set_width(width); msg.set_height(height); msg.set_step(width * rendering::PixelUtil::BytesPerPixel( @@ -483,12 +483,12 @@ bool RgbdCameraSensor::Update(const ignition::common::Time &_now) if (this->dataPtr->hasDepthFarClip && (this->dataPtr->depthBuffer[i] > this->dataPtr->depthFarClip)) { - this->dataPtr->depthBuffer[i] = ignition::math::INF_D; + this->dataPtr->depthBuffer[i] = math::INF_D; } if (this->dataPtr->hasDepthNearClip && (this->dataPtr->depthBuffer[i] < this->dataPtr->depthNearClip)) { - this->dataPtr->depthBuffer[i] = -ignition::math::INF_D; + this->dataPtr->depthBuffer[i] = -math::INF_D; } } } @@ -573,7 +573,7 @@ bool RgbdCameraSensor::Update(const ignition::common::Time &_now) unsigned char *data = this->dataPtr->image.Data(); - ignition::msgs::Image msg; + msgs::Image msg; msg.set_width(width); msg.set_height(height); msg.set_step(width * rendering::PixelUtil::BytesPerPixel( diff --git a/src/SensorFactory.cc b/src/SensorFactory.cc index 5e9cfc58..66797c74 100644 --- a/src/SensorFactory.cc +++ b/src/SensorFactory.cc @@ -89,7 +89,7 @@ std::shared_ptr SensorFactory::LoadSensorPlugin( this->dataPtr->pluginLoader.Instantiate(pluginName); auto sensorPlugin = pluginPtr->QueryInterfaceSharedPtr< - ignition::sensors::SensorPlugin>(); + SensorPlugin>(); return sensorPlugin; } diff --git a/src/SensorTypes.cc b/src/SensorTypes.cc index 365eb665..866cbbec 100644 --- a/src/SensorTypes.cc +++ b/src/SensorTypes.cc @@ -21,9 +21,9 @@ using namespace ignition; using namespace sensors; // Initialize enum iterator, and string converter -IGN_ENUM(sensorNoiseIface, sensors::SensorNoiseType, - sensors::SENSOR_NOISE_TYPE_BEGIN, - sensors::SENSOR_NOISE_TYPE_END, +IGN_ENUM(sensorNoiseIface, SensorNoiseType, + SENSOR_NOISE_TYPE_BEGIN, + SENSOR_NOISE_TYPE_END, "NO_NOISE", "CAMERA_NOISE", "SENSOR_NOISE_TYPE_END") diff --git a/src/Sensor_TEST.cc b/src/Sensor_TEST.cc index edeabdbd..edfb41f5 100644 --- a/src/Sensor_TEST.cc +++ b/src/Sensor_TEST.cc @@ -56,7 +56,7 @@ class Sensor_TEST : public ::testing::Test // Documentation inherited protected: void SetUp() override { - ignition::common::Console::SetVerbosity(4); + common::Console::SetVerbosity(4); } }; @@ -98,7 +98,7 @@ TEST(Sensor_TEST, Sensor) TEST(Sensor_TEST, AddSequence) { TestSensor sensor; - ignition::msgs::Header header; + msgs::Header header; sensor.AddSequence(&header); EXPECT_EQ("seq", header.data(0).key()); EXPECT_EQ("0", header.data(0).value(0)); @@ -117,7 +117,7 @@ TEST(Sensor_TEST, AddSequence) EXPECT_EQ("101", header.data(0).value(0)); // Add another sequence for this sensor. - ignition::msgs::Header header2; + msgs::Header header2; sensor.AddSequence(&header2, "other"); // The original header shouldn't change EXPECT_EQ(1, header.data_size()); @@ -149,14 +149,14 @@ class SensorUpdate : public ::testing::Test // Documentation inherited protected: void SetUp() override { - ignition::common::Console::SetVerbosity(4); + 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) + const msgs::PerformanceSensorMetrics &_msg) { EXPECT_EQ(kSensorName, _msg.name()); performanceMetricsMsgsCount++; diff --git a/src/ThermalCameraSensor.cc b/src/ThermalCameraSensor.cc index b4931a6b..d7093538 100644 --- a/src/ThermalCameraSensor.cc +++ b/src/ThermalCameraSensor.cc @@ -204,7 +204,7 @@ bool ThermalCameraSensor::Load(const sdf::Sensor &_sdf) // Create the thermal image publisher this->dataPtr->thermalPub = - this->dataPtr->node.Advertise( + this->dataPtr->node.Advertise( this->Topic()); if (!this->dataPtr->thermalPub) @@ -350,13 +350,13 @@ rendering::ThermalCameraPtr ThermalCameraSensor::ThermalCamera() ///////////////////////////////////////////////// common::ConnectionPtr ThermalCameraSensor::ConnectImageCallback( - std::function _callback) + std::function _callback) { return this->dataPtr->imageEvent.Connect(_callback); } ///////////////////////////////////////////////// -void ThermalCameraSensor::SetScene(ignition::rendering::ScenePtr _scene) +void ThermalCameraSensor::SetScene(rendering::ScenePtr _scene) { std::lock_guard lock(this->dataPtr->mutex); // APIs make it possible for the scene pointer to change @@ -372,7 +372,7 @@ void ThermalCameraSensor::SetScene(ignition::rendering::ScenePtr _scene) } ////////////////////////////////////////////////// -bool ThermalCameraSensor::Update(const ignition::common::Time &_now) +bool ThermalCameraSensor::Update(const common::Time &_now) { IGN_PROFILE("ThermalCameraSensor::Update"); if (!this->dataPtr->initialized) @@ -550,19 +550,19 @@ bool ThermalCameraSensorPrivate::ConvertTemperatureToImage( ////////////////////////////////////////////////// bool ThermalCameraSensorPrivate::SaveImage(const uint16_t *_data, unsigned int _width, unsigned int _height, - ignition::common::Image::PixelFormatType /*_format*/) + common::Image::PixelFormatType /*_format*/) { // Attempt to create the directory if it doesn't exist - if (!ignition::common::isDirectory(this->saveImagePath)) + if (!common::isDirectory(this->saveImagePath)) { - if (!ignition::common::createDirectories(this->saveImagePath)) + if (!common::createDirectories(this->saveImagePath)) return false; } if (_width == 0 || _height == 0) return false; - ignition::common::Image localImage; + common::Image localImage; if (static_cast(_width) != this->imgThermalBufferSize.X() || static_cast(_height) != this->imgThermalBufferSize.Y()) @@ -583,7 +583,7 @@ bool ThermalCameraSensorPrivate::SaveImage(const uint16_t *_data, localImage.SetFromData(this->imgThermalBuffer, _width, _height, common::Image::RGB_INT8); localImage.SavePNG( - ignition::common::joinPaths(this->saveImagePath, filename)); + common::joinPaths(this->saveImagePath, filename)); return true; } diff --git a/test/integration/rgbd_camera_plugin.cc b/test/integration/rgbd_camera_plugin.cc index 31232b23..2b3ad150 100644 --- a/test/integration/rgbd_camera_plugin.cc +++ b/test/integration/rgbd_camera_plugin.cc @@ -74,9 +74,9 @@ unsigned char *g_pointsRGBBuffer = nullptr; std::mutex g_infoMutex; unsigned int g_infoCounter = 0; -ignition::msgs::CameraInfo g_infoMsg; +msgs::CameraInfo g_infoMsg; -void UnpackPointCloudMsg(const ignition::msgs::PointCloudPacked &_msg, +void UnpackPointCloudMsg(const msgs::PointCloudPacked &_msg, float *_xyzBuffer, unsigned char *_rgbBuffer) { std::string msgBuffer = _msg.data(); @@ -115,7 +115,7 @@ void UnpackPointCloudMsg(const ignition::msgs::PointCloudPacked &_msg, } -void OnCameraInfo(const ignition::msgs::CameraInfo & _msg) +void OnCameraInfo(const msgs::CameraInfo & _msg) { g_infoMutex.lock(); g_infoCounter++; @@ -123,7 +123,7 @@ void OnCameraInfo(const ignition::msgs::CameraInfo & _msg) g_infoMutex.unlock(); } -void OnDepthImage(const ignition::msgs::Image &_msg) +void OnDepthImage(const msgs::Image &_msg) { g_mutex.lock(); unsigned int depthSamples = _msg.width() * _msg.height(); @@ -135,7 +135,7 @@ void OnDepthImage(const ignition::msgs::Image &_msg) g_mutex.unlock(); } -void OnImage(const ignition::msgs::Image &_msg) +void OnImage(const msgs::Image &_msg) { g_imgMutex.lock(); unsigned int imgSize = _msg.width() * _msg.height() * 3; @@ -146,7 +146,7 @@ void OnImage(const ignition::msgs::Image &_msg) g_imgMutex.unlock(); } -void OnPointCloud(const ignition::msgs::PointCloudPacked &_msg) +void OnPointCloud(const msgs::PointCloudPacked &_msg) { g_pcMutex.lock(); @@ -174,7 +174,7 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF( const std::string &_renderEngine) { // get the darn test data - std::string path = ignition::common::joinPaths(PROJECT_SOURCE_PATH, "test", + std::string path = common::joinPaths(PROJECT_SOURCE_PATH, "test", "integration", "rgbd_camera_sensor_builtin.sdf"); sdf::SDFPtr doc(new sdf::SDF()); sdf::init(doc); @@ -199,7 +199,7 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF( double near_ = clipPtr->Get("near"); double unitBoxSize = 1.0; - ignition::math::Vector3d boxPosition(3.0, 0.0, 0.0); + math::Vector3d boxPosition(3.0, 0.0, 0.0); // If ogre is not the engine, don't run the test if ((_renderEngine.compare("ogre") != 0) && @@ -211,7 +211,7 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF( } // Setup ign-rendering with an empty scene - auto *engine = ignition::rendering::engine(_renderEngine); + auto *engine = rendering::engine(_renderEngine); if (!engine) { igndbg << "Engine '" << _renderEngine @@ -219,23 +219,23 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF( return; } - ignition::rendering::ScenePtr scene = engine->CreateScene("scene"); + rendering::ScenePtr scene = engine->CreateScene("scene"); // Create an scene with a box in it scene->SetAmbientLight(0.0, 0.0, 1.0); - ignition::rendering::VisualPtr root = scene->RootVisual(); + rendering::VisualPtr root = scene->RootVisual(); // red background scene->SetBackgroundColor(1.0, 0.0, 0.0); // create blue material - ignition::rendering::MaterialPtr blue = scene->CreateMaterial(); + rendering::MaterialPtr blue = scene->CreateMaterial(); blue->SetAmbient(0.0, 0.0, 1.0); blue->SetDiffuse(0.0, 0.0, 1.0); blue->SetSpecular(0.0, 0.0, 1.0); // create box visual - ignition::rendering::VisualPtr box = scene->CreateVisual(); + rendering::VisualPtr box = scene->CreateVisual(); box->AddGeometry(scene->CreateBox()); box->SetOrigin(0.0, 0.0, 0.0); box->SetLocalPosition(boxPosition); @@ -245,10 +245,10 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF( root->AddChild(box); // do the test - ignition::sensors::Manager mgr; + sensors::Manager mgr; - ignition::sensors::RgbdCameraSensor *rgbdSensor = - mgr.CreateSensor(sensorPtr); + sensors::RgbdCameraSensor *rgbdSensor = + mgr.CreateSensor(sensorPtr); ASSERT_NE(rgbdSensor, nullptr); rgbdSensor->SetScene(scene); @@ -257,30 +257,30 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF( std::string depthTopic = "/test/integration/RgbdCameraPlugin_imagesWithBuiltinSDF/depth_image"; - WaitForMessageTestHelper depthHelper(depthTopic); + WaitForMessageTestHelper depthHelper(depthTopic); std::string imageTopic = "/test/integration/RgbdCameraPlugin_imagesWithBuiltinSDF/image"; - WaitForMessageTestHelper imageHelper(imageTopic); + WaitForMessageTestHelper imageHelper(imageTopic); std::string pointsTopic = "/test/integration/RgbdCameraPlugin_imagesWithBuiltinSDF/points"; - WaitForMessageTestHelper + WaitForMessageTestHelper pointsHelper(pointsTopic); std::string infoTopic = "/test/integration/RgbdCameraPlugin_imagesWithBuiltinSDF/camera_info"; - WaitForMessageTestHelper infoHelper(infoTopic); + WaitForMessageTestHelper infoHelper(infoTopic); // Update once to create image - mgr.RunOnce(ignition::common::Time::Zero); + mgr.RunOnce(common::Time::Zero); EXPECT_TRUE(depthHelper.WaitForMessage()) << depthHelper; EXPECT_TRUE(imageHelper.WaitForMessage()) << imageHelper; EXPECT_TRUE(pointsHelper.WaitForMessage()) << pointsHelper; // subscribe to the depth camera topic - ignition::transport::Node node; + transport::Node node; node.Subscribe(depthTopic, &OnDepthImage); // subscribe to the image topic @@ -293,15 +293,15 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF( node.Subscribe(infoTopic, &OnCameraInfo); // Update once more - mgr.RunOnce(ignition::common::Time::Zero, true); + mgr.RunOnce(common::Time::Zero, true); // wait for an image - ignition::common::Time waitTime = ignition::common::Time(0.001); + common::Time waitTime = common::Time(0.001); int counter = 0; int infoCounter = 0; int imgCounter = 0; int pcCounter = 0; - ignition::msgs::CameraInfo infoMsg; + msgs::CameraInfo infoMsg; for (int sleep = 0; sleep < 300 && (counter == 0 || infoCounter == 0 || imgCounter == 0 || pcCounter == 0); ++sleep) @@ -319,7 +319,7 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF( g_infoMutex.unlock(); g_imgMutex.unlock(); g_pcMutex.unlock(); - ignition::common::Time::Sleep(waitTime); + common::Time::Sleep(waitTime); } g_mutex.lock(); @@ -362,8 +362,8 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF( EXPECT_NEAR(g_depthBuffer[mid], expectedRangeAtMidPoint, DEPTH_TOL); // The left and right side of the depth frame should be inf - EXPECT_DOUBLE_EQ(g_depthBuffer[left], ignition::math::INF_D); - EXPECT_DOUBLE_EQ(g_depthBuffer[right], ignition::math::INF_D); + EXPECT_DOUBLE_EQ(g_depthBuffer[left], math::INF_D); + EXPECT_DOUBLE_EQ(g_depthBuffer[right], math::INF_D); } // check color @@ -405,16 +405,16 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF( float lx = g_pointsXYZBuffer[imgLeft]; float ly = g_pointsXYZBuffer[imgLeft + 1]; float lz = g_pointsXYZBuffer[imgLeft + 2]; - EXPECT_FLOAT_EQ(ignition::math::INF_D, lx); - EXPECT_FLOAT_EQ(ignition::math::INF_D, ly); - EXPECT_FLOAT_EQ(ignition::math::INF_D, lz); + EXPECT_FLOAT_EQ(math::INF_D, lx); + EXPECT_FLOAT_EQ(math::INF_D, ly); + EXPECT_FLOAT_EQ(math::INF_D, lz); float rx = g_pointsXYZBuffer[imgRight]; float ry = g_pointsXYZBuffer[imgRight + 1]; float rz = g_pointsXYZBuffer[imgRight + 2]; - EXPECT_FLOAT_EQ(ignition::math::INF_D, rx); - EXPECT_FLOAT_EQ(ignition::math::INF_D, ry); - EXPECT_FLOAT_EQ(ignition::math::INF_D, rz); + EXPECT_FLOAT_EQ(math::INF_D, rx); + EXPECT_FLOAT_EQ(math::INF_D, ry); + EXPECT_FLOAT_EQ(math::INF_D, rz); // point to the left of mid point should have larger y value than mid // point, which in turn should have large y value than point to the @@ -464,7 +464,7 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF( // result against actual point cloud data { // init the point cloud msg to be filled - ignition::msgs::PointCloudPacked pointsMsg; + msgs::PointCloudPacked pointsMsg; msgs::InitPointCloudPacked(pointsMsg, "depth2Image", true, {{"xyz", msgs::PointCloudPacked::Field::FLOAT32}, {"rgb", msgs::PointCloudPacked::Field::FLOAT32}}); @@ -473,7 +473,7 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF( pointsMsg.set_row_step(pointsMsg.point_step() * rgbdSensor->ImageWidth()); // fill msg does the conversion from depth to points - ignition::sensors::PointCloudUtil pointsUtil; + sensors::PointCloudUtil pointsUtil; pointsUtil.FillMsg(pointsMsg, 1.05, g_pointsRGBBuffer, g_depthBuffer); // Unpack the point cloud msg into separate rgb and xyz buffers @@ -527,12 +527,12 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF( // Check that for a box really close it returns -inf root->RemoveChild(box); - ignition::math::Vector3d boxPositionNear( + math::Vector3d boxPositionNear( unitBoxSize * 0.5 + near_ * 0.5, 0.0, 0.0); box->SetLocalPosition(boxPositionNear); root->AddChild(box); - mgr.RunOnce(ignition::common::Time::Zero, true); + mgr.RunOnce(common::Time::Zero, true); for (int sleep = 0; sleep < 300 && (counter == 0 || infoCounter == 0 || imgCounter == 0 || pcCounter == 0); ++sleep) @@ -550,7 +550,7 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF( g_infoMutex.unlock(); g_imgMutex.unlock(); g_pcMutex.unlock(); - ignition::common::Time::Sleep(waitTime); + common::Time::Sleep(waitTime); } g_mutex.lock(); @@ -578,7 +578,7 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF( for (unsigned int j = 0; j < rgbdSensor->ImageWidth(); ++j) { unsigned int index = step + j; - EXPECT_FLOAT_EQ(-ignition::math::INF_D, g_depthBuffer[index]); + EXPECT_FLOAT_EQ(-math::INF_D, g_depthBuffer[index]); } } } @@ -612,9 +612,9 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF( float x = g_pointsXYZBuffer[index]; float y = g_pointsXYZBuffer[index + 1]; float z = g_pointsXYZBuffer[index + 2]; - EXPECT_FLOAT_EQ(-ignition::math::INF_D, x); - EXPECT_FLOAT_EQ(-ignition::math::INF_D, y); - EXPECT_FLOAT_EQ(-ignition::math::INF_D, z); + EXPECT_FLOAT_EQ(-math::INF_D, x); + EXPECT_FLOAT_EQ(-math::INF_D, y); + EXPECT_FLOAT_EQ(-math::INF_D, z); unsigned int r = g_pointsRGBBuffer[index]; unsigned int g = g_pointsRGBBuffer[index + 1]; @@ -633,12 +633,12 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF( // Check that for a box really far it returns inf root->RemoveChild(box); - ignition::math::Vector3d boxPositionFar( + math::Vector3d boxPositionFar( unitBoxSize * 0.5 + far_ * 1.5, 0.0, 0.0); box->SetLocalPosition(boxPositionFar); root->AddChild(box); - mgr.RunOnce(ignition::common::Time::Zero, true); + mgr.RunOnce(common::Time::Zero, true); for (int sleep = 0; sleep < 300 && (counter == 0 || infoCounter == 0 || imgCounter == 0 || pcCounter == 0); ++sleep) @@ -656,7 +656,7 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF( g_infoMutex.unlock(); g_imgMutex.unlock(); g_pcMutex.unlock(); - ignition::common::Time::Sleep(waitTime); + common::Time::Sleep(waitTime); } g_mutex.lock(); @@ -684,7 +684,7 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF( for (unsigned int j = 0; j < rgbdSensor->ImageWidth(); ++j) { unsigned int index = step + j; - EXPECT_FLOAT_EQ(ignition::math::INF_D, g_depthBuffer[index]); + EXPECT_FLOAT_EQ(math::INF_D, g_depthBuffer[index]); } } } @@ -718,9 +718,9 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF( float x = g_pointsXYZBuffer[index]; float y = g_pointsXYZBuffer[index + 1]; float z = g_pointsXYZBuffer[index + 2]; - EXPECT_FLOAT_EQ(ignition::math::INF_D, x); - EXPECT_FLOAT_EQ(ignition::math::INF_D, y); - EXPECT_FLOAT_EQ(ignition::math::INF_D, z); + EXPECT_FLOAT_EQ(math::INF_D, x); + EXPECT_FLOAT_EQ(math::INF_D, y); + EXPECT_FLOAT_EQ(math::INF_D, z); unsigned int r = g_pointsRGBBuffer[index]; unsigned int g = g_pointsRGBBuffer[index + 1]; @@ -739,7 +739,7 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF( // Clean up engine->DestroyScene(scene); - ignition::rendering::unloadEngine(engine->Name()); + rendering::unloadEngine(engine->Name()); } ////////////////////////////////////////////////// @@ -749,12 +749,12 @@ TEST_P(RgbdCameraSensorTest, ImagesWithBuiltinSDF) } INSTANTIATE_TEST_CASE_P(RgbdCameraSensor, RgbdCameraSensorTest, - RENDER_ENGINE_VALUES, ignition::rendering::PrintToStringParam()); + RENDER_ENGINE_VALUES, rendering::PrintToStringParam()); ////////////////////////////////////////////////// int main(int argc, char **argv) { - ignition::common::Console::SetVerbosity(4); + common::Console::SetVerbosity(4); ::testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } From 197ea8d8cfe7c9a642a4e740e7df23d6372883fe Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 16 Aug 2022 14:37:26 -0700 Subject: [PATCH 2/9] =?UTF-8?q?=F0=9F=8E=88=203.4.0=20(#261)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Louise Poubel Signed-off-by: Louise Poubel --- CMakeLists.txt | 2 +- Changelog.md | 19 +++++++++++++++++-- 2 files changed, 18 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d1a5a798..8423159a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(ignition-sensors3 VERSION 3.3.0) +project(ignition-sensors3 VERSION 3.4.0) #============================================================================ # Find ignition-cmake diff --git a/Changelog.md b/Changelog.md index 634b21b9..90df3dd1 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,6 +1,21 @@ -## Ignition Sensors 3 +## Gazebo Sensors 3 -### Ignition Sensors 3.X.X (202X-XX-XX) +### Gazebo Sensors 3.4.0 (2022-08-16) + +1. Remove redundant namespace references + * [Pull request #258](https://github.com/gazebosim/gz-sensors/pull/258) + +1. Ignition -> Gazebo + * [Pull request #245](https://github.com/gazebosim/gz-sensors/pull/245) + +1. Conform to ros format for header field `frame_id` of sensor msgs + * [Pull request #195](https://github.com/gazebosim/gz-sensors/pull/195) + +1. Fix compiler warnings (`CMP0072` and copy elision) + * [Pull request #188](https://github.com/gazebosim/gz-sensors/pull/188) + +1. Require ign-transport >= 8.2 + * [Pull request #167](https://github.com/gazebosim/gz-sensors/pull/167) ### Ignition Sensors 3.3.0 (2021-08-26) From c754e2181a405bea1a95471badd0c172a011f357 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 26 Aug 2022 15:08:21 -0700 Subject: [PATCH 3/9] Add missing DEPENDS_ON_COMPONENTS parameters (#262) * gpu_lidar DEPENDS_ON_COMPONENT lidar This is needed to fix ign-gazebo3 builds. * Add other missing DEPENDS_ON_COMPONENT Signed-off-by: Steve Peters --- src/CMakeLists.txt | 36 ++++++++++++++++++++++++++++++------ 1 file changed, 30 insertions(+), 6 deletions(-) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 97a38024..5581a236 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -53,7 +53,11 @@ target_link_libraries(${rendering_target} ) set(camera_sources CameraSensor.cc) -ign_add_component(camera SOURCES ${camera_sources} GET_TARGET_NAME camera_target) +ign_add_component(camera + SOURCES ${camera_sources} + DEPENDS_ON_COMPONENTS rendering + GET_TARGET_NAME camera_target +) # custom compile definitions since the one provided automatically is versioned and will # make the code need to change with every major version target_compile_definitions(${camera_target} PUBLIC CameraSensor_EXPORTS) @@ -66,7 +70,11 @@ target_link_libraries(${camera_target} ) set(depth_camera_sources DepthCameraSensor.cc) -ign_add_component(depth_camera SOURCES ${depth_camera_sources} GET_TARGET_NAME depth_camera_target) +ign_add_component(depth_camera + SOURCES ${depth_camera_sources} + DEPENDS_ON_COMPONENTS camera + GET_TARGET_NAME depth_camera_target +) target_compile_definitions(${depth_camera_target} PUBLIC DepthCameraSensor_EXPORTS) target_link_libraries(${depth_camera_target} PRIVATE @@ -76,7 +84,11 @@ target_link_libraries(${depth_camera_target} ) set(lidar_sources Lidar.cc) -ign_add_component(lidar SOURCES ${lidar_sources} GET_TARGET_NAME lidar_target) +ign_add_component(lidar + SOURCES ${lidar_sources} + DEPENDS_ON_COMPONENTS rendering + GET_TARGET_NAME lidar_target +) target_compile_definitions(${lidar_target} PUBLIC Lidar_EXPORTS) target_link_libraries(${lidar_target} PUBLIC @@ -87,7 +99,11 @@ target_link_libraries(${lidar_target} ) set(gpu_lidar_sources GpuLidarSensor.cc) -ign_add_component(gpu_lidar SOURCES ${gpu_lidar_sources} GET_TARGET_NAME gpu_lidar_target) +ign_add_component(gpu_lidar + DEPENDS_ON_COMPONENTS lidar + SOURCES ${gpu_lidar_sources} + GET_TARGET_NAME gpu_lidar_target +) target_compile_definitions(${gpu_lidar_target} PUBLIC GpuLidarSensor_EXPORTS) target_link_libraries(${gpu_lidar_target} PRIVATE @@ -118,7 +134,11 @@ set(air_pressure_sources AirPressureSensor.cc) ign_add_component(air_pressure SOURCES ${air_pressure_sources} GET_TARGET_NAME air_pressure_target) set(rgbd_camera_sources RgbdCameraSensor.cc) -ign_add_component(rgbd_camera SOURCES ${rgbd_camera_sources} GET_TARGET_NAME rgbd_camera_target) +ign_add_component(rgbd_camera + SOURCES ${rgbd_camera_sources} + DEPENDS_ON_COMPONENTS camera + GET_TARGET_NAME rgbd_camera_target +) target_compile_definitions(${rgbd_camera_target} PUBLIC RgbdCameraSensor_EXPORTS) target_link_libraries(${rgbd_camera_target} PRIVATE @@ -128,7 +148,11 @@ target_link_libraries(${rgbd_camera_target} ) set(thermal_camera_sources ThermalCameraSensor.cc) -ign_add_component(thermal_camera SOURCES ${thermal_camera_sources} GET_TARGET_NAME thermal_camera_target) +ign_add_component(thermal_camera + SOURCES ${thermal_camera_sources} + DEPENDS_ON_COMPONENTS camera + GET_TARGET_NAME thermal_camera_target +) target_link_libraries(${thermal_camera_target} PRIVATE ${camera_target} From e0dab26beb387dbcf3de60bc50dd7344d844e9e5 Mon Sep 17 00:00:00 2001 From: Marq Rasmussen Date: Tue, 27 Sep 2022 02:19:56 -0600 Subject: [PATCH 4/9] Add optional optical frame id to camera sensors (#259) --- CMakeLists.txt | 2 +- src/CameraSensor.cc | 20 ++++++++++++++++---- src/RgbdCameraSensor.cc | 23 ++++++++++++++++++++--- 3 files changed, 37 insertions(+), 8 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b7562e40..6abaa3d1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -86,7 +86,7 @@ set(IGN_MSGS_VER ${ignition-msgs8_VERSION_MAJOR}) #-------------------------------------- # Find SDFormat -ign_find_package(sdformat12 REQUIRED VERSION 12.5.0) +ign_find_package(sdformat12 REQUIRED VERSION 12.6.0) set(SDF_VER ${sdformat12_VERSION_MAJOR}) set(IGN_SENSORS_PLUGIN_PATH ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}) diff --git a/src/CameraSensor.cc b/src/CameraSensor.cc index 4fc0ae4b..6ea7688b 100644 --- a/src/CameraSensor.cc +++ b/src/CameraSensor.cc @@ -129,6 +129,9 @@ class ignition::sensors::CameraSensorPrivate /// \brief Camera information message. public: msgs::CameraInfo infoMsg; + /// \brief The frame this camera uses in its camera_info topic. + public: std::string opticalFrameId{""}; + /// \brief Topic for info message. public: std::string infoTopic{""}; @@ -462,7 +465,7 @@ bool CameraSensor::Update(const std::chrono::steady_clock::duration &_now) *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now); auto frame = msg.mutable_header()->add_data(); frame->set_key("frame_id"); - frame->add_value(this->FrameId()); + frame->add_value(this->dataPtr->opticalFrameId); msg.set_data(data, this->dataPtr->camera->ImageMemorySize()); } @@ -676,11 +679,20 @@ void CameraSensor::PopulateInfo(const sdf::Camera *_cameraSdf) // Note: while Gazebo interprets the camera frame to be looking towards +X, // other tools, such as ROS, may interpret this frame as looking towards +Z. - // TODO(anyone) Expose the `frame_id` as an SDF parameter so downstream users - // can populate it with arbitrary frames. + // To make this configurable the user has the option to set an optical frame. + // If the user has set in the cameraSdf use it, + // otherwise fall back to the sensor frame. + if (_cameraSdf->OpticalFrameId().empty()) + { + this->dataPtr->opticalFrameId = this->FrameId(); + } + else + { + this->dataPtr->opticalFrameId = _cameraSdf->OpticalFrameId(); + } auto infoFrame = this->dataPtr->infoMsg.mutable_header()->add_data(); infoFrame->set_key("frame_id"); - infoFrame->add_value(this->FrameId()); + infoFrame->add_value(this->dataPtr->opticalFrameId); this->dataPtr->infoMsg.set_width(width); this->dataPtr->infoMsg.set_height(height); diff --git a/src/RgbdCameraSensor.cc b/src/RgbdCameraSensor.cc index aeab3d55..783e17df 100644 --- a/src/RgbdCameraSensor.cc +++ b/src/RgbdCameraSensor.cc @@ -119,6 +119,9 @@ class ignition::sensors::RgbdCameraSensorPrivate /// point cloud. public: unsigned int channels = 4; + /// \brief Frame ID the camera_info message header is expressed. + public: std::string opticalFrameId{""}; + /// \brief Pointer to an image to be published public: ignition::rendering::Image image; @@ -258,7 +261,7 @@ bool RgbdCameraSensor::Load(const sdf::Sensor &_sdf) // the xyz and rgb fields to be aligned to memory boundaries. This is need // by ROS1: https://github.com/ros/common_msgs/pull/77. Ideally, memory // alignment should be configured. - msgs::InitPointCloudPacked(this->dataPtr->pointMsg, this->Name(), true, + msgs::InitPointCloudPacked(this->dataPtr->pointMsg, this->FrameId(), true, {{"xyz", msgs::PointCloudPacked::Field::FLOAT32}, {"rgb", msgs::PointCloudPacked::Field::FLOAT32}}); @@ -299,6 +302,20 @@ bool RgbdCameraSensor::CreateCameras() this->dataPtr->depthCamera->SetNearClipPlane(cameraSdf->NearClip()); this->dataPtr->depthCamera->SetFarClipPlane(cameraSdf->FarClip()); + // Note: while Gazebo interprets the camera frame to be looking towards +X, + // other tools, such as ROS, may interpret this frame as looking towards +Z. + // To make this configurable the user has the option to set an optical frame. + // If the user has set in the cameraSdf use it, + // otherwise fall back to the sensor frame. + if (cameraSdf->OpticalFrameId().empty()) + { + this->dataPtr->opticalFrameId = this->FrameId(); + } + else + { + this->dataPtr->opticalFrameId = cameraSdf->OpticalFrameId(); + } + // Depth camera clip params are new and only override the camera clip // params if specified. if (cameraSdf->HasDepthCamera()) @@ -477,7 +494,7 @@ bool RgbdCameraSensor::Update(const std::chrono::steady_clock::duration &_now) *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now); auto frame = msg.mutable_header()->add_data(); frame->set_key("frame_id"); - frame->add_value(this->FrameId()); + frame->add_value(this->dataPtr->opticalFrameId); std::lock_guard lock(this->dataPtr->mutex); @@ -589,7 +606,7 @@ bool RgbdCameraSensor::Update(const std::chrono::steady_clock::duration &_now) *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now); auto frame = msg.mutable_header()->add_data(); frame->set_key("frame_id"); - frame->add_value(this->FrameId()); + frame->add_value(this->dataPtr->opticalFrameId); msg.set_data(data, rendering::PixelUtil::MemorySize(rendering::PF_R8G8B8, width, height)); From dc02bc7d340376faa6271a5709dd02c5aeb71137 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 29 Sep 2022 15:30:20 -0700 Subject: [PATCH 5/9] Add support for 16 bit image format (#276) Signed-off-by: Ian Chen Co-authored-by: Michael Carroll --- src/CameraSensor.cc | 7 ++ test/integration/camera.cc | 110 ++++++++++++++++++++++++- test/sdf/camera_sensor_l16_builtin.sdf | 29 +++++++ 3 files changed, 144 insertions(+), 2 deletions(-) create mode 100644 test/sdf/camera_sensor_l16_builtin.sdf diff --git a/src/CameraSensor.cc b/src/CameraSensor.cc index 6ea7688b..4fcf9b21 100644 --- a/src/CameraSensor.cc +++ b/src/CameraSensor.cc @@ -220,6 +220,9 @@ bool CameraSensor::CreateCamera() case sdf::PixelFormatType::L_INT8: this->dataPtr->camera->SetImageFormat(ignition::rendering::PF_L8); break; + case sdf::PixelFormatType::L_INT16: + this->dataPtr->camera->SetImageFormat(ignition::rendering::PF_L16); + break; default: ignerr << "Unsupported pixel format [" << static_cast(pixelFormat) << "]\n"; @@ -447,6 +450,10 @@ bool CameraSensor::Update(const std::chrono::steady_clock::duration &_now) format = ignition::common::Image::L_INT8; msgsPixelFormat = msgs::PixelFormatType::L_INT8; break; + case ignition::rendering::PF_L16: + format = ignition::common::Image::L_INT16; + msgsPixelFormat = msgs::PixelFormatType::L_INT16; + break; default: ignerr << "Unsupported pixel format [" << this->dataPtr->camera->ImageFormat() << "]\n"; diff --git a/test/integration/camera.cc b/test/integration/camera.cc index 36dcd1b9..a5ef6e7d 100644 --- a/test/integration/camera.cc +++ b/test/integration/camera.cc @@ -51,7 +51,7 @@ std::mutex g_mutex; unsigned int g_imgCounter = 0; -void OnGrayscaleImage(const ignition::msgs::Image &_msg) +void OnGrayscale8bitImage(const ignition::msgs::Image &_msg) { std::lock_guard lock(g_mutex); EXPECT_EQ(ignition::msgs::PixelFormatType::L_INT8, _msg.pixel_format_type()); @@ -60,6 +60,15 @@ void OnGrayscaleImage(const ignition::msgs::Image &_msg) g_imgCounter++; } +void OnGrayscale16bitImage(const ignition::msgs::Image &_msg) +{ + std::lock_guard lock(g_mutex); + EXPECT_EQ(ignition::msgs::PixelFormatType::L_INT16, _msg.pixel_format_type()); + EXPECT_EQ(256u, _msg.width()); + EXPECT_EQ(256u, _msg.height()); + g_imgCounter++; +} + class CameraSensorTest: public testing::Test, public testing::WithParamInterface { @@ -74,6 +83,9 @@ class CameraSensorTest: public testing::Test, // Create a 8 bit grayscale camera sensor and verify image format public: void ImageFormatLInt8(const std::string &_renderEngine); + + // Create a 16 bit grayscale camera sensor and verify image format + public: void ImageFormatLInt16(const std::string &_renderEngine); }; void CameraSensorTest::ImagesWithBuiltinSDF(const std::string &_renderEngine) @@ -217,9 +229,11 @@ void CameraSensorTest::ImageFormatLInt8(const std::string &_renderEngine) EXPECT_TRUE(helper.WaitForMessage()) << helper; + g_imgCounter = 0u; + // subscribe to the camera topic ignition::transport::Node node; - node.Subscribe(topic, &OnGrayscaleImage); + node.Subscribe(topic, &OnGrayscale8bitImage); // wait for a few camera frames mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); @@ -258,6 +272,98 @@ TEST_P(CameraSensorTest, LInt8ImagesWithBuiltinSDF) ImageFormatLInt8(GetParam()); } +////////////////////////////////////////////////// +void CameraSensorTest::ImageFormatLInt16(const std::string &_renderEngine) +{ + // get the darn test data + std::string path = ignition::common::joinPaths(PROJECT_SOURCE_PATH, "test", + "sdf", "camera_sensor_l16_builtin.sdf"); + sdf::SDFPtr doc(new sdf::SDF()); + sdf::init(doc); + ASSERT_TRUE(sdf::readFile(path, doc)); + ASSERT_NE(nullptr, doc->Root()); + ASSERT_TRUE(doc->Root()->HasElement("model")); + auto modelPtr = doc->Root()->GetElement("model"); + ASSERT_TRUE(modelPtr->HasElement("link")); + auto linkPtr = modelPtr->GetElement("link"); + ASSERT_TRUE(linkPtr->HasElement("sensor")); + auto sensorPtr = linkPtr->GetElement("sensor"); + + // Setup empty scene + auto *engine = ignition::rendering::engine(_renderEngine); + if (!engine) + { + igndbg << "Engine '" << _renderEngine + << "' is not supported" << std::endl; + return; + } + + ignition::rendering::ScenePtr scene = engine->CreateScene("scene"); + + // do the test + ignition::sensors::Manager mgr; + + ignition::sensors::CameraSensor *sensor = + mgr.CreateSensor(sensorPtr); + ASSERT_NE(sensor, nullptr); + sensor->SetScene(scene); + + ASSERT_NE(sensor->RenderingCamera(), nullptr); + EXPECT_NE(sensor->Id(), sensor->RenderingCamera()->Id()); + EXPECT_EQ(256u, sensor->ImageWidth()); + EXPECT_EQ(256u, sensor->ImageHeight()); + + std::string topic = "/images_l16"; + WaitForMessageTestHelper helper(topic); + + // Update once to create image + mgr.RunOnce(std::chrono::steady_clock::duration::zero()); + + EXPECT_TRUE(helper.WaitForMessage()) << helper; + + g_imgCounter = 0u; + + // subscribe to the camera topic + ignition::transport::Node node; + node.Subscribe(topic, &OnGrayscale16bitImage); + + // wait for a few camera frames + mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); + + // run to get image and check image format in callback + bool done = false; + int sleep = 0; + int maxSleep = 10; + while (!done && sleep++ < maxSleep) + { + std::lock_guard lock(g_mutex); + done = (g_imgCounter > 0); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + // test removing sensor + // first make sure the sensor objects do exist + auto sensorId = sensor->Id(); + auto cameraId = sensor->RenderingCamera()->Id(); + EXPECT_EQ(sensor, mgr.Sensor(sensorId)); + EXPECT_EQ(sensor->RenderingCamera(), scene->SensorById(cameraId)); + // remove and check sensor objects no longer exist in both sensors and + // rendering + EXPECT_TRUE(mgr.Remove(sensorId)); + EXPECT_EQ(nullptr, mgr.Sensor(sensorId)); + EXPECT_EQ(nullptr, scene->SensorById(cameraId)); + + // Clean up + engine->DestroyScene(scene); + ignition::rendering::unloadEngine(engine->Name()); +} + +////////////////////////////////////////////////// +TEST_P(CameraSensorTest, LInt16ImagesWithBuiltinSDF) +{ + ImageFormatLInt16(GetParam()); +} + INSTANTIATE_TEST_CASE_P(CameraSensor, CameraSensorTest, RENDER_ENGINE_VALUES, ignition::rendering::PrintToStringParam()); diff --git a/test/sdf/camera_sensor_l16_builtin.sdf b/test/sdf/camera_sensor_l16_builtin.sdf new file mode 100644 index 00000000..8b812bce --- /dev/null +++ b/test/sdf/camera_sensor_l16_builtin.sdf @@ -0,0 +1,29 @@ + + + + + + 10 + base_camera + /images_l16 + + 1.05 + + 256 + 256 + L_INT16 + + + 0.1 + 10.0 + + + gaussian + 0.0 + 0.007 + + + + + + From adeb66057ce156b2690f2b470716a9ae911a61ec Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 26 Oct 2022 22:47:24 +0200 Subject: [PATCH 6/9] Improved coverage Lidar (#277) Signed-off-by: ahcorde Signed-off-by: Michael Carroll Co-authored-by: Michael Carroll --- src/AirPressureSensor.cc | 12 ++--- src/AltimeterSensor.cc | 10 ++++ src/CameraSensor.cc | 12 ++--- src/DepthCameraSensor.cc | 12 ++--- src/GpuLidarSensor.cc | 13 +++--- src/ImageGaussianNoiseModel.cc | 9 ---- src/ImuSensor.cc | 12 ++--- src/ImuSensor_TEST.cc | 19 ++++---- src/Lidar.cc | 10 ++++ src/Lidar_TEST.cc | 84 ++++++++++++++++++++++++++++++---- src/LogicalCameraSensor.cc | 9 ++++ src/MagnetometerSensor.cc | 15 +++--- src/PointCloudUtil.hh | 12 ++--- src/RenderingSensor.cc | 10 ---- src/RgbdCameraSensor.cc | 23 +++------- src/Sensor_TEST.cc | 20 ++++---- src/ThermalCameraSensor.cc | 12 ++--- 17 files changed, 180 insertions(+), 114 deletions(-) diff --git a/src/AirPressureSensor.cc b/src/AirPressureSensor.cc index c91df95d..dc020688 100644 --- a/src/AirPressureSensor.cc +++ b/src/AirPressureSensor.cc @@ -15,14 +15,14 @@ * */ -#ifdef _WIN32 -#pragma warning(push) -#pragma warning(disable: 4005) -#pragma warning(disable: 4251) +#if defined(_MSC_VER) + #pragma warning(push) + #pragma warning(disable: 4005) + #pragma warning(disable: 4251) #endif #include -#ifdef _WIN32 -#pragma warning(pop) +#if defined(_MSC_VER) + #pragma warning(pop) #endif #include diff --git a/src/AltimeterSensor.cc b/src/AltimeterSensor.cc index 9d8e067b..f6a4edc9 100644 --- a/src/AltimeterSensor.cc +++ b/src/AltimeterSensor.cc @@ -15,6 +15,16 @@ * */ +#if defined(_MSC_VER) + #pragma warning(push) + #pragma warning(disable: 4005) + #pragma warning(disable: 4251) +#endif +#include +#if defined(_MSC_VER) + #pragma warning(pop) +#endif + #include #include diff --git a/src/CameraSensor.cc b/src/CameraSensor.cc index edb1f4e2..f40ab556 100644 --- a/src/CameraSensor.cc +++ b/src/CameraSensor.cc @@ -14,14 +14,14 @@ * limitations under the License. * */ -#ifdef _WIN32 -#pragma warning(push) -#pragma warning(disable: 4005) -#pragma warning(disable: 4251) +#if defined(_MSC_VER) + #pragma warning(push) + #pragma warning(disable: 4005) + #pragma warning(disable: 4251) #endif #include -#ifdef _WIN32 -#pragma warning(pop) +#if defined(_MSC_VER) + #pragma warning(pop) #endif #include diff --git a/src/DepthCameraSensor.cc b/src/DepthCameraSensor.cc index 8365317e..64b97ca2 100644 --- a/src/DepthCameraSensor.cc +++ b/src/DepthCameraSensor.cc @@ -14,14 +14,14 @@ * limitations under the License. * */ -#ifdef _WIN32 -#pragma warning(push) -#pragma warning(disable: 4005) -#pragma warning(disable: 4251) +#if defined(_MSC_VER) + #pragma warning(push) + #pragma warning(disable: 4005) + #pragma warning(disable: 4251) #endif #include -#ifdef _WIN32 -#pragma warning(pop) +#if defined(_MSC_VER) + #pragma warning(pop) #endif #include diff --git a/src/GpuLidarSensor.cc b/src/GpuLidarSensor.cc index e1c2bedb..7d199361 100644 --- a/src/GpuLidarSensor.cc +++ b/src/GpuLidarSensor.cc @@ -14,15 +14,14 @@ * limitations under the License. * */ - -#ifdef _WIN32 -#pragma warning(push) -#pragma warning(disable: 4005) -#pragma warning(disable: 4251) +#if defined(_MSC_VER) + #pragma warning(push) + #pragma warning(disable: 4005) + #pragma warning(disable: 4251) #endif #include -#ifdef _WIN32 -#pragma warning(pop) +#if defined(_MSC_VER) + #pragma warning(pop) #endif #include diff --git a/src/ImageGaussianNoiseModel.cc b/src/ImageGaussianNoiseModel.cc index 5f4bf803..00af1ab6 100644 --- a/src/ImageGaussianNoiseModel.cc +++ b/src/ImageGaussianNoiseModel.cc @@ -22,19 +22,10 @@ #include -// TODO(louise) Remove these pragmas once ign-rendering is disabling the -// warnings -#ifdef _WIN32 -#pragma warning(push) -#pragma warning(disable: 4251) -#endif #include #include #include #include -#ifdef _WIN32 -#pragma warning(pop) -#endif #include "ignition/sensors/ImageGaussianNoiseModel.hh" diff --git a/src/ImuSensor.cc b/src/ImuSensor.cc index 702220f6..90536618 100644 --- a/src/ImuSensor.cc +++ b/src/ImuSensor.cc @@ -14,14 +14,14 @@ * limitations under the License. * */ -#ifdef _WIN32 -#pragma warning(push) -#pragma warning(disable: 4005) -#pragma warning(disable: 4251) +#if defined(_MSC_VER) + #pragma warning(push) + #pragma warning(disable: 4005) + #pragma warning(disable: 4251) #endif #include -#ifdef _WIN32 -#pragma warning(pop) +#if defined(_MSC_VER) + #pragma warning(pop) #endif #include diff --git a/src/ImuSensor_TEST.cc b/src/ImuSensor_TEST.cc index 5eda3801..5614ba4f 100644 --- a/src/ImuSensor_TEST.cc +++ b/src/ImuSensor_TEST.cc @@ -14,19 +14,20 @@ * limitations under the License. * */ +#if defined(_MSC_VER) + #pragma warning(push) + #pragma warning(disable: 4005) + #pragma warning(disable: 4251) +#endif +#include +#if defined(_MSC_VER) + #pragma warning(pop) +#endif + #include #include #include -#ifdef _WIN32 -#pragma warning(push) -#pragma warning(disable: 4005) -#pragma warning(disable: 4251) -#endif -#include -#ifdef _WIN32 -#pragma warning(pop) -#endif #include #include diff --git a/src/Lidar.cc b/src/Lidar.cc index 32a2204a..eac868c9 100644 --- a/src/Lidar.cc +++ b/src/Lidar.cc @@ -14,6 +14,16 @@ * limitations under the License. * */ +#if defined(_MSC_VER) + #pragma warning(push) + #pragma warning(disable: 4005) + #pragma warning(disable: 4251) +#endif +#include +#if defined(_MSC_VER) + #pragma warning(pop) +#endif + #include #include #include diff --git a/src/Lidar_TEST.cc b/src/Lidar_TEST.cc index c35f004b..c93efc40 100644 --- a/src/Lidar_TEST.cc +++ b/src/Lidar_TEST.cc @@ -14,20 +14,20 @@ * limitations under the License. * */ +#if defined(_MSC_VER) + #pragma warning(push) + #pragma warning(disable: 4005) + #pragma warning(disable: 4251) +#endif +#include +#if defined(_MSC_VER) + #pragma warning(pop) +#endif #include #include #include #include -#ifdef _WIN32 -#pragma warning(push) -#pragma warning(disable: 4005) -#pragma warning(disable: 4251) -#endif -#include -#ifdef _WIN32 -#pragma warning(pop) -#endif #include #include @@ -140,6 +140,8 @@ TEST(Lidar_TEST, CreateLaser) ignition::sensors::Lidar *sensor = mgr.CreateSensor( lidarSDF); + EXPECT_FALSE(sensor->CreateLidar()); + // Make sure the above dynamic cast worked. ASSERT_NE(nullptr, sensor); @@ -162,6 +164,70 @@ TEST(Lidar_TEST, CreateLaser) EXPECT_TRUE(sensor->IsActive()); } +TEST(Lidar_TEST, CreateLaserFailures) +{ + sdf::Sensor sdfSensor; + sdfSensor.SetType(sdf::SensorType::CAMERA); + sdf::Lidar sdfLidarSensor; + + ignition::sensors::Lidar sensor; + EXPECT_FALSE(sensor.Load(sdfSensor)); + + EXPECT_DOUBLE_EQ(0.0, sensor.Range(-1)); + EXPECT_DOUBLE_EQ(0.0, sensor.Range(0)); + + EXPECT_FALSE(sensor.IsHorizontal()); + EXPECT_DOUBLE_EQ(640, sensor.RangeCountRatio()); + sensor.SetAngleMax(0.707); + sensor.SetAngleMin(-0.707); + EXPECT_DOUBLE_EQ(-0.707, sensor.AngleMin().Radian()); + EXPECT_DOUBLE_EQ(0.707, sensor.AngleMax().Radian()); + + sensor.SetVerticalAngleMax(0.707); + sensor.SetVerticalAngleMin(-0.707); + EXPECT_DOUBLE_EQ(-0.707, sensor.VerticalAngleMin().Radian()); + EXPECT_DOUBLE_EQ(0.707, sensor.VerticalAngleMax().Radian()); + + sensor.ApplyNoise(); + + sdfSensor.SetType(sdf::SensorType::LIDAR); + sdfSensor.SetLidarSensor(sdfLidarSensor); + + ignition::sensors::Lidar sensor2; + + EXPECT_TRUE(sensor2.Load(sdfSensor)); + EXPECT_FALSE(sensor2.Load(sdfSensor)); + + ignition::sensors::Lidar sensor3; + + sdfLidarSensor.SetHorizontalScanSamples(0); + sdfSensor.SetLidarSensor(sdfLidarSensor); + EXPECT_TRUE(sensor3.Load(sdfSensor)); + + sdfLidarSensor.SetHorizontalScanSamples(20); + + sdf::Noise noise; + noise.SetType(sdf::NoiseType::GAUSSIAN); + noise.SetMean(1.2); + noise.SetStdDev(2.3); + noise.SetBiasMean(4.5); + noise.SetBiasStdDev(6.7); + noise.SetPrecision(8.9); + + sdfLidarSensor.SetLidarNoise(noise); + sdfSensor.SetLidarSensor(sdfLidarSensor); + ignition::sensors::Lidar sensor4; + EXPECT_TRUE(sensor4.Load(sdfSensor)); + + noise.SetType(sdf::NoiseType::GAUSSIAN_QUANTIZED); + sdfLidarSensor.SetLidarNoise(noise); + sdfSensor.SetLidarSensor(sdfLidarSensor); + ignition::sensors::Lidar sensor5; + EXPECT_TRUE(sensor5.Load(sdfSensor)); + + sensor.Update(ignition::common::Time(0.1)); +} + ////////////////////////////////////////////////// int main(int argc, char **argv) { diff --git a/src/LogicalCameraSensor.cc b/src/LogicalCameraSensor.cc index a99fc548..943d1940 100644 --- a/src/LogicalCameraSensor.cc +++ b/src/LogicalCameraSensor.cc @@ -14,6 +14,15 @@ * limitations under the License. * */ +#if defined(_MSC_VER) + #pragma warning(push) + #pragma warning(disable: 4005) + #pragma warning(disable: 4251) +#endif +#include +#if defined(_MSC_VER) + #pragma warning(pop) +#endif #include diff --git a/src/MagnetometerSensor.cc b/src/MagnetometerSensor.cc index 3fbd4c72..4ea4ef10 100644 --- a/src/MagnetometerSensor.cc +++ b/src/MagnetometerSensor.cc @@ -14,15 +14,14 @@ * limitations under the License. * */ - -#ifdef _WIN32 -#pragma warning(push) -#pragma warning(disable: 4005) -#pragma warning(disable: 4251) +#if defined(_MSC_VER) + #pragma warning(push) + #pragma warning(disable: 4005) + #pragma warning(disable: 4251) #endif -#include -#ifdef _WIN32 -#pragma warning(pop) +#include +#if defined(_MSC_VER) + #pragma warning(pop) #endif #include diff --git a/src/PointCloudUtil.hh b/src/PointCloudUtil.hh index efcaea1e..adeac656 100644 --- a/src/PointCloudUtil.hh +++ b/src/PointCloudUtil.hh @@ -18,14 +18,14 @@ #ifndef IGNITION_SENSORS_POINTCLOUDUTIL_HH_ #define IGNITION_SENSORS_POINTCLOUDUTIL_HH_ -#ifdef _WIN32 -#pragma warning(push) -#pragma warning(disable: 4005) -#pragma warning(disable: 4251) +#if defined(_MSC_VER) + #pragma warning(push) + #pragma warning(disable: 4005) + #pragma warning(disable: 4251) #endif #include -#ifdef _WIN32 -#pragma warning(pop) +#if defined(_MSC_VER) + #pragma warning(pop) #endif #include diff --git a/src/RenderingSensor.cc b/src/RenderingSensor.cc index ef99bcfe..7744dc5f 100644 --- a/src/RenderingSensor.cc +++ b/src/RenderingSensor.cc @@ -17,16 +17,7 @@ #include -// TODO(louise) Remove these pragmas once ign-rendering is disabling the -// warnings -#ifdef _WIN32 -#pragma warning(push) -#pragma warning(disable: 4251) -#endif #include -#ifdef _WIN32 -#pragma warning(pop) -#endif #include "ignition/sensors/RenderingSensor.hh" @@ -112,4 +103,3 @@ void RenderingSensor::Render() } } } - diff --git a/src/RgbdCameraSensor.cc b/src/RgbdCameraSensor.cc index 2e0b19da..635cd04f 100644 --- a/src/RgbdCameraSensor.cc +++ b/src/RgbdCameraSensor.cc @@ -15,32 +15,23 @@ * */ -#ifdef _WIN32 -#pragma warning(push) -#pragma warning(disable: 4005) -#pragma warning(disable: 4251) -#endif +#if defined(_MSC_VER) + #pragma warning(push) + #pragma warning(disable: 4005) + #pragma warning(disable: 4251) #include +#endif #include -#ifdef _WIN32 -#pragma warning(pop) +#if defined(_MSC_VER) + #pragma warning(pop) #endif #include #include #include -// TODO(louise) Remove these pragmas once ign-rendering is disabling the -// warnings -#ifdef _WIN32 -#pragma warning(push) -#pragma warning(disable: 4251) -#endif #include #include -#ifdef _WIN32 -#pragma warning(pop) -#endif #include diff --git a/src/Sensor_TEST.cc b/src/Sensor_TEST.cc index edfb41f5..5f9faffe 100644 --- a/src/Sensor_TEST.cc +++ b/src/Sensor_TEST.cc @@ -14,6 +14,16 @@ * limitations under the License. * */ +#if defined(_MSC_VER) + #pragma warning(push) + #pragma warning(disable: 4005) + #pragma warning(disable: 4251) +#endif +#include +#if defined(_MSC_VER) + #pragma warning(pop) +#endif + #include #include @@ -21,16 +31,6 @@ #include -#ifdef _WIN32 -#pragma warning(push) -#pragma warning(disable: 4005) -#pragma warning(disable: 4251) -#endif -#include -#ifdef _WIN32 -#pragma warning(pop) -#endif - #include #include #include diff --git a/src/ThermalCameraSensor.cc b/src/ThermalCameraSensor.cc index d7093538..d9f5d1f3 100644 --- a/src/ThermalCameraSensor.cc +++ b/src/ThermalCameraSensor.cc @@ -15,14 +15,14 @@ * */ -#ifdef _WIN32 -#pragma warning(push) -#pragma warning(disable: 4005) -#pragma warning(disable: 4251) +#if defined(_MSC_VER) + #pragma warning(push) + #pragma warning(disable: 4005) + #pragma warning(disable: 4251) #endif #include -#ifdef _WIN32 -#pragma warning(pop) +#if defined(_MSC_VER) + #pragma warning(pop) #endif #include From 4a99c5f82ad541b31aa7ffc457e311c3de5673f2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 26 Oct 2022 23:00:55 +0200 Subject: [PATCH 7/9] Improved noise coverage (#278) Signed-off-by: ahcorde --- src/Noise_TEST.cc | 28 ++++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/src/Noise_TEST.cc b/src/Noise_TEST.cc index 1019f22c..b15aba84 100644 --- a/src/Noise_TEST.cc +++ b/src/Noise_TEST.cc @@ -22,6 +22,9 @@ #include #include +#include + + #include "ignition/sensors/Noise.hh" #include "ignition/sensors/GaussianNoiseModel.hh" @@ -414,6 +417,31 @@ TEST(NoiseTest, OnApplyNoise) } } +///////////////////////////////////////////////// +TEST(NoiseTest, NoiseFailures) +{ + sensors::Noise noise(sensors::NoiseType::CUSTOM); + + EXPECT_DOUBLE_EQ(9, noise.Apply(9, 0.1)); + EXPECT_DOUBLE_EQ(9, noise.ApplyImpl(9, 0.1)); + std::ostringstream out; + noise.Print(out); + EXPECT_EQ("Noise with type[1] does not have an overloaded Print function. " + "No more information is available.", out.str()); + + sensors::Noise noiseGaussian(sensors::NoiseType::GAUSSIAN); + + sensors::NoisePtr noiseFactory = + sensors::NoiseFactory::NewNoiseModel( + NoiseSdf("gaussian", 0, 0, 0, 0, 0), "camera"); + + sdf::Noise sdfNoise; + sdfNoise.SetType(static_cast(99)); + sensors::NoisePtr noiseFactory2 = + sensors::NoiseFactory::NewNoiseModel( + sdfNoise, "camera"); +} + ///////////////////////////////////////////////// int main(int argc, char **argv) { From d090f914a23ad023d58052c3909a90aac7791dba Mon Sep 17 00:00:00 2001 From: Brian Date: Tue, 1 Nov 2022 18:35:03 -0400 Subject: [PATCH 8/9] Camera: configure projection matrix from SDFormat (#249) Signed-off-by: Brian Chen * macOS testing: fix DYLD_LIBRARY_PATH * Test intrinsics and projection parameters Confirm that intrinsics and projection parameters in CameraInfo message match the values from SDFormat. Signed-off-by: Steve Peters Co-authored-by: Steve Peters --- src/CMakeLists.txt | 2 +- src/CameraSensor.cc | 13 +++-- src/Camera_TEST.cc | 8 +++ test/integration/CMakeLists.txt | 2 +- test/integration/camera_plugin.cc | 62 ++++++++++++++++++++++ test/integration/camera_sensor_builtin.sdf | 28 ++++++++++ 6 files changed, 106 insertions(+), 9 deletions(-) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 5581a236..7e054470 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -177,7 +177,7 @@ foreach(plugin_test ${plugin_test_targets}) if(TARGET ${plugin_test}) set(_env_vars) list(APPEND _env_vars "IGN_PLUGIN_PATH=$") - list(APPEND _env_vars "DYLD_LIBRARY_PATH=${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}") + list(APPEND _env_vars "DYLD_LIBRARY_PATH=$") set_tests_properties(${plugin_test} PROPERTIES ENVIRONMENT "${_env_vars}") diff --git a/src/CameraSensor.cc b/src/CameraSensor.cc index f40ab556..5429ab01 100644 --- a/src/CameraSensor.cc +++ b/src/CameraSensor.cc @@ -568,19 +568,18 @@ void CameraSensor::PopulateInfo(const sdf::Camera *_cameraSdf) intrinsics->add_k(0.0); intrinsics->add_k(1.0); - // TODO(anyone) Get tx and ty from SDF msgs::CameraInfo::Projection *proj = this->dataPtr->infoMsg.mutable_projection(); - proj->add_p(_cameraSdf->LensIntrinsicsFx()); + proj->add_p(_cameraSdf->LensProjectionFx()); proj->add_p(0.0); - proj->add_p(_cameraSdf->LensIntrinsicsCx()); - proj->add_p(-_cameraSdf->LensIntrinsicsFx() * this->dataPtr->baseline); + proj->add_p(_cameraSdf->LensProjectionCx()); + proj->add_p(_cameraSdf->LensProjectionTx()); proj->add_p(0.0); - proj->add_p(_cameraSdf->LensIntrinsicsFy()); - proj->add_p(_cameraSdf->LensIntrinsicsCy()); - proj->add_p(0.0); + proj->add_p(_cameraSdf->LensProjectionFy()); + proj->add_p(_cameraSdf->LensProjectionCy()); + proj->add_p(_cameraSdf->LensProjectionTy()); proj->add_p(0.0); proj->add_p(0.0); diff --git a/src/Camera_TEST.cc b/src/Camera_TEST.cc index e2fa1284..b148def7 100644 --- a/src/Camera_TEST.cc +++ b/src/Camera_TEST.cc @@ -107,6 +107,14 @@ sdf::ElementPtr CameraToSdf(const std::string &_type, << " 124" << " 1.2" << " " + << " " + << " 282" + << " 283" + << " 163" + << " 125" + << " 1" + << " 2" + << " " << " " << " " << " " diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 2c5de9fe..c0de2b96 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -59,7 +59,7 @@ foreach(plugin_test ${dri_tests} ${tests}) if(TARGET ${BINARY_NAME}) set(_env_vars) list(APPEND _env_vars "IGN_PLUGIN_PATH=$") - list(APPEND _env_vars "DYLD_LIBRARY_PATH=${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}") + list(APPEND _env_vars "DYLD_LIBRARY_PATH=$") set_tests_properties(${BINARY_NAME} PROPERTIES ENVIRONMENT "${_env_vars}") diff --git a/test/integration/camera_plugin.cc b/test/integration/camera_plugin.cc index 8b8320a5..48adae79 100644 --- a/test/integration/camera_plugin.cc +++ b/test/integration/camera_plugin.cc @@ -18,8 +18,10 @@ #include #include +#include #include #include +#include // TODO(louise) Remove these pragmas once ign-rendering is disabling the // warnings @@ -48,6 +50,20 @@ #include "TransportTestTools.hh" +std::mutex g_infoMutex; +unsigned int g_infoCounter = 0; +ignition::msgs::CameraInfo g_infoMsg; + +////////////////////////////////////////////////// +void OnCameraInfo(const ignition::msgs::CameraInfo & _msg) +{ + g_infoMutex.lock(); + g_infoCounter++; + g_infoMsg.CopyFrom(_msg); + g_infoMutex.unlock(); +} + +////////////////////////////////////////////////// class CameraSensorTest: public testing::Test, public testing::WithParamInterface { @@ -55,6 +71,7 @@ class CameraSensorTest: public testing::Test, public: void ImagesWithBuiltinSDF(const std::string &_renderEngine); }; +////////////////////////////////////////////////// void CameraSensorTest::ImagesWithBuiltinSDF(const std::string &_renderEngine) { // get the darn test data @@ -97,6 +114,12 @@ void CameraSensorTest::ImagesWithBuiltinSDF(const std::string &_renderEngine) EXPECT_EQ(std::string("base_camera"), sensor->FrameId()); + // subscribe to the camera info topic + std::string infoTopic = sensor->InfoTopic(); + ignition::transport::Node node; + node.Subscribe(infoTopic, &OnCameraInfo); + WaitForMessageTestHelper helperInfo(infoTopic); + std::string topic = "/test/integration/CameraPlugin_imagesWithBuiltinSDF"; WaitForMessageTestHelper helper(topic); @@ -104,6 +127,45 @@ void CameraSensorTest::ImagesWithBuiltinSDF(const std::string &_renderEngine) mgr.RunOnce(ignition::common::Time::Zero); EXPECT_TRUE(helper.WaitForMessage()) << helper; + EXPECT_TRUE(helperInfo.WaitForMessage()) << helperInfo; + + // Check CameraInfo properties + ignition::msgs::CameraInfo infoMsg; + g_infoMutex.lock(); + EXPECT_EQ(1u, g_infoCounter); + infoMsg.CopyFrom(g_infoMsg); + g_infoMutex.unlock(); + + { + auto intrinsics = infoMsg.intrinsics(); + EXPECT_EQ(9, intrinsics.k_size()); + EXPECT_DOUBLE_EQ(280.0, intrinsics.k(0)); + EXPECT_DOUBLE_EQ(0.0, intrinsics.k(1)); + EXPECT_DOUBLE_EQ(162.0, intrinsics.k(2)); + EXPECT_DOUBLE_EQ(0.0, intrinsics.k(3)); + EXPECT_DOUBLE_EQ(281.0, intrinsics.k(4)); + EXPECT_DOUBLE_EQ(124.0, intrinsics.k(5)); + EXPECT_DOUBLE_EQ(0.0, intrinsics.k(6)); + EXPECT_DOUBLE_EQ(0.0, intrinsics.k(7)); + EXPECT_DOUBLE_EQ(1.0, intrinsics.k(8)); + } + + { + auto projection = infoMsg.projection(); + EXPECT_EQ(12, projection.p_size()); + EXPECT_DOUBLE_EQ(282.0, projection.p(0)); + EXPECT_DOUBLE_EQ(0.0, projection.p(1)); + EXPECT_DOUBLE_EQ(163.0, projection.p(2)); + EXPECT_DOUBLE_EQ(1.0, projection.p(3)); + EXPECT_DOUBLE_EQ(0.0, projection.p(4)); + EXPECT_DOUBLE_EQ(283.0, projection.p(5)); + EXPECT_DOUBLE_EQ(125.0, projection.p(6)); + EXPECT_DOUBLE_EQ(2.0, projection.p(7)); + EXPECT_DOUBLE_EQ(0.0, projection.p(8)); + EXPECT_DOUBLE_EQ(0.0, projection.p(9)); + EXPECT_DOUBLE_EQ(1.0, projection.p(10)); + EXPECT_DOUBLE_EQ(0.0, projection.p(11)); + } // Clean up engine->DestroyScene(scene); diff --git a/test/integration/camera_sensor_builtin.sdf b/test/integration/camera_sensor_builtin.sdf index 532ed003..12662eba 100644 --- a/test/integration/camera_sensor_builtin.sdf +++ b/test/integration/camera_sensor_builtin.sdf @@ -21,6 +21,34 @@ 0.0 0.007 + + custom + false + + 1.1 + 2.2 + 3.3 + 1.2 + sin + + 0.7505 + 128 + + 280 + 281 + 162 + 124 + 1.2 + + + 282 + 283 + 163 + 125 + 1 + 2 + + From 5a82b76ba4c9e8101443e3574c93a80204d89d28 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 2 Nov 2022 07:20:49 -0700 Subject: [PATCH 9/9] RgbdCameraSensor.cc: fix include (#280) Signed-off-by: Steve Peters --- src/RgbdCameraSensor.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/RgbdCameraSensor.cc b/src/RgbdCameraSensor.cc index 635cd04f..71f97d0f 100644 --- a/src/RgbdCameraSensor.cc +++ b/src/RgbdCameraSensor.cc @@ -19,8 +19,8 @@ #pragma warning(push) #pragma warning(disable: 4005) #pragma warning(disable: 4251) -#include #endif +#include #include #if defined(_MSC_VER) #pragma warning(pop)