Add ring ID provider support

Signed-off-by: Aleksander Kamiński <[email protected]>

Author: alek-kam-robotec-ai

Code/Source/Lidar/LidarRaycaster.cpp

@@ -113,6 +113,11 @@ namespace RGL
             m_rglRaycastResults.m_fields.push_back(RGL_FIELD_ENTITY_ID_I32);
         }
 
+        if (ROS2::IsFlagEnabled(ROS2::RaycastResultFlags::Ring, flags))
+        {
+            m_rglRaycastResults.m_fields.push_back(RGL_FIELD_RING_ID_U16);
+        }
+
         m_graph.ConfigureFieldNodes(m_rglRaycastResults.m_fields.data(), m_rglRaycastResults.m_fields.size());
         m_graph.SetIsCompactEnabled(!m_returnNonHits);
     }
@@ -180,6 +185,11 @@ namespace RGL
                 });
         }
 
+        if (auto ring = raycastResults.GetFieldSpan<ROS2::RaycastResultFlags::Ring>(); ring.has_value())
+        {
+            AZStd::copy(m_rglRaycastResults.m_ringId.begin(), m_rglRaycastResults.m_ringId.end(), ring.value().begin());
+        }
+
         return AZ::Success(m_raycastResults.value());
     }
 
@@ -222,6 +232,11 @@ namespace RGL
         m_graph.SetIsCompactEnabled(!returnNonHits);
     }
 
+    void LidarRaycaster::ConfigureRayRingIds(const AZStd::vector<AZ::s32>& ringIds)
+    {
+        m_graph.ConfigureRayRingIds(ringIds);
+    }
+
     AZStd::optional<size_t> LidarRaycaster::GetRglResultsSize(
         const PipelineGraph::RaycastResults& rglResults, const ROS2::RaycastResults& results)
     {
@@ -281,6 +296,18 @@ namespace RGL
             }
         }
 
+        if (results.IsFieldPresent<ROS2::RaycastResultFlags::Ring>())
+        {
+            if (!resultsSize.has_value())
+            {
+                resultsSize = rglResults.m_ringId.size();
+            }
+            else if (resultsSize != rglResults.m_ringId.size())
+            {
+                return AZStd::nullopt;
+            }
+        }
+
         return resultsSize;
     }
 } // namespace RGL

Code/Source/Lidar/LidarRaycaster.h

@@ -50,6 +50,7 @@ namespace RGL
         void ExcludeEntities(const AZStd::vector<AZ::EntityId>& excludedEntities) override;
         void UpdateNonHitValues();
         void ConfigureNonHitReturn(bool returnNonHits) override;
+        void ConfigureRayRingIds(const AZStd::vector<AZ::s32>& ringIds) override;
 
     private:
         AZ::Uuid m_uuid;

Code/Source/Lidar/LidarSystem.cpp

@@ -30,7 +30,7 @@ namespace RGL
 
         using Features = ROS2::LidarSystemFeatures;
         static constexpr auto SupportedFeatures = aznumeric_cast<Features>(
-            Features::EntityExclusion | Features::Noise | Features::Intensity | Features::Segmentation);
+            Features::EntityExclusion | Features::Noise | Features::Intensity | Features::Segmentation | Features::RingIds);
 
         ROS2::LidarSystemRequestBus::Handler::BusConnect(AZ_CRC(name));
 

Code/Source/Lidar/PipelineGraph.cpp

@@ -21,6 +21,7 @@ namespace RGL
     {
         ConfigureRayPosesNode({ Utils::IdentityTransform });
         ConfigureRayRangesNode(0.0f, 1.0f);
+        ConfigureRayRingIds({ 0 });
         ConfigureLidarTransformNode(AZ::Matrix3x4::CreateIdentity());
         RGL_CHECK(rgl_node_raytrace(&m_nodes.m_rayTrace, nullptr));
         RGL_CHECK(rgl_node_points_compact_by_field(&m_nodes.m_pointsCompact, RGL_FIELD_IS_HIT_I32));
@@ -30,7 +31,8 @@ namespace RGL
 
         // Non-conditional connections
         RGL_CHECK(rgl_graph_node_add_child(m_nodes.m_rayPoses, m_nodes.m_rayRanges));
-        RGL_CHECK(rgl_graph_node_add_child(m_nodes.m_rayRanges, m_nodes.m_lidarTransform));
+        RGL_CHECK(rgl_graph_node_add_child(m_nodes.m_rayRanges, m_nodes.m_rayRingIds));
+        RGL_CHECK(rgl_graph_node_add_child(m_nodes.m_rayRingIds, m_nodes.m_lidarTransform));
 
         InitializeConditionalConnections();
     }
@@ -80,6 +82,11 @@ namespace RGL
         RGL_CHECK(rgl_node_rays_set_range(&m_nodes.m_rayRanges, &range, 1));
     }
 
+    void PipelineGraph::ConfigureRayRingIds(const AZStd::vector<AZ::s32>& rayRingIds)
+    {
+        RGL_CHECK(rgl_node_rays_set_ring_ids(&m_nodes.m_rayRingIds, rayRingIds.data(), rayRingIds.size()));
+    }
+
     void PipelineGraph::ConfigureFieldNodes(const rgl_field_t* fields, size_t size)
     {
         RGL_CHECK(rgl_node_points_yield(&m_nodes.m_pointsYield, fields, aznumeric_cast<int32_t>(size)));
@@ -145,6 +152,9 @@ namespace RGL
             case RGL_FIELD_IS_HIT_I32:
                 success = success && GetResult(results.m_isHit, RGL_FIELD_IS_HIT_I32);
                 break;
+            case RGL_FIELD_RING_ID_U16:
+                success = success && GetResult(results.m_ringId, RGL_FIELD_RING_ID_U16);
+                break;
             default:
                 success = false;
                 AZ_Assert(false, AZStd::string::format("Invalid result field type with RGL id %i!", field).c_str());

Code/Source/Lidar/PipelineGraph.h

@@ -39,13 +39,14 @@ namespace RGL
             AZStd::vector<float> m_intensity;
             AZStd::vector<int32_t> m_packedRglEntityId;
             AZStd::vector<int32_t> m_isHit;
+            AZStd::vector<uint16_t> m_ringId;
         };
 
         struct Nodes
         {
-            rgl_node_t m_rayPoses{ nullptr }, m_rayRanges{ nullptr }, m_lidarTransform{ nullptr }, m_angularNoise{ nullptr },
-                m_rayTrace{ nullptr }, m_distanceNoise{ nullptr }, m_rayTraceYield{ nullptr }, m_pointsCompact{ nullptr },
-                m_pointsYield{ nullptr };
+            rgl_node_t m_rayPoses{ nullptr }, m_rayRanges{ nullptr }, m_rayRingIds{ nullptr }, m_lidarTransform{ nullptr },
+                m_angularNoise{ nullptr }, m_rayTrace{ nullptr }, m_distanceNoise{ nullptr }, m_rayTraceYield{ nullptr },
+                m_pointsCompact{ nullptr }, m_pointsYield{ nullptr };
         };
 
         PipelineGraph();
@@ -58,6 +59,7 @@ namespace RGL
 
         void ConfigureRayPosesNode(const AZStd::vector<rgl_mat3x4f>& rayPoses);
         void ConfigureRayRangesNode(float min, float max);
+        void ConfigureRayRingIds(const AZStd::vector<AZ::s32>& rayRingIds);
         void ConfigureFieldNodes(const rgl_field_t* fields, size_t size);
         void ConfigureLidarTransformNode(const AZ::Matrix3x4& lidarTransform);
         void ConfigureAngularNoiseNode(float angularNoiseStdDev);

static/PipelineGraph.mmd

@@ -1,6 +1,7 @@
 flowchart TD
     RP[Ray Poses] --> RR[Ray Ranges]
-    RR --> LT[Lidar Transform]
+    RR --> RRI[Ray Ring Ids]
+    RRI --> LT[Lidar Transform]
     LT -->|Noise enabled| AN[Angular Noise]
     LT -->|Noise disabled| RT[Ray Trace]
     AN --> RT