From 12b306734677ff5e20c67a4f7289da73fe4d38e0 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 12 Jul 2024 21:53:41 +0000 Subject: [PATCH 1/3] Publish lidar scan only if there are connections Signed-off-by: Ian Chen --- include/gz/sensors/Lidar.hh | 7 +++ src/Lidar.cc | 77 ++++++++++++++++++++-------- test/integration/gpu_lidar_sensor.cc | 8 +++ 3 files changed, 70 insertions(+), 22 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..bbce4701 100644 --- a/src/Lidar.cc +++ b/src/Lidar.cc @@ -217,12 +217,7 @@ void Lidar::ApplyNoise() int index = j * this->RayCount() + i; double range = this->laserBuffer[index*3]; 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*3] = this->Clamp(range); } } } @@ -232,6 +227,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 +247,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,14 +267,15 @@ 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 * 3]; - 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]); @@ -420,10 +422,19 @@ 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; + } + + // \todo(iche033) interpolate if ray count != range count, i.e. resolution > 1 + unsigned int size = this->RayCount() * this->VerticalRayCount(); + _ranges.resize(size); - _ranges.resize(this->dataPtr->laserMsg.ranges_size()); - memcpy(&_ranges[0], this->dataPtr->laserMsg.ranges().data(), - sizeof(_ranges[0]) * this->dataPtr->laserMsg.ranges_size()); + unsigned int channels = 3u; + for (unsigned int i = 0; i < size; ++i) + _ranges[i] = this->Clamp(this->laserBuffer[i * channels]); } ////////////////////////////////////////////////// @@ -431,24 +442,32 @@ double Lidar::Range(const int _index) const { std::lock_guard lock(this->lidarMutex); - if (this->dataPtr->laserMsg.ranges_size() == 0) - { - gzwarn << "ranges not constructed yet (zero sized)\n"; - return 0.0; - } - if (_index < 0 || _index > this->dataPtr->laserMsg.ranges_size()) + // \todo(iche033) interpolate if ray count != range count, i.e. resolution > 1 + unsigned int channels = 3u; + if (!this->laserBuffer || _index >= + static_cast(this->RayCount() * this->VerticalRayCount() * channels)) { - 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 * channels]); } ////////////////////////////////////////////////// -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 + unsigned int channels = 3u; + if (!this->laserBuffer || _index >= + static_cast(this->RayCount() * this->VerticalRayCount() * channels)) + { + gzwarn << "Lidar range not available for index: " << _index << std::endl; + return 0.0; + } + return this->laserBuffer[_index * channels + 1]; } ////////////////////////////////////////////////// @@ -476,3 +495,17 @@ 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 gz::math::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 b8fba098110fb21da79307fe276419950783a881 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 12 Jul 2024 22:02:09 +0000 Subject: [PATCH 2/3] cleanup Signed-off-by: Ian Chen --- src/Lidar.cc | 36 +++++++++++++++++++----------------- 1 file changed, 19 insertions(+), 17 deletions(-) diff --git a/src/Lidar.cc b/src/Lidar.cc index bbce4701..d260eca4 100644 --- a/src/Lidar.cc +++ b/src/Lidar.cc @@ -56,6 +56,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,9 +218,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); - this->laserBuffer[index*3] = this->Clamp(range); + this->laserBuffer[index * this->dataPtr->kChannelCount] = + this->Clamp(range); } } } @@ -273,12 +277,12 @@ bool Lidar::PublishLidarScan(const std::chrono::steady_clock::duration &_now) 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 = 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]); } } @@ -432,9 +436,11 @@ void Lidar::Ranges(std::vector &_ranges) const unsigned int size = this->RayCount() * this->VerticalRayCount(); _ranges.resize(size); - unsigned int channels = 3u; for (unsigned int i = 0; i < size; ++i) - _ranges[i] = this->Clamp(this->laserBuffer[i * channels]); + { + _ranges[i] = this->Clamp( + this->laserBuffer[i * this->dataPtr->kChannelCount]); + } } ////////////////////////////////////////////////// @@ -443,15 +449,14 @@ double Lidar::Range(const int _index) const std::lock_guard lock(this->lidarMutex); // \todo(iche033) interpolate if ray count != range count, i.e. resolution > 1 - unsigned int channels = 3u; - if (!this->laserBuffer || _index >= - static_cast(this->RayCount() * this->VerticalRayCount() * channels)) + if (!this->laserBuffer || _index >= static_cast( + this->RayCount() * this->VerticalRayCount() * this->dataPtr->kChannelCount)) { gzwarn << "Lidar range not available for index: " << _index << std::endl; return 0.0; } - return this->Clamp(this->laserBuffer[_index * channels]); + return this->Clamp(this->laserBuffer[_index * this->dataPtr->kChannelCount]); } ////////////////////////////////////////////////// @@ -460,14 +465,13 @@ double Lidar::Retro(const int _index) const std::lock_guard lock(this->lidarMutex); // \todo(iche033) interpolate if ray count != range count, i.e. resolution > 1 - unsigned int channels = 3u; - if (!this->laserBuffer || _index >= - static_cast(this->RayCount() * this->VerticalRayCount() * channels)) + if (!this->laserBuffer || _index >= static_cast( + this->RayCount() * this->VerticalRayCount() * this->dataPtr->kChannelCount)) { - gzwarn << "Lidar range not available for index: " << _index << std::endl; + gzwarn << "Lidar retro not available for index: " << _index << std::endl; return 0.0; } - return this->laserBuffer[_index * channels + 1]; + return this->laserBuffer[_index * this->dataPtr->kChannelCount + 1]; } ////////////////////////////////////////////////// @@ -503,9 +507,7 @@ double Lidar::Clamp(double _range) const return this->RangeMax(); if (std::isfinite(_range)) - { return gz::math::clamp(_range, this->RangeMin(), this->RangeMax()); - } return _range; } From 5c28cce2df6f3a2a90ef857792acfe504eb47ec3 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 15 Jul 2024 16:37:56 +0000 Subject: [PATCH 3/3] line wrap, clamp Signed-off-by: Ian Chen --- src/Lidar.cc | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/Lidar.cc b/src/Lidar.cc index d260eca4..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) @@ -450,7 +453,8 @@ double Lidar::Range(const int _index) const // \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)) + this->RayCount() * this->VerticalRayCount() * + this->dataPtr->kChannelCount)) { gzwarn << "Lidar range not available for index: " << _index << std::endl; return 0.0; @@ -466,7 +470,8 @@ double Lidar::Retro(const int _index) const // \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)) + this->RayCount() * this->VerticalRayCount() * + this->dataPtr->kChannelCount)) { gzwarn << "Lidar retro not available for index: " << _index << std::endl; return 0.0; @@ -507,7 +512,7 @@ double Lidar::Clamp(double _range) const return this->RangeMax(); if (std::isfinite(_range)) - return gz::math::clamp(_range, this->RangeMin(), this->RangeMax()); + return std::clamp(_range, this->RangeMin(), this->RangeMax()); return _range; }