diff --git a/README.md b/README.md index 4ed61bf..3de0e61 100644 --- a/README.md +++ b/README.md @@ -194,6 +194,12 @@ Inside the link entity in your model, add a custom sensor: OS1 64 Pandar64 Pandar40P + Livox Avia + Livox Horizon + Livox Mid40 + Livox Mid70 + Livox Mid360 + Livox Tele15 ``` **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 diff --git a/RGLServerPlugin/include/LidarPatternLoader.hh b/RGLServerPlugin/include/LidarPatternLoader.hh index 73c9a5b..3ef2ff8 100644 --- a/RGLServerPlugin/include/LidarPatternLoader.hh +++ b/RGLServerPlugin/include/LidarPatternLoader.hh @@ -15,6 +15,10 @@ #pragma once #include +#include +#include +#include +#include #include #include @@ -26,9 +30,10 @@ namespace rgl class LidarPatternLoader { public: - using LoadFuncType = std::function&)>; + using LoadFuncType = std::function&, std::size_t&)>; - static bool Load(const sdf::ElementConstPtr& sdf, std::vector& outPattern); + static bool Load(const sdf::ElementConstPtr& sdf, std::vector& 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& outPattern); - static bool LoadPatternFromCustom(const sdf::ElementConstPtr& sdf, std::vector& outPattern); - static bool LoadPatternFromPreset(const sdf::ElementConstPtr& sdf, std::vector& outPattern); - static bool LoadPatternFromPresetPath(const sdf::ElementConstPtr& sdf, std::vector& outPattern); - static bool LoadPatternFromLidar2d(const sdf::ElementConstPtr& sdf, std::vector& outPattern); + static bool LoadPatternFromUniform(const sdf::ElementConstPtr& sdf, std::vector& outPattern, std::size_t& outPatternScanSize); + static bool LoadPatternFromCustom(const sdf::ElementConstPtr& sdf, std::vector& outPattern, std::size_t& outPatternScanSize); + static bool LoadPatternFromPreset(const sdf::ElementConstPtr& sdf, std::vector& outPattern, std::size_t& outPatternScanSize); + static bool LoadPatternFromPresetPath(const sdf::ElementConstPtr& sdf, std::vector& outPattern, std::size_t& outPatternScanSize); + static bool LoadPatternFromLidar2d(const sdf::ElementConstPtr& sdf, std::vector& 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 static std::vector LoadVector(const std::filesystem::path& path); - static std::map presetNameToFilename; + static std::map> presetNameToLoadInfo; static std::map patternLoadFunctions; }; diff --git a/RGLServerPlugin/include/RGLServerPluginInstance.hh b/RGLServerPlugin/include/RGLServerPluginInstance.hh index 52d2199..1dfa5fe 100644 --- a/RGLServerPlugin/include/RGLServerPluginInstance.hh +++ b/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 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 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"; }; diff --git a/RGLServerPlugin/src/Lidar.cc b/RGLServerPlugin/src/Lidar.cc index 7367f45..61c2a1f 100644 --- a/RGLServerPlugin/src/Lidar.cc +++ b/RGLServerPlugin/src/Lidar.cc @@ -73,7 +73,12 @@ bool RGLServerPluginInstance::LoadConfiguration(const std::shared_ptrGet(PARAM_TOPIC_ID); frameId = sdf->Get(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_ptrFindElement("pattern_lidar2d")->FindElement("horizontal")->Get("min_angle"); scanHMax = sdf->FindElement("pattern_lidar2d")->FindElement("horizontal")->Get("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))) { diff --git a/RGLServerPlugin/src/LidarPatternLoader.cc b/RGLServerPlugin/src/LidarPatternLoader.cc index aac21d8..73adfc8 100644 --- a/RGLServerPlugin/src/LidarPatternLoader.cc +++ b/RGLServerPlugin/src/LidarPatternLoader.cc @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include "LidarPatternLoader.hh" #include "gz/math/Matrix4.hh" @@ -25,33 +26,41 @@ namespace fs = std::filesystem; namespace rgl { -std::map 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 number of patterns contained in the file (> 1 for alternating patterns) +std::map> LidarPatternLoader::presetNameToLoadInfo = { + {"Alpha Prime", {"VelodyneVLS128.mat3x4f", 1}}, + {"Puck", {"VelodyneVLP16.mat3x4f", 1}}, + {"Ultra Puck", {"VelodyneVLP32C.mat3x4f", 1}}, + {"OS1 64", {"OusterOS1_64.mat3x4f", 1}}, + {"Pandar64", {"HesaiPandarQT64.mat3x4f", 1}}, + {"Pandar40P", {"HesaiPandar40P.mat3x4f", 1}}, + {"Livox Avia", {"LivoxAvia.mat3x4f", 40}}, + {"Livox Horizon", {"LivoxHorizon.mat3x4f", 40}}, + {"Livox Mid40", {"LivoxMid40.mat3x4f", 40}}, + {"Livox Mid70", {"LivoxMid70.mat3x4f", 40}}, + {"Livox Mid360", {"LivoxMid360.mat3x4f", 40}}, + {"Livox Tele15", {"LivoxTele15.mat3x4f", 40}}, }; std::map 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& outPattern) +bool LidarPatternLoader::Load(const sdf::ElementConstPtr& sdf, std::vector& outPattern, + std::size_t& outPatternScanSize) { - for (const auto &[patterName, loadFunction]: patternLoadFunctions) + for (const auto &[patternName, loadFunction]: patternLoadFunctions) { - if (!sdf->HasElement(patterName)) { + if (!sdf->HasElement(patternName)) { continue; } - gzmsg << "Trying to load '" << patterName << "' pattern...\n"; - if (loadFunction(sdf->FindElement(patterName), outPattern)) { - gzmsg << "Successfully loaded pattern '" << patterName << "'.\n"; + gzmsg << "Trying to load '" << patternName << "' pattern...\n"; + if (loadFunction(sdf->FindElement(patternName), outPattern, outPatternScanSize)) { + gzmsg << "Successfully loaded pattern '" << patternName << "'.\n"; return true; } } @@ -95,7 +104,7 @@ bool LidarPatternLoader::LoadAnglesAndSamplesElement(const sdf::ElementConstPtr& return true; } -bool LidarPatternLoader::LoadPatternFromUniform(const sdf::ElementConstPtr& sdf, std::vector& outPattern) +bool LidarPatternLoader::LoadPatternFromUniform(const sdf::ElementConstPtr& sdf, std::vector& outPattern, std::size_t& outPatternScanSize) { if (!sdf->HasElement("vertical")) { gzerr << "Failed to load uniform pattern. A vertical element is required, but it is not set.\n"; @@ -136,10 +145,13 @@ bool LidarPatternLoader::LoadPatternFromUniform(const sdf::ElementConstPtr& sdf, } vAngle += vStep; } + + outPatternScanSize = outPattern.size(); + return true; } -bool LidarPatternLoader::LoadPatternFromCustom(const sdf::ElementConstPtr& sdf, std::vector& outPattern) +bool LidarPatternLoader::LoadPatternFromCustom(const sdf::ElementConstPtr& sdf, std::vector& outPattern, std::size_t& outPatternScanSize) { if (!sdf->HasAttribute("channels")) { gzerr << "Failed to load custom pattern. A channels attribute is required, but it is not set.\n"; @@ -180,30 +192,45 @@ bool LidarPatternLoader::LoadPatternFromCustom(const sdf::ElementConstPtr& sdf, hAngle += hStep; } } + + outPatternScanSize = outPattern.size(); + return true; } -bool LidarPatternLoader::LoadPatternFromPreset(const sdf::ElementConstPtr& sdf, std::vector& outPattern) +bool LidarPatternLoader::LoadPatternFromPreset(const sdf::ElementConstPtr& sdf, std::vector& outPattern, std::size_t& outPatternScanSize) { - auto presetName = sdf->Get(); - if (!presetNameToFilename.contains(presetName)) { + const auto presetName = sdf->Get(); + if (!presetNameToLoadInfo.contains(presetName)) { gzerr << "Failed to load preset pattern. Preset '" << presetName << "' is not available.\n"; return false; } - fs::path presetPath = presetNameToFilename[presetName]; + + auto [presetPath, presetPatternCount] = presetNameToLoadInfo[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(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 (outPattern.size() % presetPatternCount != 0) { + gzerr << "Failed to load preset with alternating pattern. Total pattern vector size (" << outPattern.size() + << ") must be a multiple of the pattern count (" << presetPatternCount << ")!\n"; + return false; + } + + outPatternScanSize = outPattern.size() / presetPatternCount; + return true; } -bool LidarPatternLoader::LoadPatternFromPresetPath(const sdf::ElementConstPtr& sdf, std::vector& outPattern) +bool LidarPatternLoader::LoadPatternFromPresetPath(const sdf::ElementConstPtr& sdf, std::vector& outPattern, std::size_t& outPatternScanSize) { fs::path presetPath = fs::path(sdf->Get()); 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; } + + outPatternScanSize = outPattern.size(); + return true; } -bool LidarPatternLoader::LoadPatternFromLidar2d(const sdf::ElementConstPtr& sdf, std::vector& outPattern) +bool LidarPatternLoader::LoadPatternFromLidar2d(const sdf::ElementConstPtr& sdf, std::vector& outPattern, std::size_t& outPatternScanSize) { 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; } + outPatternScanSize = outPattern.size(); + return true; } diff --git a/lidar_patterns/LivoxAvia.mat3x4f b/lidar_patterns/LivoxAvia.mat3x4f new file mode 100644 index 0000000..70ad394 Binary files /dev/null and b/lidar_patterns/LivoxAvia.mat3x4f differ diff --git a/lidar_patterns/LivoxHorizon.mat3x4f b/lidar_patterns/LivoxHorizon.mat3x4f new file mode 100644 index 0000000..492cc5e Binary files /dev/null and b/lidar_patterns/LivoxHorizon.mat3x4f differ diff --git a/lidar_patterns/LivoxMid360.mat3x4f b/lidar_patterns/LivoxMid360.mat3x4f new file mode 100644 index 0000000..3d8faa6 Binary files /dev/null and b/lidar_patterns/LivoxMid360.mat3x4f differ diff --git a/lidar_patterns/LivoxMid40.mat3x4f b/lidar_patterns/LivoxMid40.mat3x4f new file mode 100644 index 0000000..4937755 Binary files /dev/null and b/lidar_patterns/LivoxMid40.mat3x4f differ diff --git a/lidar_patterns/LivoxMid70.mat3x4f b/lidar_patterns/LivoxMid70.mat3x4f new file mode 100644 index 0000000..4142020 Binary files /dev/null and b/lidar_patterns/LivoxMid70.mat3x4f differ diff --git a/lidar_patterns/LivoxTele15.mat3x4f b/lidar_patterns/LivoxTele15.mat3x4f new file mode 100644 index 0000000..6e5e427 Binary files /dev/null and b/lidar_patterns/LivoxTele15.mat3x4f differ