diff --git a/Gems/ROS2/Code/Include/ROS2/Lidar/LidarRegistrarBus.h b/Gems/ROS2/Code/Include/ROS2/Lidar/LidarRegistrarBus.h index f5e675967d..ae5c2bc78e 100644 --- a/Gems/ROS2/Code/Include/ROS2/Lidar/LidarRegistrarBus.h +++ b/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 }; diff --git a/Gems/ROS2/Code/Include/ROS2/Lidar/RaycastResults.h b/Gems/ROS2/Code/Include/ROS2/Lidar/RaycastResults.h index 4e5c2110b5..1e82d4c5d5 100644 --- a/Gems/ROS2/Code/Include/ROS2/Lidar/RaycastResults.h +++ b/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 + { + 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 m_segmentationData; FieldInternal m_isHit; FieldInternal m_ringId; + FieldInternal m_reflectivities; size_t m_count{}; RaycastResultFlags m_flags; }; @@ -215,6 +223,12 @@ namespace ROS2 return m_ringId; } + template<> + inline const RaycastResults::FieldInternal& RaycastResults::GetField() const + { + return m_reflectivities; + } + template RaycastResults::FieldInternal& RaycastResults::GetField() { diff --git a/Gems/ROS2/Code/Source/Lidar/LidarCore.cpp b/Gems/ROS2/Code/Source/Lidar/LidarCore.cpp index 1f5ea8d106..84d1093208 100644 --- a/Gems/ROS2/Code/Source/Lidar/LidarCore.cpp +++ b/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()) diff --git a/Gems/ROS2/Code/Source/Lidar/Publishing/PointCloudMessageFormat.cpp b/Gems/ROS2/Code/Source/Lidar/Publishing/PointCloudMessageFormat.cpp index e949e238cf..639e6e10a5 100644 --- a/Gems/ROS2/Code/Source/Lidar/Publishing/PointCloudMessageFormat.cpp +++ b/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 } diff --git a/Gems/ROS2/Code/Source/Lidar/Publishing/PointCloudMessageWriter.cpp b/Gems/ROS2/Code/Source/Lidar/Publishing/PointCloudMessageWriter.cpp index 08d70ba330..c10ca757e6 100644 --- a/Gems/ROS2/Code/Source/Lidar/Publishing/PointCloudMessageWriter.cpp +++ b/Gems/ROS2/Code/Source/Lidar/Publishing/PointCloudMessageWriter.cpp @@ -49,6 +49,10 @@ namespace ROS2 break; case RaycastResultFlags::Ring: wasWrittenTo = WriteResultIfPresent(results, fielFlag, i, skipNonHits); + break; + case RaycastResultFlags::Reflectivity: + wasWrittenTo = WriteResultIfPresent(results, fielFlag, i, skipNonHits); + break; default: break; } diff --git a/Gems/ROS2/Code/Source/Lidar/Publishing/PointCloudMessageWriter.h b/Gems/ROS2/Code/Source/Lidar/Publishing/PointCloudMessageWriter.h index d59826268c..a64b040727 100644 --- a/Gems/ROS2/Code/Source/Lidar/Publishing/PointCloudMessageWriter.h +++ b/Gems/ROS2/Code/Source/Lidar/Publishing/PointCloudMessageWriter.h @@ -210,6 +210,24 @@ namespace ROS2 return AZ::Success(); } + template<> + inline AZ::Outcome PointCloudMessageWriter:: + AssignResultFieldValue( + const ResultTraits::Type& resultValue, + FieldTraits::Type& messageFieldValue) + { + if (resultValue > aznumeric_cast(AZStd::numeric_limits::max())) + { + messageFieldValue = AZStd::numeric_limits::max(); + } + else + { + messageFieldValue = aznumeric_cast(resultValue); + } + + return AZ::Success(); + } + template bool PointCloudMessageWriter::WriteResultIfPresent(const RaycastResults& results, FieldFlags fieldFlag, size_t index, bool skipNonHits) { @@ -322,6 +340,20 @@ namespace ROS2 } } + template<> + inline void PointCloudMessageWriter::WriteResult( + const RaycastResults& results, + FieldFlags fieldFlag, + size_t fieldIndex, + AZStd::optional> isHit) + { + if (fieldFlag == FieldFlags::ReflectivityU16) + { + WriteResultToMessageField( + results.GetConstFieldSpan().value(), fieldIndex, isHit); + } + } + template void PointCloudMessageWriter::WriteResultToMessageField( RaycastResults::ConstFieldSpan fieldSpan, diff --git a/Gems/ROS2/Code/Source/Lidar/RaycastResults.cpp b/Gems/ROS2/Code/Source/Lidar/RaycastResults.cpp index 878b383e90..32b467a95d 100644 --- a/Gems/ROS2/Code/Source/Lidar/RaycastResults.cpp +++ b/Gems/ROS2/Code/Source/Lidar/RaycastResults.cpp @@ -19,6 +19,7 @@ namespace ROS2 EnsureFlagSatisfied(flags, count); EnsureFlagSatisfied(flags, count); EnsureFlagSatisfied(flags, count); + EnsureFlagSatisfied(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(); ClearFieldIfPresent(); ClearFieldIfPresent(); + ClearFieldIfPresent(); } void RaycastResults::Resize(size_t count) @@ -55,6 +58,7 @@ namespace ROS2 ResizeFieldIfPresent(count); ResizeFieldIfPresent(count); ResizeFieldIfPresent(count); + ResizeFieldIfPresent(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; }