Backport lidar optimization and frame fixes

include/gz/sensors/Lidar.hh

@@ -245,6 +245,13 @@ namespace ignition
       /// \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;
+
       IGN_COMMON_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)
@@ -55,6 +58,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;
 };
 
 //////////////////////////////////////////////////
@@ -214,14 +220,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);
       }
     }
   }
@@ -231,6 +233,12 @@ void Lidar::ApplyNoise()
 bool Lidar::PublishLidarScan(const std::chrono::steady_clock::duration &_now)
 {
   IGN_PROFILE("Lidar::PublishLidarScan");
+
+  if (!this->dataPtr->pub.HasConnections())
+  {
+    return false;
+  }
+
   if (!this->laserBuffer)
     return false;
 
@@ -242,10 +250,8 @@ bool Lidar::PublishLidarScan(const std::chrono::steady_clock::duration &_now)
   this->dataPtr->laserMsg.mutable_header()->clear_data();
   auto frame = this->dataPtr->laserMsg.mutable_header()->add_data();
   frame->set_key("frame_id");
-  // 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_ign 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
@@ -265,17 +271,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]);
     }
   }
 
@@ -419,35 +426,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)
+  {
+    ignwarn << "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))
   {
-    ignwarn << "ranges not constructed yet (zero sized)\n";
-    return 0.0;
-  }
-  if (_index < 0 || _index > this->dataPtr->laserMsg.ranges_size())
-  {
-    ignerr << "Invalid range index[" << _index << "]\n";
+    ignwarn << "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))
+  {
+    ignwarn << "Lidar retro not available for index: " << _index << std::endl;
+    return 0.0;
+  }
+  return this->laserBuffer[_index * this->dataPtr->kChannelCount + 1];
 }
 
 //////////////////////////////////////////////////
@@ -475,3 +501,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;
+}

src/Sensor.cc

@@ -108,7 +108,7 @@ class gz::sensors::SensorPrivate
   public: std::map<std::string, uint64_t> sequences;
 
   /// \brief frame id
-  public: std::string frame_id;
+  public: std::string frameId;
 
   /// \brief If sensor is active or not.
   public: bool active = true;
@@ -147,7 +147,7 @@ bool SensorPrivate::PopulateFromSDF(const sdf::Sensor &_sdf)
   {
     if (element->HasElement("ignition_frame_id"))
     {
-      this->frame_id = element->Get<std::string>("ignition_frame_id");
+      this->frameId = element->Get<std::string>("ignition_frame_id");
       // Warn if both ignition_frame_id and gz_frame_id are specified
       if (element->HasElement("gz_frame_id"))
       {
@@ -159,11 +159,11 @@ bool SensorPrivate::PopulateFromSDF(const sdf::Sensor &_sdf)
     {
       // Also read gz_frame_id to support SDF that's compatible with newer
       // versions of Gazebo.
-      this->frame_id = element->Get<std::string>("gz_frame_id");
+      this->frameId = element->Get<std::string>("gz_frame_id");
     }
     else
     {
-      this->frame_id = this->name;
+      this->frameId = this->name;
     }
   }
 
@@ -260,13 +260,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;
 }
 
 //////////////////////////////////////////////////