Skip to content

Commit 281a9f4

Browse files
iche033iche033
authored
Merge pull request #451 from gazebosim/merge_8_main_20240731
Merge 8 -> main
2 parents fdf8e02 + 4ec64c8 commit 281a9f4

File tree

5 files changed

+131
-43
lines changed

5 files changed

+131
-43
lines changed

‎include/gz/sensors/Lidar.hh‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -244,6 +244,13 @@ namespace gz
244244
/// \return Visibility mask
245245
public: uint32_t VisibilityMask() const;
246246

247+
/// \brief Clamp a finite range value to min / max range.
248+
/// +/-inf values will not be clamped because they mean lidar returns are
249+
/// outside the detectable range.
250+
/// NAN values will be clamped to max range.
251+
/// \return Clamped range value.
252+
private: double Clamp(double _range) const;
253+
247254
GZ_UTILS_WARN_IGNORE__DLL_INTERFACE_MISSING
248255
/// \brief Just a mutex for thread safety
249256
public: mutable std::mutex lidarMutex;

‎src/CameraSensor.cc‎

Lines changed: 28 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -162,12 +162,20 @@ bool CameraSensor::CreateCamera()
162162
// Add gaussian noise to camera sensor
163163
if (noiseSdf.Type() == sdf::NoiseType::GAUSSIAN)
164164
{
165-
this->dataPtr->noises[noiseType] =
166-
ImageNoiseFactory::NewNoiseModel(noiseSdf, "camera");
165+
// Skip applying noise if mean and stddev are 0 - this avoids
166+
// doing an extra render pass in gz-rendering
167+
// Note ImageGaussianNoiseModel only uses mean and stddev and does not
168+
// use bias parameters.
169+
if (!math::equal(noiseSdf.Mean(), 0.0) ||
170+
!math::equal(noiseSdf.StdDev(), 0.0))
171+
{
172+
this->dataPtr->noises[noiseType] =
173+
ImageNoiseFactory::NewNoiseModel(noiseSdf, "camera");
167174

168-
std::dynamic_pointer_cast<ImageGaussianNoiseModel>(
169-
this->dataPtr->noises[noiseType])->SetCamera(
170-
this->dataPtr->camera);
175+
std::dynamic_pointer_cast<ImageGaussianNoiseModel>(
176+
this->dataPtr->noises[noiseType])->SetCamera(
177+
this->dataPtr->camera);
178+
}
171179
}
172180
else if (noiseSdf.Type() != sdf::NoiseType::NONE)
173181
{
@@ -191,13 +199,22 @@ bool CameraSensor::CreateCamera()
191199
this->dataPtr->camera->SetHFOV(angle);
192200

193201
if (cameraSdf->Element() != nullptr &&
194-
cameraSdf->Element()->HasElement("distortion")) {
195-
this->dataPtr->distortion =
196-
ImageDistortionFactory::NewDistortionModel(*cameraSdf, "camera");
197-
this->dataPtr->distortion->Load(*cameraSdf);
202+
cameraSdf->Element()->HasElement("distortion"))
203+
{
204+
// Skip distortion of all coefficients are 0s
205+
if (!math::equal(cameraSdf->DistortionK1(), 0.0) ||
206+
!math::equal(cameraSdf->DistortionK2(), 0.0) ||
207+
!math::equal(cameraSdf->DistortionK3(), 0.0) ||
208+
!math::equal(cameraSdf->DistortionP1(), 0.0) ||
209+
!math::equal(cameraSdf->DistortionP2(), 0.0))
210+
{
211+
this->dataPtr->distortion =
212+
ImageDistortionFactory::NewDistortionModel(*cameraSdf, "camera");
213+
this->dataPtr->distortion->Load(*cameraSdf);
198214

199-
std::dynamic_pointer_cast<ImageBrownDistortionModel>(
200-
this->dataPtr->distortion)->SetCamera(this->dataPtr->camera);
215+
std::dynamic_pointer_cast<ImageBrownDistortionModel>(
216+
this->dataPtr->distortion)->SetCamera(this->dataPtr->camera);
217+
}
201218
}
202219

203220
sdf::PixelFormatType pixelFormat = cameraSdf->PixelFormat();

‎src/DepthCameraSensor.cc‎

Lines changed: 14 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -363,12 +363,20 @@ bool DepthCameraSensor::CreateCamera()
363363
// Add gaussian noise to camera sensor
364364
if (noiseSdf.Type() == sdf::NoiseType::GAUSSIAN)
365365
{
366-
this->dataPtr->noises[noiseType] =
367-
ImageNoiseFactory::NewNoiseModel(noiseSdf, "depth");
368-
369-
std::dynamic_pointer_cast<ImageGaussianNoiseModel>(
370-
this->dataPtr->noises[noiseType])->SetCamera(
371-
this->dataPtr->depthCamera);
366+
// Skip applying noise if mean and stddev are 0 - this avoids
367+
// doing an extra render pass in gz-rendering
368+
// Note ImageGaussianNoiseModel only uses mean and stddev and does not
369+
// use bias parameters.
370+
if (!math::equal(noiseSdf.Mean(), 0.0) ||
371+
!math::equal(noiseSdf.StdDev(), 0.0))
372+
{
373+
this->dataPtr->noises[noiseType] =
374+
ImageNoiseFactory::NewNoiseModel(noiseSdf, "depth");
375+
376+
std::dynamic_pointer_cast<ImageGaussianNoiseModel>(
377+
this->dataPtr->noises[noiseType])->SetCamera(
378+
this->dataPtr->depthCamera);
379+
}
372380
}
373381
else if (noiseSdf.Type() != sdf::NoiseType::NONE)
374382
{

‎src/Lidar.cc‎

Lines changed: 74 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,9 @@
1414
* limitations under the License.
1515
*
1616
*/
17+
18+
#include <algorithm>
19+
1720
#if defined(_MSC_VER)
1821
#pragma warning(push)
1922
#pragma warning(disable: 4005)
@@ -56,6 +59,9 @@ class gz::sensors::LidarPrivate
5659

5760
/// \brief Sdf sensor.
5861
public: sdf::Lidar sdfLidar;
62+
63+
/// \brief Number of channels of the raw lidar buffer
64+
public: const unsigned int kChannelCount = 3u;
5965
};
6066

6167
//////////////////////////////////////////////////
@@ -166,8 +172,16 @@ bool Lidar::Load(const sdf::Sensor &_sdf)
166172
{
167173
if (noiseSdf.Type() == sdf::NoiseType::GAUSSIAN)
168174
{
169-
this->dataPtr->noises[noiseType] =
170-
NoiseFactory::NewNoiseModel(noiseSdf);
175+
// Skip applying noise if gaussian noise params are all 0s
176+
if (!math::equal(noiseSdf.Mean(), 0.0) ||
177+
!math::equal(noiseSdf.StdDev(), 0.0) ||
178+
!math::equal(noiseSdf.BiasMean(), 0.0) ||
179+
!math::equal(noiseSdf.DynamicBiasStdDev(), 0.0) ||
180+
!math::equal(noiseSdf.DynamicBiasCorrelationTime(), 0.0))
181+
{
182+
this->dataPtr->noises[noiseType] =
183+
NoiseFactory::NewNoiseModel(noiseSdf);
184+
}
171185
}
172186
else if (noiseSdf.Type() != sdf::NoiseType::NONE)
173187
{
@@ -215,14 +229,10 @@ void Lidar::ApplyNoise()
215229
for (unsigned int i = 0; i < this->RayCount(); ++i)
216230
{
217231
int index = j * this->RayCount() + i;
218-
double range = this->laserBuffer[index*3];
232+
double range = this->laserBuffer[index * this->dataPtr->kChannelCount];
219233
range = this->dataPtr->noises[LIDAR_NOISE]->Apply(range);
220-
if (std::isfinite(range))
221-
{
222-
range = gz::math::clamp(range,
223-
this->RangeMin(), this->RangeMax());
224-
}
225-
this->laserBuffer[index*3] = range;
234+
this->laserBuffer[index * this->dataPtr->kChannelCount] =
235+
this->Clamp(range);
226236
}
227237
}
228238
}
@@ -232,6 +242,12 @@ void Lidar::ApplyNoise()
232242
bool Lidar::PublishLidarScan(const std::chrono::steady_clock::duration &_now)
233243
{
234244
GZ_PROFILE("Lidar::PublishLidarScan");
245+
246+
if (!this->dataPtr->pub.HasConnections())
247+
{
248+
return false;
249+
}
250+
235251
if (!this->laserBuffer)
236252
return false;
237253

@@ -246,7 +262,7 @@ bool Lidar::PublishLidarScan(const std::chrono::steady_clock::duration &_now)
246262
// keeping here the sensor name instead of frame_id because the visualizeLidar
247263
// plugin relies on this value to get the position of the lidar.
248264
// the ros_gz plugin is using the laserscan.proto 'frame' field
249-
frame->add_value(this->Name());
265+
frame->add_value(this->FrameId());
250266
this->dataPtr->laserMsg.set_frame(this->FrameId());
251267

252268
// Store the latest laser scans into laserMsg
@@ -266,17 +282,18 @@ bool Lidar::PublishLidarScan(const std::chrono::steady_clock::duration &_now)
266282
}
267283
}
268284

285+
// \todo(iche033) interpolate if ray count != range count, i.e. resolution > 1
269286
for (unsigned int j = 0; j < this->VerticalRangeCount(); ++j)
270287
{
271288
for (unsigned int i = 0; i < this->RangeCount(); ++i)
272289
{
273290
int index = j * this->RangeCount() + i;
274-
double range = this->laserBuffer[index*3];
291+
double range = this->laserBuffer[index * this->dataPtr->kChannelCount];
275292

276-
range = gz::math::isnan(range) ? this->RangeMax() : range;
293+
range = this->Clamp(range);
277294
this->dataPtr->laserMsg.set_ranges(index, range);
278295
this->dataPtr->laserMsg.set_intensities(index,
279-
this->laserBuffer[index * 3 + 1]);
296+
this->laserBuffer[index * this->dataPtr->kChannelCount + 1]);
280297
}
281298
}
282299

@@ -420,35 +437,54 @@ void Lidar::SetVerticalAngleMax(const double _angle)
420437
void Lidar::Ranges(std::vector<double> &_ranges) const
421438
{
422439
std::lock_guard<std::mutex> lock(this->lidarMutex);
440+
if (!this->laserBuffer)
441+
{
442+
gzwarn << "Lidar ranges not available" << std::endl;
443+
return;
444+
}
445+
446+
// \todo(iche033) interpolate if ray count != range count, i.e. resolution > 1
447+
unsigned int size = this->RayCount() * this->VerticalRayCount();
448+
_ranges.resize(size);
423449

424-
_ranges.resize(this->dataPtr->laserMsg.ranges_size());
425-
memcpy(&_ranges[0], this->dataPtr->laserMsg.ranges().data(),
426-
sizeof(_ranges[0]) * this->dataPtr->laserMsg.ranges_size());
450+
for (unsigned int i = 0; i < size; ++i)
451+
{
452+
_ranges[i] = this->Clamp(
453+
this->laserBuffer[i * this->dataPtr->kChannelCount]);
454+
}
427455
}
428456

429457
//////////////////////////////////////////////////
430458
double Lidar::Range(const int _index) const
431459
{
432460
std::lock_guard<std::mutex> lock(this->lidarMutex);
433461

434-
if (this->dataPtr->laserMsg.ranges_size() == 0)
435-
{
436-
gzwarn << "ranges not constructed yet (zero sized)\n";
437-
return 0.0;
438-
}
439-
if (_index < 0 || _index > this->dataPtr->laserMsg.ranges_size())
462+
// \todo(iche033) interpolate if ray count != range count, i.e. resolution > 1
463+
if (!this->laserBuffer || _index >= static_cast<int>(
464+
this->RayCount() * this->VerticalRayCount() *
465+
this->dataPtr->kChannelCount))
440466
{
441-
gzerr << "Invalid range index[" << _index << "]\n";
467+
gzwarn << "Lidar range not available for index: " << _index << std::endl;
442468
return 0.0;
443469
}
444470

445-
return this->dataPtr->laserMsg.ranges(_index);
471+
return this->Clamp(this->laserBuffer[_index * this->dataPtr->kChannelCount]);
446472
}
447473

448474
//////////////////////////////////////////////////
449-
double Lidar::Retro(const int /*_index*/) const
475+
double Lidar::Retro(const int _index) const
450476
{
451-
return 0.0;
477+
std::lock_guard<std::mutex> lock(this->lidarMutex);
478+
479+
// \todo(iche033) interpolate if ray count != range count, i.e. resolution > 1
480+
if (!this->laserBuffer || _index >= static_cast<int>(
481+
this->RayCount() * this->VerticalRayCount() *
482+
this->dataPtr->kChannelCount))
483+
{
484+
gzwarn << "Lidar retro not available for index: " << _index << std::endl;
485+
return 0.0;
486+
}
487+
return this->laserBuffer[_index * this->dataPtr->kChannelCount + 1];
452488
}
453489

454490
//////////////////////////////////////////////////
@@ -476,3 +512,15 @@ bool Lidar::HasConnections() const
476512
{
477513
return this->dataPtr->pub && this->dataPtr->pub.HasConnections();
478514
}
515+
516+
//////////////////////////////////////////////////
517+
double Lidar::Clamp(double _range) const
518+
{
519+
if (gz::math::isnan(_range))
520+
return this->RangeMax();
521+
522+
if (std::isfinite(_range))
523+
return std::clamp(_range, this->RangeMin(), this->RangeMax());
524+
525+
return _range;
526+
}

‎test/integration/gpu_lidar_sensor.cc‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -348,6 +348,7 @@ void GpuLidarSensorTest::DetectBox(const std::string &_renderEngine)
348348
visualBox1->AddGeometry(scene->CreateBox());
349349
visualBox1->SetLocalPosition(box01Pose.Pos());
350350
visualBox1->SetLocalRotation(box01Pose.Rot());
351+
visualBox1->SetUserData("laser_retro", 123);
351352
root->AddChild(visualBox1);
352353

353354
// Create a sensor manager
@@ -380,6 +381,7 @@ void GpuLidarSensorTest::DetectBox(const std::string &_renderEngine)
380381
// Sensor 1 should see TestBox1
381382
EXPECT_DOUBLE_EQ(sensor->Range(0), gz::math::INF_D);
382383
EXPECT_NEAR(sensor->Range(mid), expectedRangeAtMidPointBox1, LASER_TOL);
384+
EXPECT_NEAR(123, sensor->Retro(mid), 1e-1);
383385
EXPECT_DOUBLE_EQ(sensor->Range(last), gz::math::INF_D);
384386

385387
// Make sure to wait to receive the message
@@ -413,6 +415,12 @@ void GpuLidarSensorTest::DetectBox(const std::string &_renderEngine)
413415
EXPECT_NEAR(laserMsgs.back().range_min(), rangeMin, 1e-4);
414416
EXPECT_NEAR(laserMsgs.back().range_max(), rangeMax, 1e-4);
415417

418+
EXPECT_TRUE(laserMsgs.back().has_header());
419+
EXPECT_LT(1, laserMsgs.back().header().data().size());
420+
EXPECT_EQ("frame_id", laserMsgs.back().header().data(0).key());
421+
ASSERT_EQ(1, laserMsgs.back().header().data(0).value().size());
422+
EXPECT_EQ(frameId, laserMsgs.back().header().data(0).value(0));
423+
416424
ASSERT_TRUE(!pointMsgs.empty());
417425
EXPECT_EQ(5, pointMsgs.back().field_size());
418426
EXPECT_EQ("x", pointMsgs.back().field(0).name());

0 commit comments

Comments
 (0)