From d49310b2d98f76f589dc86c49cae45adc91636ec Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 10 Jul 2024 09:24:11 -0700 Subject: [PATCH 1/3] Backport frame id fixes (#446) Signed-off-by: Ian Chen --- src/DopplerVelocityLog.cc | 6 +++++ src/GpuLidarSensor.cc | 2 +- src/Sensor.cc | 10 ++++---- test/integration/dvl.cc | 23 +++++++++++++++++ test/integration/gpu_lidar_sensor.cc | 37 +++++++++++++++++++--------- 5 files changed, 60 insertions(+), 18 deletions(-) diff --git a/src/DopplerVelocityLog.cc b/src/DopplerVelocityLog.cc index ded9e25e..77414f49 100644 --- a/src/DopplerVelocityLog.cc +++ b/src/DopplerVelocityLog.cc @@ -1945,6 +1945,9 @@ namespace gz if (this->dataPtr->publishingEstimates) { auto * headerMessage = bottomModeMessage.mutable_header(); + auto frame = headerMessage->add_data(); + frame->set_key("frame_id"); + frame->add_value(this->FrameId()); this->AddSequence(headerMessage, "doppler_velocity_log"); this->dataPtr->pub.Publish(bottomModeMessage); } @@ -1964,6 +1967,9 @@ namespace gz if (this->dataPtr->publishingEstimates) { auto * headerMessage = waterMassModeMessage.mutable_header(); + auto frame = headerMessage->add_data(); + frame->set_key("frame_id"); + frame->add_value(this->FrameId()); this->AddSequence(headerMessage, "doppler_velocity_log"); this->dataPtr->pub.Publish(waterMassModeMessage); } diff --git a/src/GpuLidarSensor.cc b/src/GpuLidarSensor.cc index 0e98cddc..83e44d0d 100644 --- a/src/GpuLidarSensor.cc +++ b/src/GpuLidarSensor.cc @@ -134,7 +134,7 @@ bool GpuLidarSensor::Load(const sdf::Sensor &_sdf) // by ROS1: https://github.com/ros/common_msgs/pull/77. Ideally, memory // alignment should be configured. This same problem is in the // RgbdCameraSensor. - msgs::InitPointCloudPacked(this->dataPtr->pointMsg, this->Name(), true, + msgs::InitPointCloudPacked(this->dataPtr->pointMsg, this->FrameId(), true, {{"xyz", msgs::PointCloudPacked::Field::FLOAT32}, {"intensity", msgs::PointCloudPacked::Field::FLOAT32}, {"ring", msgs::PointCloudPacked::Field::UINT16}}); diff --git a/src/Sensor.cc b/src/Sensor.cc index 326f79f4..b7dbba91 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -138,7 +138,7 @@ class gz::sensors::SensorPrivate public: std::map sequences; /// \brief frame id - public: std::string frame_id; + public: std::string frameId; /// \brief If sensor is active or not. public: bool active = true; @@ -187,11 +187,11 @@ bool SensorPrivate::PopulateFromSDF(const sdf::Sensor &_sdf) { if (element->HasElement("gz_frame_id")) { - this->frame_id = element->Get("gz_frame_id"); + this->frameId = element->Get("gz_frame_id"); } else { - this->frame_id = this->name; + this->frameId = this->name; } } @@ -288,13 +288,13 @@ std::string Sensor::Name() const ////////////////////////////////////////////////// std::string Sensor::FrameId() const { - return this->dataPtr->frame_id; + return this->dataPtr->frameId; } ////////////////////////////////////////////////// void Sensor::SetFrameId(const std::string &_frameId) { - this->dataPtr->frame_id = _frameId; + this->dataPtr->frameId = _frameId; } ////////////////////////////////////////////////// diff --git a/test/integration/dvl.cc b/test/integration/dvl.cc index 814d5ccb..5c1c1df2 100644 --- a/test/integration/dvl.cc +++ b/test/integration/dvl.cc @@ -45,6 +45,7 @@ using DopplerVelocityLog = sensors::DopplerVelocityLog; struct DVLConfig { std::string name = "dvl"; + std::string frameId = "dvl_frame"; std::string topic = "/gz/sensors/test/dvl"; double updateRate = 30; // Hz @@ -75,6 +76,7 @@ sdf::ElementPtr MakeDVLSdf(const DVLConfig &_config) << " " << " " << " " + << " " << _config.frameId << "" << " " << _config.alwaysOn << "" << " " << _config.updateRate << "" << " " << _config.topic << "" @@ -345,6 +347,13 @@ TEST_P(DopplerVelocityLogTest, BottomTrackingWhileStatic) } EXPECT_EQ(0, message.status()); + // check frame id + EXPECT_TRUE(message.has_header()); + EXPECT_LT(1, message.header().data().size()); + EXPECT_EQ("frame_id", message.header().data(0).key()); + ASSERT_EQ(1, message.header().data(0).value().size()); + EXPECT_EQ("dvl_frame", message.header().data(0).value(0)); + this->manager.Remove(sensor->Id()); } @@ -436,6 +445,13 @@ TEST_P(DopplerVelocityLogTest, WaterMassTrackingWhileStatic) } EXPECT_EQ(0, message.status()); + // check frame id + EXPECT_TRUE(message.has_header()); + EXPECT_LT(1, message.header().data().size()); + EXPECT_EQ("frame_id", message.header().data(0).key()); + ASSERT_EQ(1, message.header().data(0).value().size()); + EXPECT_EQ("dvl_frame", message.header().data(0).value(0)); + this->manager.Remove(sensor->Id()); } @@ -519,6 +535,13 @@ TEST_P(DopplerVelocityLogTest, BottomTrackingWhileInMotion) } EXPECT_EQ(0, message.status()); + // check frame id + EXPECT_TRUE(message.has_header()); + EXPECT_LT(1, message.header().data().size()); + EXPECT_EQ("frame_id", message.header().data(0).key()); + ASSERT_EQ(1, message.header().data(0).value().size()); + EXPECT_EQ("dvl_frame", message.header().data(0).value(0)); + this->manager.Remove(sensor->Id()); } diff --git a/test/integration/gpu_lidar_sensor.cc b/test/integration/gpu_lidar_sensor.cc index a0753b40..22ef6c7f 100644 --- a/test/integration/gpu_lidar_sensor.cc +++ b/test/integration/gpu_lidar_sensor.cc @@ -65,7 +65,7 @@ sdf::ElementPtr GpuLidarToSdf(const std::string &name, const double vertResolution, const double vertMinAngle, const double vertMaxAngle, const double rangeResolution, const double rangeMin, const double rangeMax, const bool alwaysOn, - const bool visualize) + const bool visualize, const std::string &frameId) { std::ostringstream stream; stream @@ -74,6 +74,7 @@ sdf::ElementPtr GpuLidarToSdf(const std::string &name, << " " << " " << " " + << " " << frameId << "" << " " << pose << "" << " " << topic << "" << " "<< updateRate <<"" @@ -194,6 +195,7 @@ void GpuLidarSensorTest::CreateGpuLidar(const std::string &_renderEngine) const double rangeMax = 10.0; const bool alwaysOn = 1; const bool visualize = 1; + const std::string frameId = "TestGpuLidar_frame"; // Create sensor description in SDF gz::math::Pose3d testPose(gz::math::Vector3d(0, 0, 0.1), @@ -201,7 +203,7 @@ void GpuLidarSensorTest::CreateGpuLidar(const std::string &_renderEngine) sdf::ElementPtr lidarSdf = GpuLidarToSdf(name, testPose, updateRate, topic, horzSamples, horzResolution, horzMinAngle, horzMaxAngle, vertSamples, vertResolution, vertMinAngle, vertMaxAngle, - rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); + rangeResolution, rangeMin, rangeMax, alwaysOn, visualize, frameId); // Setup gz-rendering with an empty scene auto *engine = gz::rendering::engine(_renderEngine); @@ -313,6 +315,7 @@ void GpuLidarSensorTest::DetectBox(const std::string &_renderEngine) const double rangeMax = 10.0; const bool alwaysOn = 1; const bool visualize = 1; + const std::string frameId = "TestGpuLidar_frame"; // Create sensor SDF gz::math::Pose3d testPose(gz::math::Vector3d(0.0, 0.0, 0.1), @@ -320,7 +323,7 @@ void GpuLidarSensorTest::DetectBox(const std::string &_renderEngine) sdf::ElementPtr lidarSdf = GpuLidarToSdf(name, testPose, updateRate, topic, horzSamples, horzResolution, horzMinAngle, horzMaxAngle, vertSamples, vertResolution, vertMinAngle, vertMaxAngle, - rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); + rangeResolution, rangeMin, rangeMax, alwaysOn, visualize, frameId); // Create and populate scene gz::rendering::RenderEngine *engine = @@ -399,7 +402,7 @@ void GpuLidarSensorTest::DetectBox(const std::string &_renderEngine) LASER_TOL); EXPECT_DOUBLE_EQ(laserMsgs.back().ranges(last), gz::math::INF_D); - EXPECT_EQ(laserMsgs.back().frame(), name); + EXPECT_EQ(laserMsgs.back().frame(), frameId); EXPECT_NEAR(laserMsgs.back().angle_min(), horzMinAngle, 1e-4); EXPECT_NEAR(laserMsgs.back().angle_max(), horzMaxAngle, 1e-4); EXPECT_NEAR(laserMsgs.back().count(), horzSamples, 1e-4); @@ -425,6 +428,12 @@ void GpuLidarSensorTest::DetectBox(const std::string &_renderEngine) EXPECT_FALSE(pointMsgs.back().is_dense()); EXPECT_EQ(32u * horzSamples * vertSamples, pointMsgs.back().data().size()); + EXPECT_TRUE(pointMsgs.back().has_header()); + EXPECT_LT(1, pointMsgs.back().header().data().size()); + EXPECT_EQ("frame_id", pointMsgs.back().header().data(0).key()); + ASSERT_EQ(1, pointMsgs.back().header().data(0).value().size()); + EXPECT_EQ(frameId, pointMsgs.back().header().data(0).value(0)); + // Clean up rendering ptrs visualBox1.reset(); @@ -462,6 +471,7 @@ void GpuLidarSensorTest::TestThreeBoxes(const std::string &_renderEngine) const double rangeMax = 10.0; const bool alwaysOn = 1; const bool visualize = 1; + const std::string frameId = "TestGpuLidar_frame"; // Create sensor SDF gz::math::Pose3d testPose1(gz::math::Vector3d(0, 0, 0.1), @@ -469,7 +479,7 @@ void GpuLidarSensorTest::TestThreeBoxes(const std::string &_renderEngine) sdf::ElementPtr lidarSdf1 = GpuLidarToSdf(name1, testPose1, updateRate, topic1, horzSamples, horzResolution, horzMinAngle, horzMaxAngle, vertSamples, vertResolution, vertMinAngle, vertMaxAngle, - rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); + rangeResolution, rangeMin, rangeMax, alwaysOn, visualize, frameId); // Create a second sensor SDF rotated gz::math::Pose3d testPose2(gz::math::Vector3d(0, 0, 0.1), @@ -477,7 +487,7 @@ void GpuLidarSensorTest::TestThreeBoxes(const std::string &_renderEngine) sdf::ElementPtr lidarSdf2 = GpuLidarToSdf(name2, testPose2, updateRate, topic2, horzSamples, horzResolution, horzMinAngle, horzMaxAngle, vertSamples, vertResolution, vertMinAngle, vertMaxAngle, - rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); + rangeResolution, rangeMin, rangeMax, alwaysOn, visualize, frameId); // Create and populate scene gz::rendering::RenderEngine *engine = @@ -618,6 +628,7 @@ void GpuLidarSensorTest::VerticalLidar(const std::string &_renderEngine) const double rangeMax = 10.0; const bool alwaysOn = 1; const bool visualize = 1; + const std::string frameId = "TestGpuLidar_frame"; // Create sensor SDF gz::math::Pose3d testPose(gz::math::Vector3d(0.25, 0.0, 0.5), @@ -625,7 +636,7 @@ void GpuLidarSensorTest::VerticalLidar(const std::string &_renderEngine) sdf::ElementPtr lidarSdf = GpuLidarToSdf(name, testPose, updateRate, topic, horzSamples, horzResolution, horzMinAngle, horzMaxAngle, vertSamples, vertResolution, vertMinAngle, vertMaxAngle, - rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); + rangeResolution, rangeMin, rangeMax, alwaysOn, visualize, frameId); // Create and populate scene gz::rendering::RenderEngine *engine = @@ -745,6 +756,7 @@ void GpuLidarSensorTest::ManualUpdate(const std::string &_renderEngine) const double rangeMax = 10.0; const bool alwaysOn = 1; const bool visualize = 1; + const std::string frameId = "TestGpuLidar_frame"; // Create sensor SDF gz::math::Pose3d testPose1(gz::math::Vector3d(0, 0, 0.1), @@ -752,7 +764,7 @@ void GpuLidarSensorTest::ManualUpdate(const std::string &_renderEngine) sdf::ElementPtr lidarSdf1 = GpuLidarToSdf(name1, testPose1, updateRate, topic1, horzSamples, horzResolution, horzMinAngle, horzMaxAngle, vertSamples, vertResolution, vertMinAngle, vertMaxAngle, - rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); + rangeResolution, rangeMin, rangeMax, alwaysOn, visualize, frameId); // Create a second sensor SDF at an xy offset of 1 gz::math::Pose3d testPose2(gz::math::Vector3d(1, 1, 0.1), @@ -760,7 +772,7 @@ void GpuLidarSensorTest::ManualUpdate(const std::string &_renderEngine) sdf::ElementPtr lidarSdf2 = GpuLidarToSdf(name2, testPose2, updateRate, topic2, horzSamples, horzResolution, horzMinAngle, horzMaxAngle, vertSamples, vertResolution, vertMinAngle, vertMaxAngle, - rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); + rangeResolution, rangeMin, rangeMax, alwaysOn, visualize, frameId); // Create and populate scene gz::rendering::RenderEngine *engine = @@ -871,6 +883,7 @@ void GpuLidarSensorTest::Topic(const std::string &_renderEngine) const bool alwaysOn = 1; const bool visualize = 1; auto testPose = gz::math::Pose3d(); + const std::string frameId = "TestGpuLidar_frame"; // Scene auto engine = gz::rendering::engine(_renderEngine); @@ -892,7 +905,7 @@ void GpuLidarSensorTest::Topic(const std::string &_renderEngine) auto lidarSdf = GpuLidarToSdf(name, testPose, updateRate, topic, horzSamples, horzResolution, horzMinAngle, horzMaxAngle, vertSamples, vertResolution, vertMinAngle, vertMaxAngle, - rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); + rangeResolution, rangeMin, rangeMax, alwaysOn, visualize, frameId); auto lidar = mgr.CreateSensor(lidarSdf); ASSERT_NE(nullptr, lidar); @@ -906,7 +919,7 @@ void GpuLidarSensorTest::Topic(const std::string &_renderEngine) auto lidarSdf = GpuLidarToSdf(name, testPose, updateRate, topic, horzSamples, horzResolution, horzMinAngle, horzMaxAngle, vertSamples, vertResolution, vertMinAngle, vertMaxAngle, - rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); + rangeResolution, rangeMin, rangeMax, alwaysOn, visualize, frameId); auto lidar = mgr.CreateSensor(lidarSdf); ASSERT_NE(nullptr, lidar); @@ -921,7 +934,7 @@ void GpuLidarSensorTest::Topic(const std::string &_renderEngine) auto lidarSdf = GpuLidarToSdf(name, testPose, updateRate, topic, horzSamples, horzResolution, horzMinAngle, horzMaxAngle, vertSamples, vertResolution, vertMinAngle, vertMaxAngle, - rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); + rangeResolution, rangeMin, rangeMax, alwaysOn, visualize, frameId); auto sensor = mgr.CreateSensor(lidarSdf); EXPECT_EQ(nullptr, sensor); From f8a1a2ee07c625a5fab464a3253cb9f2b0ce5933 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 15 Jul 2024 13:56:15 -0700 Subject: [PATCH 2/3] Publish lidar scan only if there are lidar scan connections (#447) Signed-off-by: Ian Chen --- include/gz/sensors/Lidar.hh | 7 +++ src/Lidar.cc | 88 ++++++++++++++++++++-------- test/integration/gpu_lidar_sensor.cc | 8 +++ 3 files changed, 79 insertions(+), 24 deletions(-) diff --git a/include/gz/sensors/Lidar.hh b/include/gz/sensors/Lidar.hh index 5a4edf46..2489a20e 100644 --- a/include/gz/sensors/Lidar.hh +++ b/include/gz/sensors/Lidar.hh @@ -244,6 +244,13 @@ namespace gz /// \return Visibility mask public: uint32_t VisibilityMask() const; + /// \brief Clamp a finite range value to min / max range. + /// +/-inf values will not be clamped because they mean lidar returns are + /// outside the detectable range. + /// NAN values will be clamped to max range. + /// \return Clamped range value. + private: double Clamp(double _range) const; + GZ_UTILS_WARN_IGNORE__DLL_INTERFACE_MISSING /// \brief Just a mutex for thread safety public: mutable std::mutex lidarMutex; diff --git a/src/Lidar.cc b/src/Lidar.cc index 57ad3fef..5a1e8a03 100644 --- a/src/Lidar.cc +++ b/src/Lidar.cc @@ -14,6 +14,9 @@ * limitations under the License. * */ + +#include + #if defined(_MSC_VER) #pragma warning(push) #pragma warning(disable: 4005) @@ -56,6 +59,9 @@ class gz::sensors::LidarPrivate /// \brief Sdf sensor. public: sdf::Lidar sdfLidar; + + /// \brief Number of channels of the raw lidar buffer + public: const unsigned int kChannelCount = 3u; }; ////////////////////////////////////////////////// @@ -215,14 +221,10 @@ void Lidar::ApplyNoise() for (unsigned int i = 0; i < this->RayCount(); ++i) { int index = j * this->RayCount() + i; - double range = this->laserBuffer[index*3]; + double range = this->laserBuffer[index * this->dataPtr->kChannelCount]; range = this->dataPtr->noises[LIDAR_NOISE]->Apply(range); - if (std::isfinite(range)) - { - range = gz::math::clamp(range, - this->RangeMin(), this->RangeMax()); - } - this->laserBuffer[index*3] = range; + this->laserBuffer[index * this->dataPtr->kChannelCount] = + this->Clamp(range); } } } @@ -232,6 +234,12 @@ void Lidar::ApplyNoise() bool Lidar::PublishLidarScan(const std::chrono::steady_clock::duration &_now) { GZ_PROFILE("Lidar::PublishLidarScan"); + + if (!this->dataPtr->pub.HasConnections()) + { + return false; + } + if (!this->laserBuffer) return false; @@ -246,7 +254,7 @@ bool Lidar::PublishLidarScan(const std::chrono::steady_clock::duration &_now) // keeping here the sensor name instead of frame_id because the visualizeLidar // plugin relies on this value to get the position of the lidar. // the ros_gz plugin is using the laserscan.proto 'frame' field - frame->add_value(this->Name()); + frame->add_value(this->FrameId()); this->dataPtr->laserMsg.set_frame(this->FrameId()); // Store the latest laser scans into laserMsg @@ -266,17 +274,18 @@ bool Lidar::PublishLidarScan(const std::chrono::steady_clock::duration &_now) } } + // \todo(iche033) interpolate if ray count != range count, i.e. resolution > 1 for (unsigned int j = 0; j < this->VerticalRangeCount(); ++j) { for (unsigned int i = 0; i < this->RangeCount(); ++i) { int index = j * this->RangeCount() + i; - double range = this->laserBuffer[index*3]; + double range = this->laserBuffer[index * this->dataPtr->kChannelCount]; - range = gz::math::isnan(range) ? this->RangeMax() : range; + range = this->Clamp(range); this->dataPtr->laserMsg.set_ranges(index, range); this->dataPtr->laserMsg.set_intensities(index, - this->laserBuffer[index * 3 + 1]); + this->laserBuffer[index * this->dataPtr->kChannelCount + 1]); } } @@ -420,10 +429,21 @@ void Lidar::SetVerticalAngleMax(const double _angle) void Lidar::Ranges(std::vector &_ranges) const { std::lock_guard lock(this->lidarMutex); + if (!this->laserBuffer) + { + gzwarn << "Lidar ranges not available" << std::endl; + return; + } - _ranges.resize(this->dataPtr->laserMsg.ranges_size()); - memcpy(&_ranges[0], this->dataPtr->laserMsg.ranges().data(), - sizeof(_ranges[0]) * this->dataPtr->laserMsg.ranges_size()); + // \todo(iche033) interpolate if ray count != range count, i.e. resolution > 1 + unsigned int size = this->RayCount() * this->VerticalRayCount(); + _ranges.resize(size); + + for (unsigned int i = 0; i < size; ++i) + { + _ranges[i] = this->Clamp( + this->laserBuffer[i * this->dataPtr->kChannelCount]); + } } ////////////////////////////////////////////////// @@ -431,24 +451,32 @@ double Lidar::Range(const int _index) const { std::lock_guard lock(this->lidarMutex); - if (this->dataPtr->laserMsg.ranges_size() == 0) + // \todo(iche033) interpolate if ray count != range count, i.e. resolution > 1 + if (!this->laserBuffer || _index >= static_cast( + this->RayCount() * this->VerticalRayCount() * + this->dataPtr->kChannelCount)) { - gzwarn << "ranges not constructed yet (zero sized)\n"; - return 0.0; - } - if (_index < 0 || _index > this->dataPtr->laserMsg.ranges_size()) - { - gzerr << "Invalid range index[" << _index << "]\n"; + gzwarn << "Lidar range not available for index: " << _index << std::endl; return 0.0; } - return this->dataPtr->laserMsg.ranges(_index); + return this->Clamp(this->laserBuffer[_index * this->dataPtr->kChannelCount]); } ////////////////////////////////////////////////// -double Lidar::Retro(const int /*_index*/) const +double Lidar::Retro(const int _index) const { - return 0.0; + std::lock_guard lock(this->lidarMutex); + + // \todo(iche033) interpolate if ray count != range count, i.e. resolution > 1 + if (!this->laserBuffer || _index >= static_cast( + this->RayCount() * this->VerticalRayCount() * + this->dataPtr->kChannelCount)) + { + gzwarn << "Lidar retro not available for index: " << _index << std::endl; + return 0.0; + } + return this->laserBuffer[_index * this->dataPtr->kChannelCount + 1]; } ////////////////////////////////////////////////// @@ -476,3 +504,15 @@ bool Lidar::HasConnections() const { return this->dataPtr->pub && this->dataPtr->pub.HasConnections(); } + +////////////////////////////////////////////////// +double Lidar::Clamp(double _range) const +{ + if (gz::math::isnan(_range)) + return this->RangeMax(); + + if (std::isfinite(_range)) + return std::clamp(_range, this->RangeMin(), this->RangeMax()); + + return _range; +} diff --git a/test/integration/gpu_lidar_sensor.cc b/test/integration/gpu_lidar_sensor.cc index 22ef6c7f..264e3b72 100644 --- a/test/integration/gpu_lidar_sensor.cc +++ b/test/integration/gpu_lidar_sensor.cc @@ -348,6 +348,7 @@ void GpuLidarSensorTest::DetectBox(const std::string &_renderEngine) visualBox1->AddGeometry(scene->CreateBox()); visualBox1->SetLocalPosition(box01Pose.Pos()); visualBox1->SetLocalRotation(box01Pose.Rot()); + visualBox1->SetUserData("laser_retro", 123); root->AddChild(visualBox1); // Create a sensor manager @@ -380,6 +381,7 @@ void GpuLidarSensorTest::DetectBox(const std::string &_renderEngine) // Sensor 1 should see TestBox1 EXPECT_DOUBLE_EQ(sensor->Range(0), gz::math::INF_D); EXPECT_NEAR(sensor->Range(mid), expectedRangeAtMidPointBox1, LASER_TOL); + EXPECT_NEAR(123, sensor->Retro(mid), 1e-1); EXPECT_DOUBLE_EQ(sensor->Range(last), gz::math::INF_D); // Make sure to wait to receive the message @@ -413,6 +415,12 @@ void GpuLidarSensorTest::DetectBox(const std::string &_renderEngine) EXPECT_NEAR(laserMsgs.back().range_min(), rangeMin, 1e-4); EXPECT_NEAR(laserMsgs.back().range_max(), rangeMax, 1e-4); + EXPECT_TRUE(laserMsgs.back().has_header()); + EXPECT_LT(1, laserMsgs.back().header().data().size()); + EXPECT_EQ("frame_id", laserMsgs.back().header().data(0).key()); + ASSERT_EQ(1, laserMsgs.back().header().data(0).value().size()); + EXPECT_EQ(frameId, laserMsgs.back().header().data(0).value(0)); + ASSERT_TRUE(!pointMsgs.empty()); EXPECT_EQ(5, pointMsgs.back().field_size()); EXPECT_EQ("x", pointMsgs.back().field(0).name()); From 08ab86ba271007b0f54f4a26932e194ee1ad236d Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 29 Jul 2024 09:40:06 -0700 Subject: [PATCH 3/3] Skip apply noise / distortion if parameters are 0s (#450) Signed-off-by: Ian Chen --- src/CameraSensor.cc | 39 ++++++++++++++++++++++++++++----------- src/DepthCameraSensor.cc | 20 ++++++++++++++------ src/Lidar.cc | 12 ++++++++++-- 3 files changed, 52 insertions(+), 19 deletions(-) diff --git a/src/CameraSensor.cc b/src/CameraSensor.cc index b0f558a7..92a357a9 100644 --- a/src/CameraSensor.cc +++ b/src/CameraSensor.cc @@ -224,12 +224,20 @@ bool CameraSensor::CreateCamera() // Add gaussian noise to camera sensor if (noiseSdf.Type() == sdf::NoiseType::GAUSSIAN) { - this->dataPtr->noises[noiseType] = - ImageNoiseFactory::NewNoiseModel(noiseSdf, "camera"); + // Skip applying noise if mean and stddev are 0 - this avoids + // doing an extra render pass in gz-rendering + // Note ImageGaussianNoiseModel only uses mean and stddev and does not + // use bias parameters. + if (!math::equal(noiseSdf.Mean(), 0.0) || + !math::equal(noiseSdf.StdDev(), 0.0)) + { + this->dataPtr->noises[noiseType] = + ImageNoiseFactory::NewNoiseModel(noiseSdf, "camera"); - std::dynamic_pointer_cast( - this->dataPtr->noises[noiseType])->SetCamera( - this->dataPtr->camera); + std::dynamic_pointer_cast( + this->dataPtr->noises[noiseType])->SetCamera( + this->dataPtr->camera); + } } else if (noiseSdf.Type() != sdf::NoiseType::NONE) { @@ -253,13 +261,22 @@ bool CameraSensor::CreateCamera() this->dataPtr->camera->SetHFOV(angle); if (cameraSdf->Element() != nullptr && - cameraSdf->Element()->HasElement("distortion")) { - this->dataPtr->distortion = - ImageDistortionFactory::NewDistortionModel(*cameraSdf, "camera"); - this->dataPtr->distortion->Load(*cameraSdf); + cameraSdf->Element()->HasElement("distortion")) + { + // Skip distortion of all coefficients are 0s + if (!math::equal(cameraSdf->DistortionK1(), 0.0) || + !math::equal(cameraSdf->DistortionK2(), 0.0) || + !math::equal(cameraSdf->DistortionK3(), 0.0) || + !math::equal(cameraSdf->DistortionP1(), 0.0) || + !math::equal(cameraSdf->DistortionP2(), 0.0)) + { + this->dataPtr->distortion = + ImageDistortionFactory::NewDistortionModel(*cameraSdf, "camera"); + this->dataPtr->distortion->Load(*cameraSdf); - std::dynamic_pointer_cast( - this->dataPtr->distortion)->SetCamera(this->dataPtr->camera); + std::dynamic_pointer_cast( + this->dataPtr->distortion)->SetCamera(this->dataPtr->camera); + } } sdf::PixelFormatType pixelFormat = cameraSdf->PixelFormat(); diff --git a/src/DepthCameraSensor.cc b/src/DepthCameraSensor.cc index 52f88d8c..41ed4e52 100644 --- a/src/DepthCameraSensor.cc +++ b/src/DepthCameraSensor.cc @@ -362,12 +362,20 @@ bool DepthCameraSensor::CreateCamera() // Add gaussian noise to camera sensor if (noiseSdf.Type() == sdf::NoiseType::GAUSSIAN) { - this->dataPtr->noises[noiseType] = - ImageNoiseFactory::NewNoiseModel(noiseSdf, "depth"); - - std::dynamic_pointer_cast( - this->dataPtr->noises[noiseType])->SetCamera( - this->dataPtr->depthCamera); + // Skip applying noise if mean and stddev are 0 - this avoids + // doing an extra render pass in gz-rendering + // Note ImageGaussianNoiseModel only uses mean and stddev and does not + // use bias parameters. + if (!math::equal(noiseSdf.Mean(), 0.0) || + !math::equal(noiseSdf.StdDev(), 0.0)) + { + this->dataPtr->noises[noiseType] = + ImageNoiseFactory::NewNoiseModel(noiseSdf, "depth"); + + std::dynamic_pointer_cast( + this->dataPtr->noises[noiseType])->SetCamera( + this->dataPtr->depthCamera); + } } else if (noiseSdf.Type() != sdf::NoiseType::NONE) { diff --git a/src/Lidar.cc b/src/Lidar.cc index 5a1e8a03..c67d5dc4 100644 --- a/src/Lidar.cc +++ b/src/Lidar.cc @@ -172,8 +172,16 @@ bool Lidar::Load(const sdf::Sensor &_sdf) { if (noiseSdf.Type() == sdf::NoiseType::GAUSSIAN) { - this->dataPtr->noises[noiseType] = - NoiseFactory::NewNoiseModel(noiseSdf); + // Skip applying noise if gaussian noise params are all 0s + if (!math::equal(noiseSdf.Mean(), 0.0) || + !math::equal(noiseSdf.StdDev(), 0.0) || + !math::equal(noiseSdf.BiasMean(), 0.0) || + !math::equal(noiseSdf.DynamicBiasStdDev(), 0.0) || + !math::equal(noiseSdf.DynamicBiasCorrelationTime(), 0.0)) + { + this->dataPtr->noises[noiseType] = + NoiseFactory::NewNoiseModel(noiseSdf); + } } else if (noiseSdf.Type() != sdf::NoiseType::NONE) {