Added Logic to flag pointcloud as not dense if invalid point is Detec…

…ted (#180) Signed-off-by: Carlos Mendes <[email protected]> Co-authored-by: Louise Poubel <[email protected]>
gazebosim · Dec 21, 2021 · 16e52cc · 16e52cc
1 parent 8ae7167
commit 16e52cc
Show file tree

Hide file tree

Showing 3 changed files with 14 additions and 3 deletions.
diff --git a/Migration.md b/Migration.md
@@ -5,6 +5,12 @@ Deprecated code produces compile-time warnings. These warning serve as
 notification to users that their code should be upgraded. The next major
 release will remove the deprecated code.
 
+## Ignition Sensors 6.0.1 to 6.X.X
+
+### Modifications
+1. Point Cloud Density flag (**is_dense** from GpuLidarSensor) is set to **false** if invalid points (NaN or +/-INF) are found, and **true** otherwise.
+
+
 ## Ignition Sensors 5.X to 6.X
 
 1. Sensors aren't loaded as plugins anymore. Instead, downstream libraries must

diff --git a/src/GpuLidarSensor.cc b/src/GpuLidarSensor.cc
@@ -256,7 +256,6 @@ bool GpuLidarSensor::Update(const std::chrono::steady_clock::duration &_now)
     // Set the time stamp
     *this->dataPtr->pointMsg.mutable_header()->mutable_stamp() =
       msgs::Convert(_now);
-    this->dataPtr->pointMsg.set_is_dense(true);
 
     this->dataPtr->FillPointCloudMsg(this->laserBuffer);
 
@@ -326,7 +325,8 @@ void GpuLidarSensorPrivate::FillPointCloudMsg(const float *_laserBuffer)
   msgBuffer->resize(this->pointMsg.row_step() *
       this->pointMsg.height());
   char *msgBufferIndex = msgBuffer->data();
-
+  // Set Pointcloud as dense. Change if invalid points are found.
+  bool isDense { true };
   // Iterate over scan and populate point cloud
   for (uint32_t j = 0; j < height; ++j)
   {
@@ -337,6 +337,10 @@ void GpuLidarSensorPrivate::FillPointCloudMsg(const float *_laserBuffer)
       // Index of current point, and the depth value at that point
       auto index = j * width * channels + i * channels;
       float depth = _laserBuffer[index];
+      // Validate Depth/Radius and update pointcloud density flag
+      if (isDense)
+        isDense = !(ignition::math::isnan(depth) || std::isinf(depth));
+
       float intensity = _laserBuffer[index + 1];
       uint16_t ring = j;
 
@@ -371,5 +375,6 @@ void GpuLidarSensorPrivate::FillPointCloudMsg(const float *_laserBuffer)
     }
     inclination += verticleAngleStep;
   }
+  this->pointMsg.set_is_dense(isDense);
 }
 
diff --git a/test/integration/gpu_lidar_sensor.cc b/test/integration/gpu_lidar_sensor.cc
@@ -411,7 +411,7 @@ void GpuLidarSensorTest::DetectBox(const std::string &_renderEngine)
   EXPECT_FALSE(pointMsgs.back().is_bigendian());
   EXPECT_EQ(32u, pointMsgs.back().point_step());
   EXPECT_EQ(32u * horzSamples, pointMsgs.back().row_step());
-  EXPECT_TRUE(pointMsgs.back().is_dense());
+  EXPECT_FALSE(pointMsgs.back().is_dense());
   EXPECT_EQ(32u * horzSamples * vertSamples, pointMsgs.back().data().size());
 
   // Clean up