Add pointcloud ordering

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

Author: alek-kam-robotec-ai

Code/Source/Lidar/LidarRaycaster.cpp

@@ -29,7 +29,7 @@ namespace RGL
 
     LidarRaycaster::LidarRaycaster(LidarRaycaster&& other)
         : m_uuid{ other.m_uuid }
-        , m_isMaxRangeEnabled{ other.m_isMaxRangeEnabled }
+        , m_returnNonHits{ other.m_returnNonHits }
         , m_range{ other.m_range }
         , m_graph{ std::move(other.m_graph) }
         , m_rayTransforms{ AZStd::move(other.m_rayTransforms) }
@@ -169,6 +169,18 @@ namespace RGL
                 Utils::UnpackRglEntityId);
         }
 
+        if (auto isHit = raycastResults.GetFieldSpan<ROS2::RaycastResultFlags::IsHit>(); isHit.has_value())
+        {
+            AZStd::transform(
+                m_rglRaycastResults.m_isHit.begin(),
+                m_rglRaycastResults.m_isHit.end(),
+                isHit.value().begin(),
+                [](int32_t isHit)
+                {
+                    return isHit != 0;
+                });
+        }
+
         return AZ::Success(m_raycastResults.value());
     }
 
@@ -193,7 +205,7 @@ namespace RGL
         float minRangeNonHitValue = -AZStd::numeric_limits<float>::infinity();
         float maxRangeNonHitValue = AZStd::numeric_limits<float>::infinity();
 
-        if (m_isMaxRangeEnabled && m_range.has_value())
+        if (m_returnNonHits && m_range.has_value())
         {
             minRangeNonHitValue = m_range.value().m_min;
             maxRangeNonHitValue = m_range.value().m_max;
@@ -202,10 +214,9 @@ namespace RGL
         m_graph.ConfigureRaytraceNodeNonHits(minRangeNonHitValue, maxRangeNonHitValue);
     }
 
-    void LidarRaycaster::ConfigureMaxRangePointAddition(bool addMaxRangePoints)
+    void LidarRaycaster::ConfigureNonHitReturn(bool returnNonHits)
     {
-        m_isMaxRangeEnabled = addMaxRangePoints;
-
+        m_returnNonHits = returnNonHits;
         UpdateNonHitValues();
 
         // We need to configure if points should be compacted to minimize the CPU operations when retrieving raycast results.
@@ -259,11 +270,23 @@ namespace RGL
             }
         }
 
+        if (results.IsFieldPresent<ROS2::RaycastResultFlags::IsHit>())
+        {
+            if (!resultsSize.has_value())
+            {
+                resultsSize = rglResults.m_isHit.size();
+            }
+            else if (resultsSize != rglResults.m_isHit.size())
+            {
+                return AZStd::nullopt;
+            }
+        }
+
         return resultsSize;
     }
 
     bool LidarRaycaster::ShouldEnableCompact() const
     {
-        return !m_raycastResults->IsFieldPresent<ROS2::RaycastResultFlags::Range>() && !m_isMaxRangeEnabled;
+        return !m_raycastResults->IsFieldPresent<ROS2::RaycastResultFlags::Range>() && !m_returnNonHits;
     }
 } // namespace RGL

Code/Source/Lidar/LidarRaycaster.h

@@ -49,12 +49,12 @@ namespace RGL
             [[maybe_unused]] float distanceNoiseStdDevRisePerMeter) override;
         void ExcludeEntities(const AZStd::vector<AZ::EntityId>& excludedEntities) override;
         void UpdateNonHitValues();
-        void ConfigureMaxRangePointAddition(bool addMaxRangePoints) override;
+        void ConfigureNonHitReturn(bool returnNonHits) override;
 
     private:
         AZ::Uuid m_uuid;
 
-        bool m_isMaxRangeEnabled{ false }; //!< Determines whether max range point addition is enabled.
+        bool m_returnNonHits{ false }; //!< Determines whether max range point addition is enabled.
 
         AZStd::optional<ROS2::RayRange> m_range{};
         AZStd::vector<AZ::Matrix3x4> m_rayTransforms{ AZ::Matrix3x4::CreateIdentity() };

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::MaxRangePoints | Features::Noise | Features::Intensity | Features::Segmentation);
+            Features::EntityExclusion | Features::Noise | Features::Intensity | Features::Segmentation);
 
         ROS2::LidarSystemRequestBus::Handler::BusConnect(AZ_CRC(name));
 

Code/Source/Lidar/PipelineGraph.cpp

@@ -142,8 +142,9 @@ namespace RGL
             case RGL_FIELD_ENTITY_ID_I32:
                 success = success && GetResult(results.m_packedRglEntityId, RGL_FIELD_ENTITY_ID_I32);
                 break;
-            case RGL_FIELD_IS_HIT_I32: // Not needed.
-                continue;
+            case RGL_FIELD_IS_HIT_I32:
+                success = success && GetResult(results.m_isHit, RGL_FIELD_IS_HIT_I32);
+                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

@@ -38,6 +38,7 @@ namespace RGL
             AZStd::vector<float> m_distance;
             AZStd::vector<float> m_intensity;
             AZStd::vector<int32_t> m_packedRglEntityId;
+            AZStd::vector<int32_t> m_isHit;
         };
 
         struct Nodes