[ROS2] Raycast Results Refactor (#751)

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

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,206 @@
+/*
+ * 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)
+
+    inline bool IsFlagEnabled(RaycastResultFlags flag, RaycastResultFlags flags)
+    {
+        return (flags & flag) == flag;
+    }
+
+    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 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::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,25 @@ 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 occurred.");
         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_TracePrintf("Lidar Sensor Component", "No results from raycast\n");
-            return EmptyResults;
+            AZ_Error(__func__, false, "Unable to obtain raycast results. %s", results.GetError());
+            return {};
         }
-        return m_lastScanResults;
+
+        AZ_Warning("Lidar Sensor Component", !results.GetValue().IsEmpty(), "No results from raycast\n");
+
+        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,9 +49,13 @@ 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;
@@ -59,7 +63,7 @@ namespace ROS2
         AZ::RPI::AuxGeomDrawPtr m_drawQueue;
 
         AZStd::vector<AZ::Vector3> m_lastRotations;
-        RaycastResult m_lastScanResults;
+        AZStd::vector<AZ::Vector3> m_lastPoints;
 
         AZ::EntityId m_entityId;
     };

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

@@ -112,7 +112,7 @@ namespace ROS2
         return requests;
     }
 
-    RaycastResult LidarRaycaster::PerformRaycast(const AZ::Transform& lidarTransform)
+    AZ::Outcome<RaycastResults, const char*> LidarRaycaster::PerformRaycast(const AZ::Transform& lidarTransform)
     {
         AZ_Assert(!m_rayRotations.empty(), "Ray poses are not configured. Unable to Perform a raycast.");
         AZ_Assert(m_range.has_value(), "Ray range is not configured. Unable to Perform a raycast.");
@@ -125,16 +125,19 @@ namespace ROS2
         const AZStd::vector<AZ::Vector3> rayDirections = LidarTemplateUtils::RotationsToDirections(m_rayRotations, lidarTransform);
         AzPhysics::SceneQueryRequests requests = prepareRequests(lidarTransform, rayDirections);
 
-        RaycastResult results;
-        const bool handlePoints = (m_resultFlags & RaycastResultFlags::Points) == RaycastResultFlags::Points;
-        const bool handleRanges = (m_resultFlags & RaycastResultFlags::Ranges) == RaycastResultFlags::Ranges;
+        const bool handlePoints = (m_resultFlags & RaycastResultFlags::Point) == RaycastResultFlags::Point;
+        const bool handleRanges = (m_resultFlags & RaycastResultFlags::Range) == RaycastResultFlags::Range;
+        RaycastResults results(m_resultFlags, rayDirections.size());
+
+        AZStd::optional<RaycastResults::FieldSpan<RaycastResultFlags::Point>::iterator> pointIt;
+        AZStd::optional<RaycastResults::FieldSpan<RaycastResultFlags::Range>::iterator> rangeIt;
         if (handlePoints)
         {
-            results.m_points.reserve(rayDirections.size());
+            pointIt = results.GetFieldSpan<RaycastResultFlags::Point>().value().begin();
         }
         if (handleRanges)
         {
-            results.m_ranges.reserve(rayDirections.size());
+            rangeIt = results.GetFieldSpan<RaycastResultFlags::Range>().value().begin();
         }
 
         auto* sceneInterface = AZ::Interface<AzPhysics::SceneInterface>::Get();
@@ -144,7 +147,8 @@ namespace ROS2
             AZ::Transform::CreateFromQuaternionAndTranslation(lidarTransform.GetRotation(), lidarTransform.GetTranslation()).GetInverse();
         const float maxRange = m_addMaxRangePoints ? m_range->m_max : AZStd::numeric_limits<float>::infinity();
 
-        for (int i = 0; i < requestResults.size(); i++)
+        size_t usedSize = 0U;
+        for (size_t i = 0U; i < requestResults.size(); i++)
         {
             const auto& requestResult = requestResults[i];
             float hitRange = requestResult ? requestResult.m_hits[0].m_distance : maxRange;
@@ -152,25 +156,40 @@ namespace ROS2
             {
                 hitRange = -AZStd::numeric_limits<float>::infinity();
             }
+
+            bool assigned = false;
             if (handleRanges)
             {
-                results.m_ranges.push_back(hitRange);
+                *rangeIt.value() = hitRange;
+                ++rangeIt.value();
+                assigned = true;
             }
+
             if (handlePoints)
             {
                 if (hitRange == maxRange)
                 {
                     // to properly visualize max points they need to be transformed to local coordinate system before applying maxRange
                     const AZ::Vector3 maxPoint = lidarTransform.TransformPoint(localTransform.TransformVector(rayDirections[i]) * hitRange);
-                    results.m_points.push_back(maxPoint);
+                    *pointIt.value() = maxPoint;
+                    ++pointIt.value();
+                    assigned = true;
                 }
                 else if (!AZStd::isinf(hitRange))
                 {
                     // otherwise they are already calculated by PhysX
-                    results.m_points.push_back(requestResult.m_hits[0].m_position);
+                    *pointIt.value() = requestResult.m_hits[0].m_position;
+                    ++pointIt.value();
+                    assigned = true;
                 }
             }
+
+            if (assigned)
+            {
+                ++usedSize;
+            }
         }
+        results.Resize(usedSize);
 
         return results;
     }

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

@@ -31,7 +31,7 @@ namespace ROS2
         void ConfigureRayRange(RayRange range) override;
         void ConfigureRaycastResultFlags(RaycastResultFlags flags) override;
 
-        RaycastResult PerformRaycast(const AZ::Transform& lidarTransform) override;
+        AZ::Outcome<RaycastResults, const char*> PerformRaycast(const AZ::Transform& lidarTransform) override;
 
         void ConfigureIgnoredCollisionLayers(const AZStd::unordered_set<AZ::u32>& layerIndices) override;
         void ConfigureMaxRangePointAddition(bool addMaxRangePoints) override;
@@ -44,7 +44,7 @@ namespace ROS2
         AZ::EntityId m_sceneEntityId;
         AzPhysics::SceneHandle m_sceneHandle{ AzPhysics::InvalidSceneHandle };
 
-        RaycastResultFlags m_resultFlags{ RaycastResultFlags::Points };
+        RaycastResultFlags m_resultFlags{ RaycastResultFlags::Point };
         AZStd::optional<RayRange> m_range{};
         bool m_addMaxRangePoints{ false };
         AZStd::vector<AZ::Quaternion> m_rayRotations{ { AZ::Quaternion::CreateZero() } };

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

@@ -0,0 +1,46 @@
+/*
+ * 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
+ *
+ */
+#include <Lidar/PointCloudMessageBuilder.h>
+#include <sensor_msgs/point_cloud2_iterator.hpp>
+
+namespace ROS2
+{
+    PointCloud2MessageBuilder::PointCloud2MessageBuilder(
+        const AZStd::string& frameId, builtin_interfaces::msg::Time timeStamp, size_t count)
+    {
+        sensor_msgs::msg::PointCloud2 message{};
+        message.header.frame_id = frameId.data();
+        message.header.stamp = timeStamp;
+        message.height = 1;
+        message.width = count;
+
+        m_message = message;
+    }
+
+    PointCloud2MessageBuilder& PointCloud2MessageBuilder::AddField(const char* name, uint8_t dataType, size_t count)
+    {
+        sensor_msgs::msg::PointField point_field;
+        point_field.name = name;
+        point_field.count = count;
+        point_field.datatype = dataType;
+        point_field.offset = m_offset;
+        m_message.fields.push_back(point_field);
+
+        m_offset += point_field.count * sizeOfPointField(dataType);
+        return *this;
+    }
+
+    sensor_msgs::msg::PointCloud2 PointCloud2MessageBuilder::Get()
+    {
+        m_message.point_step = m_offset;
+        m_message.row_step = m_message.width * m_message.point_step;
+        m_message.data.resize(m_message.row_step);
+
+        return m_message;
+    }
+} // namespace ROS2

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

@@ -0,0 +1,26 @@
+/*
+ * 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 <sensor_msgs/msg/point_cloud2.hpp>
+#include <AzCore/std/optional.h>
+
+namespace ROS2
+{
+    class PointCloud2MessageBuilder
+    {
+    public:
+        PointCloud2MessageBuilder(const AZStd::string& frameId, builtin_interfaces::msg::Time timeStamp, size_t count);
+        PointCloud2MessageBuilder& AddField(const char* name, uint8_t dataType, size_t count = 1);
+        sensor_msgs::msg::PointCloud2 Get();
+
+    private:
+        size_t m_offset = 0U;
+        sensor_msgs::msg::PointCloud2 m_message;
+    };
+} // namespace ROS2

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

@@ -106,13 +106,20 @@ namespace ROS2
 
     void ROS2Lidar2DSensorComponent::FrequencyTick()
     {
-        const RaycastResult& lastScanResults = m_lidarCore.PerformRaycast();
+        AZStd::optional<RaycastResults> results = m_lidarCore.PerformRaycast();
 
-        if (!m_sensorConfiguration.m_publishingEnabled)
-        { // Skip publishing when it is disabled.
+        if (!results.has_value() || !m_sensorConfiguration.m_publishingEnabled)
+        {
             return;
         }
 
+        PublishRaycastResults(results.value());
+    }
+
+    void ROS2Lidar2DSensorComponent::PublishRaycastResults(const RaycastResults& results)
+    {
+        const bool isIntensityEnabled = m_lidarCore.m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::Intensity;
+
         auto* ros2Frame = GetEntity()->FindComponent<ROS2FrameComponent>();
         auto message = sensor_msgs::msg::LaserScan();
         message.header.frame_id = ros2Frame->GetFrameID().data();
@@ -127,7 +134,14 @@ namespace ROS2
         message.scan_time = 1.f / m_sensorConfiguration.m_frequency;
         message.time_increment = 0.0f;
 
-        message.ranges.assign(lastScanResults.m_ranges.begin(), lastScanResults.m_ranges.end());
+        const auto rangeField = results.GetConstFieldSpan<RaycastResultFlags::Range>().value();
+        message.ranges.assign(rangeField.begin(), rangeField.end());
+        if (isIntensityEnabled)
+        {
+            const auto intensityField = results.GetConstFieldSpan<RaycastResultFlags::Intensity>().value();
+            message.intensities.assign(intensityField.begin(), intensityField.end());
+        }
+
         m_laserScanPublisher->publish(message);
     }
 } // namespace ROS2

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

@@ -44,6 +44,8 @@ namespace ROS2
         //////////////////////////////////////////////////////////////////////////
         void FrequencyTick();
 
+        void PublishRaycastResults(const RaycastResults& results);
+
         std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::LaserScan>> m_laserScanPublisher;
 
         LidarCore m_lidarCore;

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

@@ -9,6 +9,7 @@
 #include <Atom/RPI.Public/AuxGeom/AuxGeomFeatureProcessorInterface.h>
 #include <Atom/RPI.Public/Scene.h>
 #include <Lidar/LidarRegistrarSystemComponent.h>
+#include <Lidar/PointCloudMessageBuilder.h>
 #include <Lidar/ROS2LidarSensorComponent.h>
 #include <ROS2/Frame/ROS2FrameComponent.h>
 #include <ROS2/Utilities/ROS2Names.h>
@@ -139,89 +140,70 @@ namespace ROS2
                 aznumeric_cast<AZ::u64>(timestamp.sec) * aznumeric_cast<AZ::u64>(1.0e9f) + timestamp.nanosec);
         }
 
-        const RaycastResult& lastScanResults = m_lidarCore.PerformRaycast();
+        AZStd::optional<RaycastResults> lastScanResults = m_lidarCore.PerformRaycast();
 
-        if (m_canRaycasterPublish || !m_sensorConfiguration.m_publishingEnabled)
-        { // Skip publishing when it is disabled or can be handled by the raycaster.
+        if (!lastScanResults.has_value() || m_canRaycasterPublish || !m_sensorConfiguration.m_publishingEnabled)
+        {
             return;
         }
 
-        PublishRaycastResults(lastScanResults);
+        PublishRaycastResults(lastScanResults.value());
     }
 
-    void ROS2LidarSensorComponent::PublishRaycastResults(const RaycastResult& results)
+    void ROS2LidarSensorComponent::PublishRaycastResults(const RaycastResults& results)
     {
         const bool isIntensityEnabled = m_lidarCore.m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::Intensity;
 
-        auto* ros2Frame = GetEntity()->FindComponent<ROS2FrameComponent>();
-        auto message = sensor_msgs::msg::PointCloud2();
-        message.header.frame_id = ros2Frame->GetFrameID().data();
-        message.header.stamp = ROS2Interface::Get()->GetROSTimestamp();
-        message.height = 1;
-        message.width = results.m_points.size();
+        auto builder = PointCloud2MessageBuilder(
+            GetEntity()->FindComponent<ROS2FrameComponent>()->GetFrameID(), ROS2Interface::Get()->GetROSTimestamp(), results.GetCount());
+
+        builder.AddField("x", sensor_msgs::msg::PointField::FLOAT32)
+            .AddField("y", sensor_msgs::msg::PointField::FLOAT32)
+            .AddField("z", sensor_msgs::msg::PointField::FLOAT32);
 
-        sensor_msgs::PointCloud2Modifier modifier(message);
         if (isIntensityEnabled)
         {
-            modifier.setPointCloud2Fields(
-                4,
-                "x",
-                1,
-                sensor_msgs::msg::PointField::FLOAT32,
-                "y",
-                1,
-                sensor_msgs::msg::PointField::FLOAT32,
-                "z",
-                1,
-                sensor_msgs::msg::PointField::FLOAT32,
-                "intensity",
-                1,
-                sensor_msgs::msg::PointField::FLOAT32);
+            builder.AddField("intensity", sensor_msgs::msg::PointField::FLOAT32);
         }
-        else
-        {
-            modifier.setPointCloud2Fields(
-                3,
-                "x",
-                1,
-                sensor_msgs::msg::PointField::FLOAT32,
-                "y",
-                1,
-                sensor_msgs::msg::PointField::FLOAT32,
-                "z",
-                1,
-                sensor_msgs::msg::PointField::FLOAT32);
-        }
-        modifier.resize(results.m_points.size());
+        sensor_msgs::msg::PointCloud2 message = builder.Get();
+
+
+        sensor_msgs::PointCloud2Iterator<float> messageXIt(message, "x");
+        sensor_msgs::PointCloud2Iterator<float> messageYIt(message, "y");
+        sensor_msgs::PointCloud2Iterator<float> messageZIt(message, "z");
 
-        sensor_msgs::PointCloud2Iterator<float> xIt(message, "x");
-        sensor_msgs::PointCloud2Iterator<float> yIt(message, "y");
-        sensor_msgs::PointCloud2Iterator<float> zIt(message, "z");
+        const auto positionField = results.GetConstFieldSpan<RaycastResultFlags::Point>().value();
+        auto positionIt = positionField.begin();
 
-        AZStd::optional<sensor_msgs::PointCloud2Iterator<float>> intensityIt;
+        AZStd::optional<sensor_msgs::PointCloud2Iterator<float>> messageIntensityIt;
+        AZStd::optional<RaycastResults::FieldSpan<RaycastResultFlags::Intensity>::const_iterator> intensityIt;
         if (isIntensityEnabled)
         {
-            intensityIt = sensor_msgs::PointCloud2Iterator<float>(message, "intensity");
+            messageIntensityIt = sensor_msgs::PointCloud2Iterator<float>(message, "intensity");
+            intensityIt = results.GetConstFieldSpan<RaycastResultFlags::Intensity>().value().begin();
         }
 
         const auto entityTransform = GetEntity()->FindComponent<AzFramework::TransformComponent>();
         const auto inverseLidarTM = entityTransform->GetWorldTM().GetInverse();
-        for (size_t i = 0; i < results.m_points.size(); ++i)
+        for (size_t i = 0; i < results.GetCount(); ++i)
         {
-            const AZ::Vector3 globalPoint = inverseLidarTM.TransformPoint(results.m_points[i]);
-            *xIt = globalPoint.GetX();
-            *yIt = globalPoint.GetY();
-            *zIt = globalPoint.GetZ();
+            AZ::Vector3 point = *positionIt;
+            const AZ::Vector3 globalPoint = inverseLidarTM.TransformPoint(point);
+            *messageXIt = globalPoint.GetX();
+            *messageYIt = globalPoint.GetY();
+            *messageZIt = globalPoint.GetZ();
 
-            if (isIntensityEnabled)
+            if (messageIntensityIt.has_value() && intensityIt.has_value())
             {
-                *intensityIt.value() = results.m_intensities[i];
+                *messageIntensityIt.value() = *intensityIt.value();
                 ++intensityIt.value();
+                ++messageIntensityIt.value();
             }
 
-            ++xIt;
-            ++yIt;
-            ++zIt;
+            ++positionIt;
+            ++messageXIt;
+            ++messageYIt;
+            ++messageZIt;
         }
 
         m_pointCloudPublisher->publish(message);

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

@@ -44,7 +44,7 @@ namespace ROS2
     private:
         //////////////////////////////////////////////////////////////////////////
         void FrequencyTick();
-        void PublishRaycastResults(const RaycastResult& results);
+        void PublishRaycastResults(const RaycastResults& results);
 
         bool m_canRaycasterPublish = false;
         std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::PointCloud2>> m_pointCloudPublisher;

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

@@ -0,0 +1,71 @@
+/*
+ * 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
+ *
+ */
+#include <ROS2/Lidar/RaycastResults.h>
+
+namespace ROS2
+{
+    RaycastResults::RaycastResults(RaycastResultFlags flags, size_t count)
+        : m_count{ count }
+    {
+        EnsureFlagSatisfied<RaycastResultFlags::Point>(flags, count);
+        EnsureFlagSatisfied<RaycastResultFlags::Range>(flags, count);
+        EnsureFlagSatisfied<RaycastResultFlags::Intensity>(flags, count);
+    }
+
+    RaycastResults::RaycastResults(RaycastResults&& other)
+        : m_count{ other.m_count }
+        , m_points{ AZStd::move(other.m_points) }
+        , m_ranges{ AZStd::move(other.m_ranges) }
+        , m_intensities{ AZStd::move(other.m_intensities) }
+    {
+        other.m_count = 0U;
+    }
+
+    void RaycastResults::Clear()
+    {
+        m_count = 0U;
+        ClearFieldIfPresent<RaycastResultFlags::Point>();
+        ClearFieldIfPresent<RaycastResultFlags::Range>();
+        ClearFieldIfPresent<RaycastResultFlags::Intensity>();
+    }
+
+    void RaycastResults::Resize(size_t count)
+    {
+        m_count = count;
+        ResizeFieldIfPresent<RaycastResultFlags::Point>(count);
+        ResizeFieldIfPresent<RaycastResultFlags::Range>(count);
+        ResizeFieldIfPresent<RaycastResultFlags::Intensity>(count);
+    }
+
+    RaycastResults& RaycastResults::operator=(RaycastResults&& other)
+    {
+        if (this == &other)
+        {
+            return *this;
+        }
+
+        m_count = other.m_count;
+        other.m_count = 0U;
+
+        m_points = AZStd::move(other.m_points);
+        m_ranges = AZStd::move(other.m_ranges);
+        m_intensities = AZStd::move(other.m_intensities);
+
+        return *this;
+    }
+
+    bool RaycastResults::IsEmpty() const
+    {
+        return GetCount() == 0U;
+    }
+
+    size_t RaycastResults::GetCount() const
+    {
+        return m_count;
+    }
+} // namespace ROS2

Gems/ROS2/Code/ros2_files.cmake

@@ -70,6 +70,9 @@ set(FILES
         Source/Lidar/LidarTemplate.h
         Source/Lidar/LidarTemplateUtils.cpp
         Source/Lidar/LidarTemplateUtils.h
+        Source/Lidar/PointCloudMessageBuilder.cpp
+        Source/Lidar/PointCloudMessageBuilder.h
+        Source/Lidar/RaycastResults.cpp
         Source/Lidar/LidarCore.cpp
         Source/Lidar/LidarCore.h
         Source/Lidar/ROS2Lidar2DSensorComponent.cpp

Gems/ROS2/Code/ros2_header_files.cmake

@@ -40,6 +40,7 @@ set(FILES
         Include/ROS2/Lidar/LidarRaycasterBus.h
         Include/ROS2/Lidar/LidarSystemBus.h
         Include/ROS2/Lidar/LidarRegistrarBus.h
+        Include/ROS2/Lidar/RaycastResults.h
         Include/ROS2/ROS2Bus.h
         Include/ROS2/ROS2GemUtilities.h
         Include/ROS2/Sensor/Events/EventSourceAdapter.h