Publish lidar scan only if there are lidar scan connections

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;

src/Lidar.cc

@@ -14,6 +14,9 @@
  * limitations under the License.
  *
 */
+
+#include <algorithm>
+
 #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,35 +429,54 @@ void Lidar::SetVerticalAngleMax(const double _angle)
 void Lidar::Ranges(std::vector<double> &_ranges) const
 {
   std::lock_guard<std::mutex> 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]);
+  }
 }
 
 //////////////////////////////////////////////////
 double Lidar::Range(const int _index) const
 {
   std::lock_guard<std::mutex> 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<int>(
+      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<std::mutex> lock(this->lidarMutex);
+
+  // \todo(iche033) interpolate if ray count != range count, i.e. resolution > 1
+  if (!this->laserBuffer || _index >= static_cast<int>(
+      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;
+}

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());