Skip to content

Commit

Permalink
Implement support for non-repetitive/alternating lidar scan patterns (#…
Browse files Browse the repository at this point in the history 
…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
  • Loading branch information
@Roboterbastler
Roboterbastler authored Jan 7, 2025
1 parent 7184305 commit 7e5833b
Show file tree
Hide file tree
Showing 11 changed files with 129 additions and 44 deletions.
6 changes: 6 additions & 0 deletions README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
21 changes: 13 additions & 8 deletions RGLServerPlugin/include/LidarPatternLoader.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,10 @@
#pragma once

#include <filesystem>
#include <functional>
#include <map>
#include <string>
#include <utility>
#include <vector>

#include <rgl/api/core.h>
Expand All @@ -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() {};
Expand All @@ -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,
Expand All @@ -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;
};

Expand Down
6 changes: 5 additions & 1 deletion RGLServerPlugin/include/RGLServerPluginInstance.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -86,6 +87,7 @@ private:
gz::math::Angle scanHMax;
int scanHSamples;
std::vector<rgl_mat3x4f> lidarPattern;
std::size_t alternatingPatternIndex = 0;

struct ResultPointCloud
{
Expand Down Expand Up @@ -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;
Expand All @@ -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";
};
Expand Down
54 changes: 46 additions & 8 deletions RGLServerPlugin/src/Lidar.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand All @@ -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;
Expand All @@ -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)) ||
Expand All @@ -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)) ||
Expand Down Expand Up @@ -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)
{
Expand Down Expand Up @@ -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))) {
Expand Down
Loading

0 comments on commit 7e5833b

Please sign in to comment.