Add reflectivity support

Signed-off-by: Aleksander Kamiński <[email protected]>
RobotecAI · Dec 12, 2024 · 87aaf2d · 87aaf2d
1 parent 122b41e
commit 87aaf2d
Show file tree

Hide file tree

Showing 3 changed files with 26 additions and 0 deletions.
diff --git 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
@@ -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
@@ -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