[ROS2] Raycast Results Refactor

Gems/ROS2/Code/Include/ROS2/Lidar/LidarRaycasterBus.h

@@ -11,7 +11,9 @@
 #include <AzCore/EBus/EBus.h>
 #include <AzCore/Math/Transform.h>
 #include <AzCore/Math/Vector3.h>
+#include <AzCore/Outcome/Outcome.h>
 #include <ROS2/Communication/QoS.h>
+#include <ROS2/Lidar/RaycastResults.h>
 
 namespace ROS2
 {
@@ -84,23 +86,6 @@ namespace ROS2
     //! Unique id used by lidar raycasters.
     using LidarId = StronglyTypedUuid<struct LidarIdTag>;
 
-    enum class RaycastResultFlags : AZ::u8
-    {
-        Points = (1 << 0), //!< return 3D point coordinates
-        Ranges = (1 << 1), //!< return array of distances
-        Intensities = (1 << 2), //!< return intensity data
-    };
-
-    //! Bitwise operators for RaycastResultFlags
-    AZ_DEFINE_ENUM_BITWISE_OPERATORS(RaycastResultFlags)
-
-    struct RaycastResult
-    {
-        AZStd::vector<AZ::Vector3> m_points;
-        AZStd::vector<float> m_ranges;
-        AZStd::vector<float> m_intensities;
-    };
-
     //! Structure used to describe both minimal and maximal
     //! ray travel distance in meters.
     struct RayRange
@@ -134,8 +119,10 @@ namespace ROS2
         //! Schedules a raycast that originates from the point described by the lidarTransform.
         //! @param lidarTransform Current transform from global to lidar reference frame.
         //! @param flags Used to request different kinds of data returned by raycast query
-        //! @return Results of the raycast in the requested form including 3D space coordinates and/or ranges.
-        virtual RaycastResult PerformRaycast(const AZ::Transform& lidarTransform) = 0;
+        //! @return Results of the raycast in the requested form if the raycast was successfull or an error message if it was not.
+        //! The returned error messages are c-style string literals which are statically allocated and therefore do not need to be
+        //! dealocated.
+        virtual AZ::Outcome<RaycastResults, const char*> PerformRaycast(const AZ::Transform& lidarTransform) = 0;
 
         //! Configures ray Gaussian Noise parameters.
         //! Each call overrides the previous configuration.

Gems/ROS2/Code/Include/ROS2/Lidar/RaycastResults.h

@@ -0,0 +1,216 @@
+/*
+ * Copyright (c) Contributors to the Open 3D Engine Project.
+ * For complete copyright and license terms please see the LICENSE at the root of this distribution.
+ *
+ * SPDX-License-Identifier: Apache-2.0 OR MIT
+ *
+ */
+#pragma once
+
+#include <AzCore/std/containers/span.h>
+
+namespace ROS2
+{
+    enum class RaycastResultFlags : AZ::u8
+    {
+        Point = (1 << 0), //!< return 3D point coordinates
+        Range = (1 << 1), //!< return array of distances
+        Intensity = (1 << 2), //!< return intensity data
+    };
+
+    //! Bitwise operators for RaycastResultFlags
+    AZ_DEFINE_ENUM_BITWISE_OPERATORS(RaycastResultFlags)
+
+    template<RaycastResultFlags F>
+    bool IsFlagEnabled(RaycastResultFlags flags)
+    {
+        return (flags & F) == F;
+    }
+
+    template<RaycastResultFlags F>
+    struct ResultTraits;
+
+    template<>
+    struct ResultTraits<RaycastResultFlags::Point>
+    {
+        using Type = AZ::Vector3;
+    };
+
+    template<>
+    struct ResultTraits<RaycastResultFlags::Range>
+    {
+        using Type = float;
+    };
+
+    template<>
+    struct ResultTraits<RaycastResultFlags::Intensity>
+    {
+        using Type = float;
+    };
+
+    //! Class used for storing the results of a raycast.
+    //! It guarantees a uniform length of all its fields.
+    class RaycastResults
+    {
+    public:
+        template<RaycastResultFlags F, typename RT = typename ResultTraits<F>::Type>
+        using FieldSpan = AZStd::span<RT>;
+
+        template<RaycastResultFlags F>
+        using ConstFieldSpan = FieldSpan<F, const typename ResultTraits<F>::Type>;
+
+        explicit RaycastResults(RaycastResultFlags flags, size_t count = 0U);
+        RaycastResults(const RaycastResults& other) = default;
+        RaycastResults(RaycastResults&& other);
+
+        [[nodiscard]] bool IsEmpty() const;
+        template<RaycastResultFlags F>
+        [[nodiscard]] bool IsFlagSatisfied(RaycastResultFlags requestedFlags) const;
+        [[nodiscard]] bool SatisfiesResultFlags(RaycastResultFlags flags) const;
+
+        template<RaycastResultFlags F>
+        [[nodiscard]] bool IsFieldPresent() const;
+
+        [[nodiscard]] size_t GetCount() const;
+
+        template<RaycastResultFlags F>
+        AZStd::optional<ConstFieldSpan<F>> GetConstFieldSpan() const;
+
+        template<RaycastResultFlags F>
+        AZStd::optional<FieldSpan<F>> GetFieldSpan();
+
+        void Clear();
+        void Resize(size_t count);
+
+        RaycastResults& operator=(const RaycastResults& other) = default;
+        RaycastResults& operator=(RaycastResults&& other);
+
+    private:
+        template<RaycastResultFlags F, typename RT = typename ResultTraits<F>::Type>
+        using FieldInternal = AZStd::optional<AZStd::vector<RT>>;
+
+        template<RaycastResultFlags F>
+        const FieldInternal<F>& GetField() const;
+
+        template<RaycastResultFlags F>
+        FieldInternal<F>& GetField();
+
+        template<RaycastResultFlags F>
+        void ResizeFieldIfPresent(size_t count);
+
+        template<RaycastResultFlags F>
+        void EnsureFlagSatisfied(RaycastResultFlags flags, size_t count);
+
+        template<RaycastResultFlags F>
+        void ClearFieldIfPresent();
+
+        size_t m_count{};
+        FieldInternal<RaycastResultFlags::Point> m_points;
+        FieldInternal<RaycastResultFlags::Range> m_ranges;
+        FieldInternal<RaycastResultFlags::Intensity> m_intensities;
+    };
+
+    template<RaycastResultFlags F>
+    bool RaycastResults::IsFlagSatisfied(RaycastResultFlags requestedFlags) const
+    {
+        return !IsFlagEnabled<F>(requestedFlags) || IsFieldPresent<F>();
+    }
+
+    template<RaycastResultFlags F>
+    bool RaycastResults::IsFieldPresent() const
+    {
+        return GetField<F>().has_value();
+    }
+
+    template<RaycastResultFlags F>
+    AZStd::optional<RaycastResults::ConstFieldSpan<F>> RaycastResults::GetConstFieldSpan() const
+    {
+        auto& field = GetField<F>();
+        if (!field.has_value())
+        {
+            return {};
+        }
+
+        return AZStd::span(field->begin(), field->size());
+    }
+
+    template<RaycastResultFlags F>
+    AZStd::optional<RaycastResults::FieldSpan<F>> RaycastResults::GetFieldSpan()
+    {
+        auto& field = GetField<F>();
+        if (!field.has_value())
+        {
+            return {};
+        }
+
+        return AZStd::span(field->begin(), field->size());
+    }
+
+    template<>
+    inline const RaycastResults::FieldInternal<RaycastResultFlags::Point>& RaycastResults::GetField<RaycastResultFlags::Point>() const
+    {
+        return m_points;
+    }
+
+    template<>
+    inline const RaycastResults::FieldInternal<RaycastResultFlags::Range>& RaycastResults::GetField<RaycastResultFlags::Range>() const
+    {
+        return m_ranges;
+    }
+
+    template<>
+    inline const RaycastResults::FieldInternal<RaycastResultFlags::Intensity>& RaycastResults::GetField<RaycastResultFlags::Intensity>()
+        const
+    {
+        return m_intensities;
+    }
+
+    template<RaycastResultFlags F>
+    RaycastResults::FieldInternal<F>& RaycastResults::GetField()
+    {
+        return const_cast<FieldInternal<F>&>(static_cast<const RaycastResults*>(this)->GetField<F>());
+    }
+
+    template<RaycastResultFlags F>
+    void RaycastResults::ClearFieldIfPresent()
+    {
+        auto& field = GetField<F>();
+        if (!field.has_value())
+        {
+            return;
+        }
+
+        field->clear();
+    }
+
+    template<RaycastResultFlags F>
+    void RaycastResults::ResizeFieldIfPresent(size_t count)
+    {
+        auto& field = GetField<F>();
+        if (!field.has_value())
+        {
+            return;
+        }
+
+        field->resize(count);
+    }
+
+    template<RaycastResultFlags F>
+    void RaycastResults::EnsureFlagSatisfied(RaycastResultFlags flags, size_t count)
+    {
+        if (!IsFlagEnabled<F>(flags))
+        {
+            return;
+        }
+
+        auto& field = GetField<F>();
+        if (!field.has_value())
+        {
+            field = AZStd::vector<typename ResultTraits<F>::Type>(count);
+        }
+        else
+        {
+            field->resize(count);
+        }
+    }
+} // namespace ROS2

Gems/ROS2/Code/Source/Lidar/LidarCore.cpp

@@ -72,13 +72,10 @@ namespace ROS2
                 m_lidarConfiguration.m_lidarParameters.m_noiseParameters.m_distanceNoiseStdDevRisePerMeter);
         }
 
-        RaycastResultFlags requestedFlags = RaycastResultFlags::Ranges | RaycastResultFlags::Points;
-        if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::Intensity)
-        {
-            requestedFlags |= RaycastResultFlags::Intensities;
-        }
-
-        LidarRaycasterRequestBus::Event(m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRaycastResultFlags, requestedFlags);
+        LidarRaycasterRequestBus::Event(
+            m_lidarRaycasterId,
+            &LidarRaycasterRequestBus::Events::ConfigureRaycastResultFlags,
+            GetRaycastResultFlagsForConfig(m_lidarConfiguration));
 
         if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::CollisionLayers)
         {
@@ -103,6 +100,23 @@ namespace ROS2
         }
     }
 
+    void LidarCore::UpdatePoints(const RaycastResults& results)
+    {
+        const auto pointsField = results.GetConstFieldSpan<RaycastResultFlags::Point>().value();
+        m_lastPoints.assign(pointsField.begin(), pointsField.end());
+    }
+
+    RaycastResultFlags LidarCore::GetRaycastResultFlagsForConfig(const LidarSensorConfiguration& configuration)
+    {
+        RaycastResultFlags flags = RaycastResultFlags::Range | RaycastResultFlags::Point;
+        if (configuration.m_lidarSystemFeatures & LidarSystemFeatures::Intensity)
+        {
+            flags |= RaycastResultFlags::Intensity;
+        }
+
+        return flags;
+    }
+
     LidarCore::LidarCore(const AZStd::vector<LidarTemplate::LidarModel>& availableModels)
         : m_lidarConfiguration(availableModels)
     {
@@ -115,7 +129,7 @@ namespace ROS2
 
     void LidarCore::VisualizeResults() const
     {
-        if (m_lastScanResults.m_points.empty())
+        if (m_lastPoints.empty())
         {
             return;
         }
@@ -124,8 +138,8 @@ namespace ROS2
         {
             const uint8_t pixelSize = 2;
             AZ::RPI::AuxGeomDraw::AuxGeomDynamicDrawArguments drawArgs;
-            drawArgs.m_verts = m_lastScanResults.m_points.data();
-            drawArgs.m_vertCount = m_lastScanResults.m_points.size();
+            drawArgs.m_verts = m_lastPoints.data();
+            drawArgs.m_vertCount = m_lastPoints.size();
             drawArgs.m_colors = &AZ::Colors::Red;
             drawArgs.m_colorCount = 1;
             drawArgs.m_opacityType = AZ::RPI::AuxGeomDraw::OpacityType::Opaque;
@@ -163,21 +177,28 @@ namespace ROS2
         return m_lidarRaycasterId;
     }
 
-    const RaycastResult& LidarCore::PerformRaycast()
+    AZStd::optional<RaycastResults> LidarCore::PerformRaycast()
     {
-        static const RaycastResult EmptyResults{};
-
         AZ::Entity* entity = nullptr;
         AZ::ComponentApplicationBus::BroadcastResult(entity, &AZ::ComponentApplicationRequests::FindEntity, m_entityId);
         const auto entityTransform = entity->FindComponent<AzFramework::TransformComponent>();
 
+        AZ::Outcome<RaycastResults, const char*> results = AZ::Failure("EBus failure occured.");
         LidarRaycasterRequestBus::EventResult(
-            m_lastScanResults, m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::PerformRaycast, entityTransform->GetWorldTM());
-        if (m_lastScanResults.m_points.empty())
+            results, m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::PerformRaycast, entityTransform->GetWorldTM());
+        if (!results.IsSuccess())
+        {
+            AZ_Error(__func__, false, "Unable to obtain raycast results. %s", results.GetError());
+            return {};
+        }
+
+        if (results.GetValue().IsEmpty())
         {
             AZ_TracePrintf("Lidar Sensor Component", "No results from raycast\n");
-            return EmptyResults;
         }
-        return m_lastScanResults;
+
+        UpdatePoints(results.GetValue());
+
+        return results.GetValue();
     }
 } // namespace ROS2

Gems/ROS2/Code/Source/Lidar/LidarCore.h

@@ -37,7 +37,7 @@ namespace ROS2
 
         //! Perform a raycast.
         //! @return Results of the raycast.
-        const RaycastResult& PerformRaycast();
+        AZStd::optional<RaycastResults> PerformRaycast();
         //! Visualize the results of the last performed raycast.
         void VisualizeResults() const;
 
@@ -49,17 +49,21 @@ namespace ROS2
         LidarSensorConfiguration m_lidarConfiguration;
 
     private:
+        static RaycastResultFlags GetRaycastResultFlagsForConfig(const LidarSensorConfiguration& configuration);
+
         void ConnectToLidarRaycaster();
         void ConfigureLidarRaycaster();
 
+        void UpdatePoints(const RaycastResults& results);
+
         //! An unordered map of lidar implementations to their raycasters created by this LidarSensorComponent.
         AZStd::unordered_map<AZStd::string, LidarId> m_implementationToRaycasterMap;
         LidarId m_lidarRaycasterId;
 
         AZ::RPI::AuxGeomDrawPtr m_drawQueue;
 
         AZStd::vector<AZ::Vector3> m_lastRotations;
-        RaycastResult m_lastScanResults;
+        AZStd::vector<AZ::Vector3> m_lastPoints;
 
         AZ::EntityId m_entityId;
     };