Implement support for non-repetitive/alternating lidar scan patterns (#52)

* Implement alternating lidar pattern

Improve performance by only toggling between existing nodes in graph

Simplify pattern loader

* Add Livox Mid360 preset

* Add Livox Avia preset

* Add Livox Horizon preset

* Add Livox Mid40 preset

* Add Livox Mid70 preset

* Add Livox Tele15 preset

* Add new lidar presets to readme

* Refactor LidarPatternLoader

Author: Roboterbastler

README.md

@@ -194,6 +194,12 @@ Inside the link entity in your model, add a custom sensor:
   <pattern_preset>OS1 64</pattern_preset>
   <pattern_preset>Pandar64</pattern_preset>
   <pattern_preset>Pandar40P</pattern_preset>
+  <pattern_preset>Livox Avia</pattern_preset>
+  <pattern_preset>Livox Horizon</pattern_preset>
+  <pattern_preset>Livox Mid40</pattern_preset>
+  <pattern_preset>Livox Mid70</pattern_preset>
+  <pattern_preset>Livox Mid360</pattern_preset>
+  <pattern_preset>Livox Tele15</pattern_preset>
   ```
   **Note:** Before launching the simulation it is required to set `RGL_PATTERNS_DIR` environment variable with the path to pattern presets directory (`lidar_patterns` from repository).
   ```shell

RGLServerPlugin/include/LidarPatternLoader.hh

@@ -15,6 +15,10 @@
 #pragma once
 
 #include <filesystem>
+#include <functional>
+#include <map>
+#include <string>
+#include <utility>
 #include <vector>
 
 #include <rgl/api/core.h>
@@ -26,9 +30,10 @@ namespace rgl
 class LidarPatternLoader
 {
 public:
-    using LoadFuncType = std::function<bool(const sdf::ElementConstPtr&, std::vector<rgl_mat3x4f>&)>;
+    using LoadFuncType = std::function<bool(const sdf::ElementConstPtr&, std::vector<rgl_mat3x4f>&, std::size_t&)>;
 
-    static bool Load(const sdf::ElementConstPtr& sdf, std::vector<rgl_mat3x4f>& outPattern);
+    static bool Load(const sdf::ElementConstPtr& sdf, std::vector<rgl_mat3x4f>& outPattern,
+                     std::size_t& outPatternScanSize);
 
 private:
     LidarPatternLoader() {};
@@ -37,11 +42,11 @@ private:
                                             gz::math::Angle& angleMin, gz::math::Angle& angleMax,
                                             int& samples);
 
-    static bool LoadPatternFromUniform(const sdf::ElementConstPtr& sdf, std::vector<rgl_mat3x4f>& outPattern);
-    static bool LoadPatternFromCustom(const sdf::ElementConstPtr& sdf, std::vector<rgl_mat3x4f>& outPattern);
-    static bool LoadPatternFromPreset(const sdf::ElementConstPtr& sdf, std::vector<rgl_mat3x4f>& outPattern);
-    static bool LoadPatternFromPresetPath(const sdf::ElementConstPtr& sdf, std::vector<rgl_mat3x4f>& outPattern);
-    static bool LoadPatternFromLidar2d(const sdf::ElementConstPtr& sdf, std::vector<rgl_mat3x4f>& outPattern);
+    static bool LoadPatternFromUniform(const sdf::ElementConstPtr& sdf, std::vector<rgl_mat3x4f>& outPattern, std::size_t& outPatternScanSize);
+    static bool LoadPatternFromCustom(const sdf::ElementConstPtr& sdf, std::vector<rgl_mat3x4f>& outPattern, std::size_t& outPatternScanSize);
+    static bool LoadPatternFromPreset(const sdf::ElementConstPtr& sdf, std::vector<rgl_mat3x4f>& outPattern, std::size_t& outPatternScanSize);
+    static bool LoadPatternFromPresetPath(const sdf::ElementConstPtr& sdf, std::vector<rgl_mat3x4f>& outPattern, std::size_t& outPatternScanSize);
+    static bool LoadPatternFromLidar2d(const sdf::ElementConstPtr& sdf, std::vector<rgl_mat3x4f>& outPattern, std::size_t& outPatternScanSize);
 
     static rgl_mat3x4f AnglesToRglMat3x4f(const gz::math::Angle& roll,
                                           const gz::math::Angle& pitch,
@@ -50,7 +55,7 @@ private:
     template<typename T>
     static std::vector<T> LoadVector(const std::filesystem::path& path);
 
-    static std::map<std::string, std::string> presetNameToFilename;
+    static std::map<std::string, std::pair<std::string, std::size_t>> presetNameToLoadInfo;
     static std::map<std::string, LoadFuncType> patternLoadFunctions;
 };
 

RGLServerPlugin/include/RGLServerPluginInstance.hh

@@ -66,6 +66,7 @@ private:
                      gz::sim::EntityComponentManager& ecm);
 
     void UpdateLidarPose(const gz::sim::EntityComponentManager& ecm);
+    void UpdateAlternatingLidarPattern();
 
     bool ShouldRayTrace(std::chrono::steady_clock::duration sim_time,
                         bool paused);
@@ -86,6 +87,7 @@ private:
     gz::math::Angle scanHMax;
     int scanHSamples;
     std::vector<rgl_mat3x4f> lidarPattern;
+    std::size_t alternatingPatternIndex = 0;
 
     struct ResultPointCloud
     {
@@ -118,7 +120,7 @@ private:
     gz::transport::Node::Publisher pointCloudWorldPublisher;
     gz::transport::Node gazeboNode;
 
-    rgl_node_t rglNodeUseRays = nullptr;
+    std::vector<rgl_node_t> rglNodesUseRays;
     rgl_node_t rglNodeLidarPose = nullptr;
     rgl_node_t rglNodeSetRange = nullptr;
     rgl_node_t rglNodeRaytrace = nullptr;
@@ -136,6 +138,8 @@ private:
     int onPausedSimUpdateCounter = 0;
     const int onPausedSimRaytraceInterval = 100;
 
+    std::size_t lidarPatternSampleSize = 0;
+
     const std::string worldFrameId = "world";
     const std::string worldTopicPostfix = "/world";
 };

RGLServerPlugin/src/Lidar.cc

@@ -73,7 +73,12 @@ bool RGLServerPluginInstance::LoadConfiguration(const std::shared_ptr<const sdf:
     topicName = sdf->Get<std::string>(PARAM_TOPIC_ID);
     frameId = sdf->Get<std::string>(PARAM_FRAME_ID);
 
-    if (!LidarPatternLoader::Load(sdf, lidarPattern)) {
+    if (!LidarPatternLoader::Load(sdf, lidarPattern, lidarPatternSampleSize)) {
+        return false;
+    }
+
+    if ((lidarPattern.size() % lidarPatternSampleSize) != 0) {
+        gzerr << "Total pattern size (" << lidarPattern.size() << ") must be a multiple of the sample size (" << lidarPatternSampleSize << "). Disabling plugin.\n";
         return false;
     }
 
@@ -83,7 +88,7 @@ bool RGLServerPluginInstance::LoadConfiguration(const std::shared_ptr<const sdf:
         publishLaserScan = true;
         scanHMin = sdf->FindElement("pattern_lidar2d")->FindElement("horizontal")->Get<float>("min_angle");
         scanHMax = sdf->FindElement("pattern_lidar2d")->FindElement("horizontal")->Get<float>("max_angle");
-        scanHSamples = lidarPattern.size();
+        scanHSamples = lidarPatternSampleSize;
     }
 
     return true;
@@ -102,15 +107,25 @@ void RGLServerPluginInstance::CreateLidar(gz::sim::Entity entity,
 
     // Resize result data containers with the maximum possible point count (number of lasers).
     // This improves performance in runtime because no additional allocations are needed.
-    resultPointCloud.data.resize(resultPointCloud.pointSize * lidarPattern.size());
+    resultPointCloud.data.resize(resultPointCloud.pointSize * lidarPatternSampleSize);
     if (publishLaserScan)
     {
-        resultLaserScan.distances.resize(lidarPattern.size());
-        resultLaserScan.intensities.resize(lidarPattern.size());
+        resultLaserScan.distances.resize(lidarPatternSampleSize);
+        resultLaserScan.intensities.resize(lidarPatternSampleSize);
+    }
+
+    for (std::size_t i = 0; i < lidarPattern.size(); i += lidarPatternSampleSize)
+    {
+      rglNodesUseRays.emplace_back();
+      if (!CheckRGL(rgl_node_rays_from_mat3x4f(&rglNodesUseRays.back(),
+                                               lidarPattern.data() + i,
+                                               lidarPatternSampleSize))) {
+          gzerr << "Failed to create RGL nodes when initializing lidar. Disabling plugin.\n";
+          return;
+      }
     }
 
-    if (!CheckRGL(rgl_node_rays_from_mat3x4f(&rglNodeUseRays, lidarPattern.data(), lidarPattern.size())) ||
-        !CheckRGL(rgl_node_rays_set_range(&rglNodeSetRange, &lidarMinMaxRange, 1)) ||
+    if (!CheckRGL(rgl_node_rays_set_range(&rglNodeSetRange, &lidarMinMaxRange, 1)) ||
         !CheckRGL(rgl_node_rays_transform(&rglNodeLidarPose, &identity)) ||
         !CheckRGL(rgl_node_raytrace(&rglNodeRaytrace, nullptr)) ||
         !CheckRGL(rgl_node_points_compact_by_field(&rglNodeCompact, RGL_FIELD_IS_HIT_I32)) ||
@@ -123,7 +138,7 @@ void RGLServerPluginInstance::CreateLidar(gz::sim::Entity entity,
         return;
     }
 
-    if (!CheckRGL(rgl_graph_node_add_child(rglNodeUseRays, rglNodeSetRange)) ||
+    if (!CheckRGL(rgl_graph_node_add_child(rglNodesUseRays.front(), rglNodeSetRange)) ||
         !CheckRGL(rgl_graph_node_add_child(rglNodeSetRange, rglNodeLidarPose)) ||
         !CheckRGL(rgl_graph_node_add_child(rglNodeLidarPose, rglNodeRaytrace)) ||
         !CheckRGL(rgl_graph_node_add_child(rglNodeRaytrace, rglNodeCompact)) ||
@@ -166,6 +181,25 @@ void RGLServerPluginInstance::UpdateLidarPose(const gz::sim::EntityComponentMana
     CheckRGL(rgl_node_points_transform(&rglNodeToLidarFrame, &rglWorldToLidar));
 }
 
+void RGLServerPluginInstance::UpdateAlternatingLidarPattern()
+{
+    // remove old child
+    if(!CheckRGL(rgl_graph_node_remove_child(rglNodesUseRays[alternatingPatternIndex], rglNodeSetRange)))
+    {
+        gzerr << "Failed to update alternating lidar pattern, not able to remove child.\n";
+        return;
+    }
+
+    alternatingPatternIndex = (alternatingPatternIndex + 1) % rglNodesUseRays.size();
+
+    // add new child
+    if(!CheckRGL(rgl_graph_node_add_child(rglNodesUseRays[alternatingPatternIndex], rglNodeSetRange)))
+    {
+        gzerr << "Failed to update alternating lidar pattern, not able to add new child.\n";
+        return;
+    }
+}
+
 bool RGLServerPluginInstance::ShouldRayTrace(std::chrono::steady_clock::duration simTime,
                                              bool paused)
 {
@@ -194,6 +228,10 @@ bool RGLServerPluginInstance::ShouldRayTrace(std::chrono::steady_clock::duration
 
 void RGLServerPluginInstance::RayTrace(std::chrono::steady_clock::duration simTime)
 {
+    if (rglNodesUseRays.size() > 1) {
+        UpdateAlternatingLidarPattern();
+    }
+
     lastRaytraceTime = simTime;
 
     if (!CheckRGL(rgl_graph_run(rglNodeRaytrace))) {