Review changes

Signed-off-by: Aleksander Kamiński <aleksander.kaminski@robotec.ai>
RobotecAI · Sep 5, 2024 · 580d01c · 580d01c
1 parent f2b72d9
commit 580d01c
Show file tree

Hide file tree

Showing 3 changed files with 31 additions and 13 deletions.
diff --git a/Code/Source/Entity/EntityManager.cpp b/Code/Source/Entity/EntityManager.cpp
@@ -130,15 +130,17 @@ namespace RGL
             }
         }
 
+        static constexpr uint8_t DefaultClassID = 0U;
         if (!classId.has_value())
         {
             AZ_Warning(
                 "EntityManager",
                 false,
-                "Entity with ID: %s has no class tag. Assigning default class ID: 0",
-                m_entityId.ToString().c_str());
+                "Entity with ID: %s has no class tag. Assigning default class ID: %u",
+                m_entityId.ToString().c_str(),
+                DefaultClassID);
         }
 
-        return Utils::PackRglEntityId(ROS2::SegmentationIds{ m_segmentationEntityId, classId.value_or(0U) });
+        return Utils::PackRglEntityId(ROS2::SegmentationIds{ m_segmentationEntityId, classId.value_or(DefaultClassID) });
     }
 } // namespace RGL
diff --git a/Code/Source/Lidar/LidarRaycaster.cpp b/Code/Source/Lidar/LidarRaycaster.cpp
@@ -234,6 +234,8 @@ namespace RGL
     AZStd::optional<size_t> LidarRaycaster::GetRglResultsSize(
         const PipelineGraph::RaycastResults& rglResults, const ROS2::RaycastResults& results)
     {
+        // Size consistent among all (present) fields.
+        // If no consensus is reached or no fields are present, set to AZStd::nullopt.
         AZStd::optional<size_t> resultsSize;
         if (results.IsFieldPresent<ROS2::RaycastResultFlags::Point>())
         {
@@ -242,27 +244,35 @@ namespace RGL
 
         if (results.IsFieldPresent<ROS2::RaycastResultFlags::Range>())
         {
-            if (resultsSize.has_value() && resultsSize != rglResults.m_distance.size())
+            if (!resultsSize.has_value())
+            {
+                resultsSize = rglResults.m_distance.size();
+            }
+            else if (resultsSize != rglResults.m_distance.size())
             {
                 return AZStd::nullopt;
             }
-
-            resultsSize = rglResults.m_distance.size();
         }
 
         if (results.IsFieldPresent<ROS2::RaycastResultFlags::Intensity>())
         {
-            if (resultsSize.has_value() && resultsSize != rglResults.m_intensity.size())
+            if (!resultsSize.has_value())
+            {
+                resultsSize = rglResults.m_intensity.size();
+            }
+            else if (resultsSize != rglResults.m_intensity.size())
             {
                 return AZStd::nullopt;
             }
-
-            resultsSize = rglResults.m_intensity.size();
         }
 
         if (results.IsFieldPresent<ROS2::RaycastResultFlags::SegmentationData>())
         {
-            if (resultsSize.has_value() && resultsSize != rglResults.m_packedRglEntityId.size())
+            if (!resultsSize.has_value())
+            {
+                resultsSize = rglResults.m_packedRglEntityId.size();
+            }
+            else if (resultsSize != rglResults.m_packedRglEntityId.size())
             {
                 return AZStd::nullopt;
             }

diff --git a/Code/Source/Utilities/RGLUtils.h b/Code/Source/Utilities/RGLUtils.h
@@ -8,16 +8,22 @@
 #pragma once
 
 #include <AzCore/Math/Matrix3x4.h>
-#include <AzCore/std/containers/set.h>
-#include <AzCore/std/smart_ptr/shared_ptr.h>
-#include <AzCore/std/string/string.h>
 #include <ROS2/Lidar/RaycastResults.h>
 #include <rgl/api/core.h>
 
 namespace RGL::Utils
 {
+    //! Packs an entity ID and a segmentation class ID into an 32-bit integer.
+    //! Entity IDs must be generated using the GenerateSegmentationEntityId function.
+    //! @see GenerateSegmentationEntityId
     int32_t PackRglEntityId(ROS2::SegmentationIds);
+
+    //! Unpacks a packed RGL entity ID into an entity ID and a segmentation class ID.
+    //! @see PackRglEntityId
     ROS2::SegmentationIds UnpackRglEntityId(int32_t rglPackedEntityId);
+
+    //! Generates a new unique ID for an entity.
+    //! This ID can then be used to generate a packed RGL entity ID.
     int32_t GenerateSegmentationEntityId();
 
     //! If the provided status signifies an error, prints the last RGL error message.