From 87aaf2dfde6412cf07b4455e228b4643a38a62d6 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Aleksander=20Kami=C5=84ski?=
 <aleksander.kaminski@robotec.ai>
Date: Thu, 12 Dec 2024 15:16:00 +0100
Subject: [PATCH] Add reflectivity support
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: Aleksander Kamiński <aleksander.kaminski@robotec.ai>
---
 Code/Source/Lidar/LidarRaycaster.cpp | 22 ++++++++++++++++++++++
 Code/Source/Lidar/PipelineGraph.cpp  |  3 +++
 Code/Source/Lidar/PipelineGraph.h    |  1 +
 3 files changed, 26 insertions(+)

diff --git a/Code/Source/Lidar/LidarRaycaster.cpp b/Code/Source/Lidar/LidarRaycaster.cpp
index 284d4ef..10a326b 100644
--- a/Code/Source/Lidar/LidarRaycaster.cpp
+++ b/Code/Source/Lidar/LidarRaycaster.cpp
@@ -118,6 +118,11 @@ namespace RGL
             m_rglRaycastResults.m_fields.push_back(RGL_FIELD_RING_ID_U16);
         }
 
+        if (ROS2::IsFlagEnabled(ROS2::RaycastResultFlags::Reflectivity, flags))
+        {
+            m_rglRaycastResults.m_fields.push_back(RGL_FIELD_REFLECTIVITY_F32);
+        }
+
         m_graph.ConfigureFieldNodes(m_rglRaycastResults.m_fields.data(), m_rglRaycastResults.m_fields.size());
         m_graph.SetIsCompactEnabled(!m_returnNonHits);
     }
@@ -190,6 +195,11 @@ namespace RGL
             AZStd::copy(m_rglRaycastResults.m_ringId.begin(), m_rglRaycastResults.m_ringId.end(), ring.value().begin());
         }
 
+        if (auto reflectivity = raycastResults.GetFieldSpan<ROS2::RaycastResultFlags::Reflectivity>(); reflectivity.has_value())
+        {
+            AZStd::copy(m_rglRaycastResults.m_reflectivity.begin(), m_rglRaycastResults.m_reflectivity.end(), reflectivity.value().begin());
+        }
+
         return AZ::Success(m_raycastResults.value());
     }
 
@@ -308,6 +318,18 @@ namespace RGL
             }
         }
 
+        if (results.IsFieldPresent<ROS2::RaycastResultFlags::Reflectivity>())
+        {
+            if (!resultsSize.has_value())
+            {
+                resultsSize = rglResults.m_reflectivity.size();
+            }
+            else if (resultsSize != rglResults.m_reflectivity.size())
+            {
+                return AZStd::nullopt;
+            }
+        }
+
         return resultsSize;
     }
 } // namespace RGL
diff --git a/Code/Source/Lidar/PipelineGraph.cpp b/Code/Source/Lidar/PipelineGraph.cpp
index cd18205..5f9cf17 100644
--- a/Code/Source/Lidar/PipelineGraph.cpp
+++ b/Code/Source/Lidar/PipelineGraph.cpp
@@ -155,6 +155,9 @@ namespace RGL
             case RGL_FIELD_RING_ID_U16:
                 success = success && GetResult(results.m_ringId, RGL_FIELD_RING_ID_U16);
                 break;
+            case RGL_FIELD_REFLECTIVITY_F32:
+                success = success && GetResult(results.m_reflectivity, RGL_FIELD_REFLECTIVITY_F32);
+                break;
             default:
                 success = false;
                 AZ_Assert(false, AZStd::string::format("Invalid result field type with RGL id %i!", field).c_str());
diff --git a/Code/Source/Lidar/PipelineGraph.h b/Code/Source/Lidar/PipelineGraph.h
index d1c84f0..b846f6c 100644
--- a/Code/Source/Lidar/PipelineGraph.h
+++ b/Code/Source/Lidar/PipelineGraph.h
@@ -40,6 +40,7 @@ namespace RGL
             AZStd::vector<int32_t> m_packedRglEntityId;
             AZStd::vector<int32_t> m_isHit;
             AZStd::vector<uint16_t> m_ringId;
+            AZStd::vector<float> m_reflectivity;
         };
 
         struct Nodes