Add reflectivity field support

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

@@ -24,7 +24,8 @@ namespace ROS2
         Intensity =             1 << 3,
         Segmentation =          1 << 4,
         RingIds =               1 << 5,
-        All =                   (1 << 6) - 1, // All feature bits enabled.
+        Reflectivity =          1 << 6,
+        All =                   (1 << 7) - 1, // All feature bits enabled.
         // clang-format on
     };
 

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

@@ -24,7 +24,8 @@ namespace ROS2
         SegmentationData        = (1 << 3), //!< return segmentation data
         IsHit                   = (1 << 4), //!< return ray hit booleans
         Ring                    = (1 << 5), //!< return ray ring ids
-        All                     = (1 << 6) - 1U,
+        Reflectivity            = (1 << 6),
+        All                     = (1 << 7) - 1U,
         // clang-format on
     };
 
@@ -81,6 +82,12 @@ namespace ROS2
         using Type = AZ::u16;
     };
 
+    template<>
+    struct ResultTraits<RaycastResultFlags::Reflectivity>
+    {
+        using Type = float;
+    };
+
     //! Class used for storing the results of a raycast.
     //! It guarantees a uniform length of all its fields.
     class RaycastResults
@@ -143,6 +150,7 @@ namespace ROS2
         FieldInternal<RaycastResultFlags::SegmentationData> m_segmentationData;
         FieldInternal<RaycastResultFlags::IsHit> m_isHit;
         FieldInternal<RaycastResultFlags::Ring> m_ringId;
+        FieldInternal<RaycastResultFlags::Reflectivity> m_reflectivities;
         size_t m_count{};
         RaycastResultFlags m_flags;
     };
@@ -215,6 +223,12 @@ namespace ROS2
         return m_ringId;
     }
 
+    template<>
+    inline const RaycastResults::FieldInternal<RaycastResultFlags::Reflectivity>& RaycastResults::GetField<RaycastResultFlags::Reflectivity>() const
+    {
+        return m_reflectivities;
+    }
+
     template<RaycastResultFlags F>
     RaycastResults::FieldInternal<F>& RaycastResults::GetField()
     {

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

@@ -46,6 +46,11 @@ namespace ROS2
             flags |= RaycastResultFlags::Intensity;
         }
 
+        if (configuration.m_lidarSystemFeatures & LidarSystemFeatures::Reflectivity)
+        {
+            flags |= RaycastResultFlags::Reflectivity;
+        }
+
         if (configuration.m_lidarSystemFeatures & LidarSystemFeatures::Segmentation && configuration.m_isSegmentationEnabled)
         {
             if (ClassSegmentationInterface::Get())

Gems/ROS2/Code/Source/Lidar/Publishing/PointCloudMessageFormat.cpp

@@ -62,7 +62,8 @@ namespace ROS2
         case FieldFlags::RangeU32:                  return RaycastResultFlags::Range;
         case FieldFlags::SegmentationData96:        return RaycastResultFlags::SegmentationData;
         case FieldFlags::RingU8:
-        case FieldFlags::RingU16:                    return RaycastResultFlags::Ring;
+        case FieldFlags::RingU16:                   return RaycastResultFlags::Ring;
+        case FieldFlags::ReflectivityU16:           return RaycastResultFlags::Reflectivity;
         default:                                    return RaycastResultFlags::None;
             // clang-format on
         }

Gems/ROS2/Code/Source/Lidar/Publishing/PointCloudMessageWriter.cpp

@@ -49,6 +49,10 @@ namespace ROS2
                 break;
             case RaycastResultFlags::Ring:
                 wasWrittenTo = WriteResultIfPresent<RaycastResultFlags::Ring>(results, fielFlag, i, skipNonHits);
+                break;
+            case RaycastResultFlags::Reflectivity:
+                wasWrittenTo = WriteResultIfPresent<RaycastResultFlags::Reflectivity>(results, fielFlag, i, skipNonHits);
+                break;
             default:
                 break;
             }

Gems/ROS2/Code/Source/Lidar/Publishing/PointCloudMessageWriter.h

@@ -210,6 +210,24 @@ namespace ROS2
         return AZ::Success();
     }
 
+    template<>
+    inline AZ::Outcome<void, AZStd::string_view> PointCloudMessageWriter::
+        AssignResultFieldValue<RaycastResultFlags::Reflectivity, FieldFlags::ReflectivityU16>(
+            const ResultTraits<RaycastResultFlags::Reflectivity>::Type& resultValue,
+            FieldTraits<FieldFlags::ReflectivityU16>::Type& messageFieldValue)
+    {
+        if (resultValue > aznumeric_cast<float>(AZStd::numeric_limits<AZ::u16>::max()))
+        {
+            messageFieldValue = AZStd::numeric_limits<AZ::u16>::max();
+        }
+        else
+        {
+            messageFieldValue = aznumeric_cast<AZ::u16>(resultValue);
+        }
+
+        return AZ::Success();
+    }
+
     template<RaycastResultFlags R>
     bool PointCloudMessageWriter::WriteResultIfPresent(const RaycastResults& results, FieldFlags fieldFlag, size_t index, bool skipNonHits)
     {
@@ -322,6 +340,20 @@ namespace ROS2
         }
     }
 
+    template<>
+    inline void PointCloudMessageWriter::WriteResult<RaycastResultFlags::Reflectivity>(
+        const RaycastResults& results,
+        FieldFlags fieldFlag,
+        size_t fieldIndex,
+        AZStd::optional<RaycastResults::ConstFieldSpan<RaycastResultFlags::IsHit>> isHit)
+    {
+        if (fieldFlag == FieldFlags::ReflectivityU16)
+        {
+            WriteResultToMessageField<RaycastResultFlags::Reflectivity, FieldFlags::ReflectivityU16>(
+                results.GetConstFieldSpan<RaycastResultFlags::Reflectivity>().value(), fieldIndex, isHit);
+        }
+    }
+
     template<RaycastResultFlags R, FieldFlags F>
     void PointCloudMessageWriter::WriteResultToMessageField(
         RaycastResults::ConstFieldSpan<R> fieldSpan,

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

@@ -19,6 +19,7 @@ namespace ROS2
         EnsureFlagSatisfied<RaycastResultFlags::SegmentationData>(flags, count);
         EnsureFlagSatisfied<RaycastResultFlags::IsHit>(flags, count);
         EnsureFlagSatisfied<RaycastResultFlags::Ring>(flags, count);
+        EnsureFlagSatisfied<RaycastResultFlags::Reflectivity>(flags, count);
     }
 
     RaycastResults::RaycastResults(RaycastResults&& other)
@@ -28,6 +29,7 @@ namespace ROS2
         , m_segmentationData{ AZStd::move(other.m_segmentationData) }
         , m_isHit{ AZStd::move(other.m_isHit) }
         , m_ringId{ AZStd::move(other.m_ringId) }
+        , m_reflectivities{ AZStd::move(other.m_reflectivities) }
         , m_count{ other.m_count }
         , m_flags{ other.m_flags }
     {
@@ -44,6 +46,7 @@ namespace ROS2
         ClearFieldIfPresent<RaycastResultFlags::SegmentationData>();
         ClearFieldIfPresent<RaycastResultFlags::IsHit>();
         ClearFieldIfPresent<RaycastResultFlags::Ring>();
+        ClearFieldIfPresent<RaycastResultFlags::Reflectivity>();
     }
 
     void RaycastResults::Resize(size_t count)
@@ -55,6 +58,7 @@ namespace ROS2
         ResizeFieldIfPresent<RaycastResultFlags::SegmentationData>(count);
         ResizeFieldIfPresent<RaycastResultFlags::IsHit>(count);
         ResizeFieldIfPresent<RaycastResultFlags::Ring>(count);
+        ResizeFieldIfPresent<RaycastResultFlags::Reflectivity>(count);
     }
 
     RaycastResults& RaycastResults::operator=(RaycastResults&& other)
@@ -76,6 +80,7 @@ namespace ROS2
         m_segmentationData = AZStd::move(other.m_segmentationData);
         m_isHit = AZStd::move(other.m_isHit);
         m_ringId = AZStd::move(other.m_ringId);
+        m_reflectivities = AZStd::move(other.m_reflectivities);
 
         return *this;
     }