Add Laserscan support (#28)

* LaserScan support update

* readme update

* fix no intensity segfault

* add lidar2d pattern loader + utils

* bug fix

* Hacky metadata

* YieldNode switching

* fix node for visualize

* clean up + get min max angles from sdf

* minor cleanup

* Readme Update

* Apply suggestions from code review

Co-authored-by: Mateusz Szczygielski <[email protected]>

* clear vector before resereving

---------

Co-authored-by: Mateusz Szczygielski <[email protected]>

Author: ajsampathk

README.md

@@ -185,6 +185,29 @@ Inside the link entity in your model, add a custom sensor:
   <pattern_preset_path>/home/some1/your-preset.mat3x4f</pattern_preset_path>
   ```
 
+- **pattern_lidar2d**\
+  Almost the same as `pattern_uniform` but only has 
+  the `horizontal` element and publishes a 
+  `LaserScan` message instead of a point cloud
+  ```xml
+  <sensor name="Pattern2DLidar" type="custom">
+    <plugin filename="RGLServerPluginInstance" name="rgl::RGLServerPluginInstance">
+      <range>100</range>
+      <update_rate>10</update_rate>
+      <update_on_paused_sim>false</update_on_paused_sim>
+      <topic>rgl_lidar</topic>
+      <frame>RGLLidar</frame>
+      <pattern_lidar2d>
+        <horizontal>
+            <samples>1800</samples>
+            <min_angle>-3.14159</min_angle>
+            <max_angle>3.14159</max_angle>
+        </horizontal>
+    </pattern_lidar2d>
+    </plugin>
+  </sensor>
+  ```
+
 ## How to visualize in Gazebo
 To enable non-uniform point clouds visualization in Gazebo Fortress that are produced by `RGLServerPlugin`, we port [PointCloud gui plugin](https://github.com/gazebosim/gz-gui/tree/gz-gui7/src/plugins/point_cloud) from Gazebo Garden and create `RGLVisualize`. It reimplements the minimal functionality of receiving PointCloudPacked messages (in a world coordinate frame) and rendering them in the scene.
 
@@ -202,4 +225,4 @@ At [Robotec.AI](https://robotec.ai/) we care about every little detail of our pr
 ## Ray pattern comparison
 | **RGL uniform pattern** | **gpu_lidar uniform pattern** |
 |---|---|
-| ![](docs/images/RGL_uniform.png) | ![](docs/images/GPU_lidar_uniform.png) |
+| ![](docs/images/RGL_uniform.png) | ![](docs/images/GPU_lidar_uniform.png) |

RGLServerPlugin/include/LidarPatternLoader.hh

@@ -42,11 +42,12 @@ private:
     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 rgl_mat3x4f AnglesToRglMat3x4f(const ignition::math::Angle& roll,
                                           const ignition::math::Angle& pitch,
                                           const ignition::math::Angle& yaw);
-    
+
     template<typename T>
     static std::vector<T> LoadVector(const std::filesystem::path& path);
 

RGLServerPlugin/include/RGLServerPluginInstance.hh

@@ -70,26 +70,34 @@ private:
     void RayTrace(std::chrono::steady_clock::duration sim_time);
 
     ignition::msgs::PointCloudPacked CreatePointCloudMsg(std::chrono::steady_clock::duration sim_time, std::string frame, int hitpointCount);
+    ignition::msgs::LaserScan CreateLaserScanMsg(std::chrono::steady_clock::duration sim_time, std::string frame, int hitpointCount);
 
     void DestroyLidar();
 
     std::string topicName;
     std::string frameId;
     float lidarRange;
+    ignition::math::Angle scanHMin;
+    ignition::math::Angle scanHMax;
+    int scanHSamples;
     std::vector<rgl_mat3x4f> lidarPattern;
     std::vector<rgl_vec3f> resultPointCloud;
+    std::vector<float> resultDistances;
 
     bool updateOnPausedSim = false;
+    bool publishLaserScan = false;
 
     ignition::gazebo::Entity thisLidarEntity;
     ignition::transport::Node::Publisher pointCloudPublisher;
+    ignition::transport::Node::Publisher laserScanPublisher;
     ignition::transport::Node::Publisher pointCloudWorldPublisher;
     ignition::transport::Node gazeboNode;
 
     rgl_node_t rglNodeUseRays = nullptr;
     rgl_node_t rglNodeLidarPose = nullptr;
     rgl_node_t rglNodeRaytrace = nullptr;
     rgl_node_t rglNodeCompact = nullptr;
+    rgl_node_t rglNodeYield = nullptr;
     rgl_node_t rglNodeToLidarFrame = nullptr;
 
     std::chrono::steady_clock::duration raytraceIntervalTime;

RGLServerPlugin/src/Lidar.cc

@@ -67,6 +67,17 @@ bool RGLServerPluginInstance::LoadConfiguration(const std::shared_ptr<const sdf:
     if (!LidarPatternLoader::Load(sdf, lidarPattern)) {
         return false;
     }
+
+    // Check for 2d pattern and get LaserScan parameters
+    if (sdf->HasElement("pattern_lidar2d")) {
+        ignmsg << "Lidar is 2D, switching to publish LaserScan messages";
+        publishLaserScan = true;
+        resultDistances.resize(lidarPattern.size());
+        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();
+    }
+
     // Resize container for result point cloud
     resultPointCloud.resize(lidarPattern.size());
 
@@ -84,10 +95,19 @@ void RGLServerPluginInstance::CreateLidar(ignition::gazebo::Entity entity,
         0, 0, 1, 0
     };
 
+    // set desired fields for API call
+    std::vector<rgl_field_t> yieldFields;
+    yieldFields.push_back(RGL_FIELD_XYZ_F32);
+
+    if (publishLaserScan) {
+        yieldFields.push_back(RGL_FIELD_DISTANCE_F32);
+    }
+
     if (!CheckRGL(rgl_node_rays_from_mat3x4f(&rglNodeUseRays, lidarPattern.data(), lidarPattern.size())) ||
         !CheckRGL(rgl_node_rays_transform(&rglNodeLidarPose, &identity)) ||
         !CheckRGL(rgl_node_raytrace(&rglNodeRaytrace, nullptr, lidarRange)) ||
         !CheckRGL(rgl_node_points_compact(&rglNodeCompact)) ||
+        !CheckRGL(rgl_node_points_yield(&rglNodeYield, yieldFields.data(), yieldFields.size())) ||
         !CheckRGL(rgl_node_points_transform(&rglNodeToLidarFrame, &identity))) {
 
         ignerr << "Failed to create RGL nodes when initializing lidar. Disabling plugin.\n";
@@ -103,7 +123,23 @@ void RGLServerPluginInstance::CreateLidar(ignition::gazebo::Entity entity,
         return;
     }
 
-    pointCloudPublisher = gazeboNode.Advertise<ignition::msgs::PointCloudPacked>(topicName);
+    if (!publishLaserScan) {
+
+        if(!CheckRGL(rgl_graph_node_add_child(rglNodeToLidarFrame, rglNodeYield))) {
+            ignerr << "Failed to connect RGL nodes when initializing lidar. Disabling plugin.\n";
+        }
+        ignmsg << "Start publishing PointCloudPacked messages on topic '" << topicName << "'\n";
+        pointCloudPublisher = gazeboNode.Advertise<ignition::msgs::PointCloudPacked>(topicName);
+
+    } else {
+
+        if(!CheckRGL(rgl_graph_node_add_child(rglNodeRaytrace, rglNodeYield))) {
+            ignerr << "Failed to connect RGL nodes when initializing lidar. Disabling plugin.\n";
+        }
+        ignmsg << "Start publishing LaserScan messages on topic '" << topicName << "'\n";
+        laserScanPublisher = gazeboNode.Advertise<ignition::msgs::LaserScan>(topicName);
+
+    }
     pointCloudWorldPublisher = gazeboNode.Advertise<ignition::msgs::PointCloudPacked>(topicName + worldTopicPostfix);
 
     isLidarInitialized = true;
@@ -155,15 +191,30 @@ void RGLServerPluginInstance::RayTrace(std::chrono::steady_clock::duration simTi
     }
 
     int32_t hitpointCount = 0;
-    if (!CheckRGL(rgl_graph_get_result_size(rglNodeToLidarFrame, RGL_FIELD_XYZ_F32, &hitpointCount, nullptr)) ||
-        !CheckRGL(rgl_graph_get_result_data(rglNodeToLidarFrame, RGL_FIELD_XYZ_F32, resultPointCloud.data()))) {
 
-        ignerr << "Failed to get result data from RGL lidar.\n";
+    if (!publishLaserScan) {
+        if (!CheckRGL(rgl_graph_get_result_size(rglNodeYield, RGL_FIELD_XYZ_F32, &hitpointCount, nullptr)) ||
+            !CheckRGL(rgl_graph_get_result_data(rglNodeYield, RGL_FIELD_XYZ_F32, resultPointCloud.data()))) {
+
+            ignerr << "Failed to get result data from RGL lidar.\n";
+            return;
+            }
+    } else {
+        if (!CheckRGL(rgl_graph_get_result_size(rglNodeYield, RGL_FIELD_DISTANCE_F32, &hitpointCount, nullptr)) ||
+            !CheckRGL(rgl_graph_get_result_data(rglNodeYield, RGL_FIELD_DISTANCE_F32, resultDistances.data()))) {
+
+        ignerr << "Failed to get result distances from RGL lidar.\n";
         return;
+        }
     }
 
-    auto msg = CreatePointCloudMsg(simTime, frameId, hitpointCount);
-    pointCloudPublisher.Publish(msg);
+    if (!publishLaserScan) {
+        auto msg = CreatePointCloudMsg(simTime, frameId, hitpointCount);
+        pointCloudPublisher.Publish(msg);
+    } else {
+        auto msg = CreateLaserScanMsg(simTime, frameId, hitpointCount);
+        laserScanPublisher.Publish(msg);
+    }
 
     if (pointCloudWorldPublisher.HasConnections()) {
         if (!CheckRGL(rgl_graph_get_result_size(rglNodeToLidarFrame, RGL_FIELD_XYZ_F32, &hitpointCount, nullptr)) ||
@@ -177,6 +228,36 @@ void RGLServerPluginInstance::RayTrace(std::chrono::steady_clock::duration simTi
     }
 }
 
+ignition::msgs::LaserScan RGLServerPluginInstance::CreateLaserScanMsg(std::chrono::steady_clock::duration simTime, std::string frame, int hitpointCount)
+{
+    ignition::msgs::LaserScan outMsg;
+    *outMsg.mutable_header()->mutable_stamp() = ignition::msgs::Convert(simTime);
+    auto _frame = outMsg.mutable_header()->add_data();
+    _frame->set_key("frame_id");
+    _frame->add_value(frame);
+
+    outMsg.set_frame(frame);
+    outMsg.set_count(hitpointCount);
+
+    outMsg.set_range_min(0.0);
+    outMsg.set_range_max(lidarRange);
+
+    ignition::math::Angle hStep((scanHMax-scanHMin)/scanHSamples);
+
+    outMsg.set_angle_min(scanHMin.Radian());
+    outMsg.set_angle_max(scanHMax.Radian());
+    outMsg.set_angle_step(hStep.Radian());
+
+    if (outMsg.ranges_size() != hitpointCount) {
+        for (int i=0; i < hitpointCount; ++i) {
+            outMsg.add_ranges(resultDistances[i]);
+            outMsg.add_intensities(100.0);
+        }
+    }
+
+    return outMsg;
+}
+
 ignition::msgs::PointCloudPacked RGLServerPluginInstance::CreatePointCloudMsg(std::chrono::steady_clock::duration simTime, std::string frame, int hitpointCount)
 {
     ignition::msgs::PointCloudPacked outMsg;
@@ -202,7 +283,11 @@ void RGLServerPluginInstance::DestroyLidar()
         ignerr << "Failed to destroy RGL lidar.\n";
     }
     // Reset publishers
-    pointCloudPublisher = ignition::transport::Node::Publisher();
+    if (!publishLaserScan) {
+        pointCloudPublisher = ignition::transport::Node::Publisher();
+    } else {
+        laserScanPublisher = ignition::transport::Node::Publisher();
+    }
     pointCloudWorldPublisher = ignition::transport::Node::Publisher();
     isLidarInitialized = false;
 }

RGLServerPlugin/src/LidarPatternLoader.cc

@@ -37,7 +37,8 @@ std::map<std::string, LidarPatternLoader::LoadFuncType> LidarPatternLoader::patt
     {"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_preset_path", std::bind(&LidarPatternLoader::LoadPatternFromPresetPath, _1, _2)},
+    {"pattern_lidar2d", std::bind(&LidarPatternLoader::LoadPatternFromLidar2d, _1, _2)}
 };
 
 bool LidarPatternLoader::Load(const sdf::ElementConstPtr& sdf, std::vector<rgl_mat3x4f>& outPattern)
@@ -213,6 +214,38 @@ bool LidarPatternLoader::LoadPatternFromPresetPath(const sdf::ElementConstPtr& s
     return true;
 }
 
+bool LidarPatternLoader::LoadPatternFromLidar2d(const sdf::ElementConstPtr& sdf, std::vector<rgl_mat3x4f>& outPattern)
+{
+    if (!sdf->HasElement("horizontal")) {
+        ignerr << "Failed to load uniform pattern. A horizontal element is required, but it is not set.\n";
+        return false;
+    }
+
+    ignition::math::Angle hMin, hMax;
+    int hSamples;
+
+    if (!LoadAnglesAndSamplesElement(sdf->FindElement("horizontal"), hMin, hMax, hSamples)) {
+        return false;
+    }
+
+    outPattern.clear();
+    outPattern.reserve(hSamples);
+
+    ignition::math::Angle hStep((hMax - hMin) / static_cast<double>(hSamples));
+
+    auto hAngle = hMin;
+    for (int i = 0; i < hSamples; ++i) {
+        outPattern.push_back(
+            AnglesToRglMat3x4f(ignition::math::Angle::Zero,
+                                // Inverse and shift 90deg pitch to match uniform pattern from Gazebo
+                                ignition::math::Angle::HalfPi,
+                                hAngle));
+        hAngle += hStep;
+    }
+
+    return true;
+}
+
 template<typename T>
 std::vector<T> LidarPatternLoader::LoadVector(const fs::path& path)
 {