Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Merged
merged 9 commits into from
Jan 7, 2025
Merged

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

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
9455ed3
Implement alternating lidar pattern
Roboterbastler Nov 4, 2024
d50f971
Add Livox Mid360 preset
Roboterbastler Nov 6, 2024
584c049
Add Livox Avia preset
Roboterbastler Nov 6, 2024
c417dc7
Add Livox Horizon preset
Roboterbastler Nov 6, 2024
d221938
Add Livox Mid40 preset
Roboterbastler Nov 6, 2024
518efb2
Add Livox Mid70 preset
Roboterbastler Nov 6, 2024
a22a264
Add Livox Tele15 preset
Roboterbastler Nov 6, 2024
65dd338
Add new lidar presets to readme
Roboterbastler Nov 8, 2024
c58dd15
Refactor LidarPatternLoader
Roboterbastler Jan 6, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 6 additions & 0 deletions README.md
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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