From d09b8d21a92d2b070b83b33590b43e930bf959f9 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/FindRGL.cmake                   |  4 +---
 Code/Source/Lidar/LidarRaycaster.cpp | 22 ++++++++++++++++++++++
 Code/Source/Lidar/LidarSystem.cpp    |  2 +-
 Code/Source/Lidar/PipelineGraph.cpp  |  3 +++
 Code/Source/Lidar/PipelineGraph.h    |  1 +
 5 files changed, 28 insertions(+), 4 deletions(-)

diff --git a/Code/FindRGL.cmake b/Code/FindRGL.cmake
index 81d7164..c60dca4 100644
--- a/Code/FindRGL.cmake
+++ b/Code/FindRGL.cmake
@@ -11,12 +11,11 @@
 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 # See the License for the specific language governing permissions and
 # limitations under the License.
-set(RGL_VERSION 0.19.0)
+set(RGL_VERSION 0.20.0)
 set(RGL_TAG v${RGL_VERSION})
 
 # Metadata files used to determine if RGL download is required
 set(RGL_VERSION_METADATA_FILE ${CMAKE_CURRENT_BINARY_DIR}/RGL_VERSION)
-set(ROS_DISTRO_METADATA_FILE ${CMAKE_CURRENT_BINARY_DIR}/ROS_DISTRO)
 
 set(ROS_DISTRO $ENV{ROS_DISTRO})
 set(RGL_LINUX_ZIP_FILENAME_BASE RGL-core-linux-x64)
@@ -42,7 +41,6 @@ if (NOT EXISTS ${RGL_DOWNLOAD_IN_PROGRESS_FILE})
 
     # Read metadata
     set(RGL_VERSION_METADATA " ")
-    set(ROS_DISTRO_METADATA " ")
     if (EXISTS ${RGL_VERSION_METADATA_FILE})
         file(READ ${RGL_VERSION_METADATA_FILE} RGL_VERSION_METADATA)
     endif ()
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/LidarSystem.cpp b/Code/Source/Lidar/LidarSystem.cpp
index 5572c60..72376dc 100644
--- a/Code/Source/Lidar/LidarSystem.cpp
+++ b/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::RingIds);
+            Features::EntityExclusion | Features::Noise | Features::Intensity | Features::Segmentation | Features::RingIds | Features::Reflectivity);
 
         ROS2::LidarSystemRequestBus::Handler::BusConnect(AZ_CRC(name));
 
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