Implement support for non-repetitive/alternating lidar scan patterns

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& outPatternSampleSize);
 
 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& outPatternSampleSize);
+    static bool LoadPatternFromCustom(const sdf::ElementConstPtr& sdf, std::vector<rgl_mat3x4f>& outPattern, std::size_t& outPatternSampleSize);
+    static bool LoadPatternFromPreset(const sdf::ElementConstPtr& sdf, std::vector<rgl_mat3x4f>& outPattern, std::size_t& outPatternSampleSize);
+    static bool LoadPatternFromPresetPath(const sdf::ElementConstPtr& sdf, std::vector<rgl_mat3x4f>& outPattern, std::size_t& outPatternSampleSize);
+    static bool LoadPatternFromLidar2d(const sdf::ElementConstPtr& sdf, std::vector<rgl_mat3x4f>& outPattern, std::size_t& outPatternSampleSize);
 
     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>> presetNameToFilename;
     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))) {

RGLServerPlugin/src/LidarPatternLoader.cc

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include <cstdlib>
+#include <tuple>
 
 #include "LidarPatternLoader.hh"
 #include "gz/math/Matrix4.hh"
@@ -25,32 +26,41 @@ namespace fs = std::filesystem;
 namespace rgl
 {
 
-std::map<std::string, std::string> LidarPatternLoader::presetNameToFilename = {
-    {"Alpha Prime", "VelodyneVLS128.mat3x4f"},
-    {"Puck", "VelodyneVLP16.mat3x4f"},
-    {"Ultra Puck", "VelodyneVLP32C.mat3x4f"},
-    {"OS1 64", "OusterOS1_64.mat3x4f"},
-    {"Pandar64", "HesaiPandarQT64.mat3x4f"},
-    {"Pandar40P", "HesaiPandar40P.mat3x4f"}
+// map preset name to pair of filename and sample size (for alternating patterns)
+// set sample size to 0 to indicate non-alternating patterns
+std::map<std::string, std::pair<std::string, std::size_t>> LidarPatternLoader::presetNameToFilename = {
+    {"Alpha Prime", {"VelodyneVLS128.mat3x4f", 0}},
+    {"Puck", {"VelodyneVLP16.mat3x4f", 0}},
+    {"Ultra Puck", {"VelodyneVLP32C.mat3x4f", 0}},
+    {"OS1 64", {"OusterOS1_64.mat3x4f", 0}},
+    {"Pandar64", {"HesaiPandarQT64.mat3x4f", 0}},
+    {"Pandar40P", {"HesaiPandar40P.mat3x4f", 0}},
+    {"Livox Avia", {"LivoxAvia.mat3x4f", 24000}},
+    {"Livox Horizon", {"LivoxHorizon.mat3x4f", 24000}},
+    {"Livox Mid40", {"LivoxMid40.mat3x4f", 10000}},
+    {"Livox Mid70", {"LivoxMid70.mat3x4f", 10000}},
+    {"Livox Mid360", {"LivoxMid360.mat3x4f", 20000}},
+    {"Livox Tele15", {"LivoxTele15.mat3x4f", 24000}},
 };
 
 std::map<std::string, LidarPatternLoader::LoadFuncType> LidarPatternLoader::patternLoadFunctions = {
-    {"pattern_uniform", std::bind(&LidarPatternLoader::LoadPatternFromUniform, _1, _2)},
-    {"pattern_custom", std::bind(&LidarPatternLoader::LoadPatternFromCustom, _1, _2)},
-    {"pattern_preset", std::bind(&LidarPatternLoader::LoadPatternFromPreset, _1, _2)},
-    {"pattern_preset_path", std::bind(&LidarPatternLoader::LoadPatternFromPresetPath, _1, _2)},
-    {"pattern_lidar2d", std::bind(&LidarPatternLoader::LoadPatternFromLidar2d, _1, _2)}
+    {"pattern_uniform", std::bind(&LidarPatternLoader::LoadPatternFromUniform, _1, _2, _3)},
+    {"pattern_custom", std::bind(&LidarPatternLoader::LoadPatternFromCustom, _1, _2, _3)},
+    {"pattern_preset", std::bind(&LidarPatternLoader::LoadPatternFromPreset, _1, _2, _3)},
+    {"pattern_preset_path", std::bind(&LidarPatternLoader::LoadPatternFromPresetPath, _1, _2, _3)},
+    {"pattern_lidar2d", std::bind(&LidarPatternLoader::LoadPatternFromLidar2d, _1, _2, _3)},
 };
 
-bool LidarPatternLoader::Load(const sdf::ElementConstPtr& sdf, std::vector<rgl_mat3x4f>& outPattern)
+bool LidarPatternLoader::Load(const sdf::ElementConstPtr& sdf, std::vector<rgl_mat3x4f>& outPattern,
+                              std::size_t& outPatternSampleSize)
 {
     for (const auto &[patterName, loadFunction]: patternLoadFunctions)
     {
         if (!sdf->HasElement(patterName)) {
             continue;
         }
         gzmsg << "Trying to load '" << patterName << "' pattern...\n";
-        if (loadFunction(sdf->FindElement(patterName), outPattern)) {
+        if (loadFunction(sdf->FindElement(patterName), outPattern, outPatternSampleSize)) {
             gzmsg << "Successfully loaded pattern '" << patterName << "'.\n";
             return true;
         }
@@ -95,7 +105,7 @@ bool LidarPatternLoader::LoadAnglesAndSamplesElement(const sdf::ElementConstPtr&
     return true;
 }
 
-bool LidarPatternLoader::LoadPatternFromUniform(const sdf::ElementConstPtr& sdf, std::vector<rgl_mat3x4f>& outPattern)
+bool LidarPatternLoader::LoadPatternFromUniform(const sdf::ElementConstPtr& sdf, std::vector<rgl_mat3x4f>& outPattern, std::size_t& outPatternSampleSize)
 {
     if (!sdf->HasElement("vertical")) {
         gzerr << "Failed to load uniform pattern. A vertical element is required, but it is not set.\n";
@@ -136,10 +146,13 @@ bool LidarPatternLoader::LoadPatternFromUniform(const sdf::ElementConstPtr& sdf,
         }
         vAngle += vStep;
     }
+
+    outPatternSampleSize = outPattern.size();
+
     return true;
 }
 
-bool LidarPatternLoader::LoadPatternFromCustom(const sdf::ElementConstPtr& sdf, std::vector<rgl_mat3x4f>& outPattern)
+bool LidarPatternLoader::LoadPatternFromCustom(const sdf::ElementConstPtr& sdf, std::vector<rgl_mat3x4f>& outPattern, std::size_t& outPatternSampleSize)
 {
     if (!sdf->HasAttribute("channels")) {
         gzerr << "Failed to load custom pattern. A channels attribute is required, but it is not set.\n";
@@ -180,30 +193,44 @@ bool LidarPatternLoader::LoadPatternFromCustom(const sdf::ElementConstPtr& sdf,
             hAngle += hStep;
         }
     }
+
+    outPatternSampleSize = outPattern.size();
+
     return true;
 }
 
-bool LidarPatternLoader::LoadPatternFromPreset(const sdf::ElementConstPtr& sdf, std::vector<rgl_mat3x4f>& outPattern)
+bool LidarPatternLoader::LoadPatternFromPreset(const sdf::ElementConstPtr& sdf, std::vector<rgl_mat3x4f>& outPattern, std::size_t& outPatternSampleSize)
 {
     auto presetName = sdf->Get<std::string>();
     if (!presetNameToFilename.contains(presetName)) {
         gzerr << "Failed to load preset pattern. Preset '" << presetName << "' is not available.\n";
         return false;
     }
-    fs::path presetPath = presetNameToFilename[presetName];
+    fs::path presetPath;
+    std::tie(presetPath, outPatternSampleSize) = presetNameToFilename[presetName];
     if (const char* presetDir = std::getenv(PATTERNS_DIR_ENV)) {
-        presetPath = fs::path(presetDir) / presetNameToFilename[presetName];
+        presetPath = fs::path(presetDir) / presetPath;
     }
     gzmsg << "Loading pattern_preset '" << presetName << "'...\n";
     outPattern = LoadVector<rgl_mat3x4f>(presetPath);
     if (outPattern.size() == 0) {
         gzerr << "Failed to load preset. Make sure the environment variable '" << PATTERNS_DIR_ENV << "' is set correctly.\n";
         return false;
     }
+
+    if (outPatternSampleSize == 0) {
+        // non-alternating patterns have sample size equal to total pattern size
+        outPatternSampleSize = outPattern.size();
+    }
+    else if (outPattern.size() % outPatternSampleSize != 0) {
+        gzerr << "Failed to load preset alternating pattern. Total pattern size (" << outPattern.size() << ") must be a multiple of the sample size!\n";
+        return false;
+    }
+
     return true;
 }
 
-bool LidarPatternLoader::LoadPatternFromPresetPath(const sdf::ElementConstPtr& sdf, std::vector<rgl_mat3x4f>& outPattern)
+bool LidarPatternLoader::LoadPatternFromPresetPath(const sdf::ElementConstPtr& sdf, std::vector<rgl_mat3x4f>& outPattern, std::size_t& outPatternSampleSize)
 {
     fs::path presetPath = fs::path(sdf->Get<std::string>());
     gzmsg << "Loading preset from path '" << presetPath << "'...\n";
@@ -212,10 +239,13 @@ bool LidarPatternLoader::LoadPatternFromPresetPath(const sdf::ElementConstPtr& s
         gzerr << "Failed to load preset from path.\n";
         return false;
     }
+
+    outPatternSampleSize = outPattern.size();
+
     return true;
 }
 
-bool LidarPatternLoader::LoadPatternFromLidar2d(const sdf::ElementConstPtr& sdf, std::vector<rgl_mat3x4f>& outPattern)
+bool LidarPatternLoader::LoadPatternFromLidar2d(const sdf::ElementConstPtr& sdf, std::vector<rgl_mat3x4f>& outPattern, std::size_t& outPatternSampleSize)
 {
     if (!sdf->HasElement("horizontal")) {
         gzerr << "Failed to load uniform pattern. A horizontal element is required, but it is not set.\n";
@@ -244,6 +274,8 @@ bool LidarPatternLoader::LoadPatternFromLidar2d(const sdf::ElementConstPtr& sdf,
         hAngle += hStep;
     }
 
+    outPatternSampleSize = outPattern.size();
+
     return true;
 }