From b2ec5b6b413e356b7474bba99f2819015e1c25dc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Aleksander=20Kami=C5=84ski?= Date: Thu, 22 Aug 2024 09:03:02 +0200 Subject: [PATCH 1/3] Refactor Raycast Results MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Aleksander Kamiński --- .../Include/ROS2/Lidar/LidarRaycasterBus.h | 25 +- .../Code/Include/ROS2/Lidar/RaycastResults.h | 216 ++++++++++++++++++ Gems/ROS2/Code/Source/Lidar/LidarCore.cpp | 55 +++-- Gems/ROS2/Code/Source/Lidar/LidarCore.h | 8 +- .../ROS2/Code/Source/Lidar/LidarRaycaster.cpp | 39 +++- Gems/ROS2/Code/Source/Lidar/LidarRaycaster.h | 4 +- .../Source/Lidar/PointCloudMessageBuilder.cpp | 53 +++++ .../Source/Lidar/PointCloudMessageBuilder.h | 26 +++ .../Lidar/ROS2Lidar2DSensorComponent.cpp | 22 +- .../Source/Lidar/ROS2Lidar2DSensorComponent.h | 2 + .../Source/Lidar/ROS2LidarSensorComponent.cpp | 94 +++----- .../Source/Lidar/ROS2LidarSensorComponent.h | 2 +- .../ROS2/Code/Source/Lidar/RaycastResults.cpp | 73 ++++++ Gems/ROS2/Code/ros2_files.cmake | 3 + Gems/ROS2/Code/ros2_header_files.cmake | 1 + 15 files changed, 512 insertions(+), 111 deletions(-) create mode 100644 Gems/ROS2/Code/Include/ROS2/Lidar/RaycastResults.h create mode 100644 Gems/ROS2/Code/Source/Lidar/PointCloudMessageBuilder.cpp create mode 100644 Gems/ROS2/Code/Source/Lidar/PointCloudMessageBuilder.h create mode 100644 Gems/ROS2/Code/Source/Lidar/RaycastResults.cpp diff --git a/Gems/ROS2/Code/Include/ROS2/Lidar/LidarRaycasterBus.h b/Gems/ROS2/Code/Include/ROS2/Lidar/LidarRaycasterBus.h index 7dfce43ed..3f32d0d63 100644 --- a/Gems/ROS2/Code/Include/ROS2/Lidar/LidarRaycasterBus.h +++ b/Gems/ROS2/Code/Include/ROS2/Lidar/LidarRaycasterBus.h @@ -11,7 +11,9 @@ #include #include #include +#include #include +#include namespace ROS2 { @@ -84,23 +86,6 @@ namespace ROS2 //! Unique id used by lidar raycasters. using LidarId = StronglyTypedUuid; - 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 m_points; - AZStd::vector m_ranges; - AZStd::vector 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 PerformRaycast(const AZ::Transform& lidarTransform) = 0; //! Configures ray Gaussian Noise parameters. //! Each call overrides the previous configuration. diff --git a/Gems/ROS2/Code/Include/ROS2/Lidar/RaycastResults.h b/Gems/ROS2/Code/Include/ROS2/Lidar/RaycastResults.h new file mode 100644 index 000000000..d2c04e35e --- /dev/null +++ b/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 + +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 + bool IsFlagEnabled(RaycastResultFlags flags) + { + return (flags & F) == F; + } + + template + struct ResultTraits; + + template<> + struct ResultTraits + { + using Type = AZ::Vector3; + }; + + template<> + struct ResultTraits + { + using Type = float; + }; + + 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 + { + public: + template::Type> + using FieldSpan = AZStd::span; + + template + using ConstFieldSpan = FieldSpan::Type>; + + explicit RaycastResults(RaycastResultFlags flags, size_t count = 0U); + RaycastResults(const RaycastResults& other) = default; + RaycastResults(RaycastResults&& other); + + [[nodiscard]] bool IsEmpty() const; + template + [[nodiscard]] bool IsFlagSatisfied(RaycastResultFlags requestedFlags) const; + [[nodiscard]] bool SatisfiesResultFlags(RaycastResultFlags flags) const; + + template + [[nodiscard]] bool IsFieldPresent() const; + + [[nodiscard]] size_t GetCount() const; + + template + AZStd::optional> GetConstFieldSpan() const; + + template + AZStd::optional> GetFieldSpan(); + + void Clear(); + void Resize(size_t count); + + RaycastResults& operator=(const RaycastResults& other) = default; + RaycastResults& operator=(RaycastResults&& other); + + private: + template::Type> + using FieldInternal = AZStd::optional>; + + template + const FieldInternal& GetField() const; + + template + FieldInternal& GetField(); + + template + void ResizeFieldIfPresent(size_t count); + + template + void EnsureFlagSatisfied(RaycastResultFlags flags, size_t count); + + template + void ClearFieldIfPresent(); + + size_t m_count{}; + FieldInternal m_points; + FieldInternal m_ranges; + FieldInternal m_intensities; + }; + + template + bool RaycastResults::IsFlagSatisfied(RaycastResultFlags requestedFlags) const + { + return !IsFlagEnabled(requestedFlags) || IsFieldPresent(); + } + + template + bool RaycastResults::IsFieldPresent() const + { + return GetField().has_value(); + } + + template + AZStd::optional> RaycastResults::GetConstFieldSpan() const + { + auto& field = GetField(); + if (!field.has_value()) + { + return {}; + } + + return AZStd::span(field->begin(), field->size()); + } + + template + AZStd::optional> RaycastResults::GetFieldSpan() + { + auto& field = GetField(); + if (!field.has_value()) + { + return {}; + } + + return AZStd::span(field->begin(), field->size()); + } + + template<> + inline const RaycastResults::FieldInternal& RaycastResults::GetField() const + { + return m_points; + } + + template<> + inline const RaycastResults::FieldInternal& RaycastResults::GetField() const + { + return m_ranges; + } + + template<> + inline const RaycastResults::FieldInternal& RaycastResults::GetField() + const + { + return m_intensities; + } + + template + RaycastResults::FieldInternal& RaycastResults::GetField() + { + return const_cast&>(static_cast(this)->GetField()); + } + + template + void RaycastResults::ClearFieldIfPresent() + { + auto& field = GetField(); + if (!field.has_value()) + { + return; + } + + field->clear(); + } + + template + void RaycastResults::ResizeFieldIfPresent(size_t count) + { + auto& field = GetField(); + if (!field.has_value()) + { + return; + } + + field->resize(count); + } + + template + void RaycastResults::EnsureFlagSatisfied(RaycastResultFlags flags, size_t count) + { + if (!IsFlagEnabled(flags)) + { + return; + } + + auto& field = GetField(); + if (!field.has_value()) + { + field = AZStd::vector::Type>(count); + } + else + { + field->resize(count); + } + } +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Lidar/LidarCore.cpp b/Gems/ROS2/Code/Source/Lidar/LidarCore.cpp index 8ed211800..57361d681 100644 --- a/Gems/ROS2/Code/Source/Lidar/LidarCore.cpp +++ b/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().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& 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 LidarCore::PerformRaycast() { - static const RaycastResult EmptyResults{}; - AZ::Entity* entity = nullptr; AZ::ComponentApplicationBus::BroadcastResult(entity, &AZ::ComponentApplicationRequests::FindEntity, m_entityId); const auto entityTransform = entity->FindComponent(); + AZ::Outcome 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 diff --git a/Gems/ROS2/Code/Source/Lidar/LidarCore.h b/Gems/ROS2/Code/Source/Lidar/LidarCore.h index 445441214..b1351c365 100644 --- a/Gems/ROS2/Code/Source/Lidar/LidarCore.h +++ b/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 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 m_implementationToRaycasterMap; LidarId m_lidarRaycasterId; @@ -59,7 +63,7 @@ namespace ROS2 AZ::RPI::AuxGeomDrawPtr m_drawQueue; AZStd::vector m_lastRotations; - RaycastResult m_lastScanResults; + AZStd::vector m_lastPoints; AZ::EntityId m_entityId; }; diff --git a/Gems/ROS2/Code/Source/Lidar/LidarRaycaster.cpp b/Gems/ROS2/Code/Source/Lidar/LidarRaycaster.cpp index 710d41b89..f6970d093 100644 --- a/Gems/ROS2/Code/Source/Lidar/LidarRaycaster.cpp +++ b/Gems/ROS2/Code/Source/Lidar/LidarRaycaster.cpp @@ -112,7 +112,7 @@ namespace ROS2 return requests; } - RaycastResult LidarRaycaster::PerformRaycast(const AZ::Transform& lidarTransform) + AZ::Outcome 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 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::iterator> pointIt; + AZStd::optional::iterator> rangeIt; if (handlePoints) { - results.m_points.reserve(rayDirections.size()); + pointIt = results.GetFieldSpan().value().begin(); } if (handleRanges) { - results.m_ranges.reserve(rayDirections.size()); + rangeIt = results.GetFieldSpan().value().begin(); } auto* sceneInterface = AZ::Interface::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::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::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; } diff --git a/Gems/ROS2/Code/Source/Lidar/LidarRaycaster.h b/Gems/ROS2/Code/Source/Lidar/LidarRaycaster.h index dbec76dc0..08ed6ec45 100644 --- a/Gems/ROS2/Code/Source/Lidar/LidarRaycaster.h +++ b/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 PerformRaycast(const AZ::Transform& lidarTransform) override; void ConfigureIgnoredCollisionLayers(const AZStd::unordered_set& 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 m_range{}; bool m_addMaxRangePoints{ false }; AZStd::vector m_rayRotations{ { AZ::Quaternion::CreateZero() } }; diff --git a/Gems/ROS2/Code/Source/Lidar/PointCloudMessageBuilder.cpp b/Gems/ROS2/Code/Source/Lidar/PointCloudMessageBuilder.cpp new file mode 100644 index 000000000..b4bd534f9 --- /dev/null +++ b/Gems/ROS2/Code/Source/Lidar/PointCloudMessageBuilder.cpp @@ -0,0 +1,53 @@ +/* + * 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 +#include + +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) + { + AZ_Assert(m_message.has_value(), "Programmer error: Message builder was invalid. Double result retrieval."); + + 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.value().fields.push_back(point_field); + + m_offset += point_field.count * sizeOfPointField(dataType); + return *this; + } + + sensor_msgs::msg::PointCloud2 PointCloud2MessageBuilder::Build() + { + AZ_Assert(m_message.has_value(), "Programmer error: Message builder was invalid. Double result retrieval."); + auto& message = m_message.value(); + + message.point_step = m_offset; + message.row_step = message.width * message.point_step; + message.data.resize(message.height * message.row_step); + + sensor_msgs::msg::PointCloud2 result = m_message.value(); + m_message = {}; // Builder invalidated. + return result; + } +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Lidar/PointCloudMessageBuilder.h b/Gems/ROS2/Code/Source/Lidar/PointCloudMessageBuilder.h new file mode 100644 index 000000000..95fffc7cf --- /dev/null +++ b/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 +#include + +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 Build(); + + private: + size_t m_offset = 0U; + AZStd::optional m_message; + }; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp b/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp index 64492e538..dd302a05b 100644 --- a/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp @@ -106,13 +106,20 @@ namespace ROS2 void ROS2Lidar2DSensorComponent::FrequencyTick() { - const RaycastResult& lastScanResults = m_lidarCore.PerformRaycast(); + AZStd::optional 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(); 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().value(); + message.ranges.assign(rangeField.begin(), rangeField.end()); + if (isIntensityEnabled) + { + const auto intensityField = results.GetConstFieldSpan().value(); + message.intensities.assign(intensityField.begin(), intensityField.end()); + } + m_laserScanPublisher->publish(message); } } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.h b/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.h index f5a5875ae..9da7e9f3f 100644 --- a/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.h +++ b/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.h @@ -44,6 +44,8 @@ namespace ROS2 ////////////////////////////////////////////////////////////////////////// void FrequencyTick(); + void PublishRaycastResults(const RaycastResults& results); + std::shared_ptr> m_laserScanPublisher; LidarCore m_lidarCore; diff --git a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp index f1e25668b..e30a77b17 100644 --- a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp @@ -9,6 +9,7 @@ #include #include #include +#include #include #include #include @@ -139,89 +140,70 @@ namespace ROS2 aznumeric_cast(timestamp.sec) * aznumeric_cast(1.0e9f) + timestamp.nanosec); } - const RaycastResult& lastScanResults = m_lidarCore.PerformRaycast(); + AZStd::optional 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(); - 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()->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.Build(); + + + sensor_msgs::PointCloud2Iterator messageXIt(message, "x"); + sensor_msgs::PointCloud2Iterator messageYIt(message, "y"); + sensor_msgs::PointCloud2Iterator messageZIt(message, "z"); - sensor_msgs::PointCloud2Iterator xIt(message, "x"); - sensor_msgs::PointCloud2Iterator yIt(message, "y"); - sensor_msgs::PointCloud2Iterator zIt(message, "z"); + const auto positionField = results.GetConstFieldSpan().value(); + auto positionIt = positionField.begin(); - AZStd::optional> intensityIt; + AZStd::optional> messageIntensityIt; + AZStd::optional::const_iterator> intensityIt; if (isIntensityEnabled) { - intensityIt = sensor_msgs::PointCloud2Iterator(message, "intensity"); + messageIntensityIt = sensor_msgs::PointCloud2Iterator(message, "intensity"); + intensityIt = results.GetConstFieldSpan().value().begin(); } const auto entityTransform = GetEntity()->FindComponent(); 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); diff --git a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.h b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.h index f2b09756c..8bb49cac6 100644 --- a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.h +++ b/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> m_pointCloudPublisher; diff --git a/Gems/ROS2/Code/Source/Lidar/RaycastResults.cpp b/Gems/ROS2/Code/Source/Lidar/RaycastResults.cpp new file mode 100644 index 000000000..d67abe9e1 --- /dev/null +++ b/Gems/ROS2/Code/Source/Lidar/RaycastResults.cpp @@ -0,0 +1,73 @@ +/* + * 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 + +namespace ROS2 +{ + RaycastResults::RaycastResults(RaycastResultFlags flags, size_t count) + : m_count{ count } + { + EnsureFlagSatisfied(flags, count); + EnsureFlagSatisfied(flags, count); + EnsureFlagSatisfied(flags, count); + } + + RaycastResults::RaycastResults(RaycastResults&& other) + { + *this = other; + } + + void RaycastResults::Clear() + { + m_count = 0U; + ClearFieldIfPresent(); + ClearFieldIfPresent(); + ClearFieldIfPresent(); + } + + void RaycastResults::Resize(size_t count) + { + m_count = count; + ResizeFieldIfPresent(count); + ResizeFieldIfPresent(count); + ResizeFieldIfPresent(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; + } + + bool RaycastResults::SatisfiesResultFlags(RaycastResultFlags flags) const + { + return IsFlagSatisfied(flags) && IsFlagSatisfied(flags) && + IsFlagSatisfied(flags); + } + + size_t RaycastResults::GetCount() const + { + return m_count; + } +} // namespace ROS2 diff --git a/Gems/ROS2/Code/ros2_files.cmake b/Gems/ROS2/Code/ros2_files.cmake index f8e66e87b..9b0614d87 100644 --- a/Gems/ROS2/Code/ros2_files.cmake +++ b/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 diff --git a/Gems/ROS2/Code/ros2_header_files.cmake b/Gems/ROS2/Code/ros2_header_files.cmake index 822ae73fd..e6ca1bd63 100644 --- a/Gems/ROS2/Code/ros2_header_files.cmake +++ b/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 From 929629fcb93306940ff4f4e323e893e8d06f4220 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Aleksander=20Kami=C5=84ski?= Date: Fri, 23 Aug 2024 07:55:32 +0200 Subject: [PATCH 2/3] Review changes MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Aleksander Kamiński --- Gems/ROS2/Code/Include/ROS2/Lidar/RaycastResults.h | 9 ++++----- Gems/ROS2/Code/Source/Lidar/LidarCore.cpp | 5 +---- 2 files changed, 5 insertions(+), 9 deletions(-) diff --git a/Gems/ROS2/Code/Include/ROS2/Lidar/RaycastResults.h b/Gems/ROS2/Code/Include/ROS2/Lidar/RaycastResults.h index d2c04e35e..8d2bb3f9e 100644 --- a/Gems/ROS2/Code/Include/ROS2/Lidar/RaycastResults.h +++ b/Gems/ROS2/Code/Include/ROS2/Lidar/RaycastResults.h @@ -21,10 +21,9 @@ namespace ROS2 //! Bitwise operators for RaycastResultFlags AZ_DEFINE_ENUM_BITWISE_OPERATORS(RaycastResultFlags) - template - bool IsFlagEnabled(RaycastResultFlags flags) + inline bool IsFlagEnabled(RaycastResultFlags flag, RaycastResultFlags flags) { - return (flags & F) == F; + return (flags & flag) == flag; } template @@ -113,7 +112,7 @@ namespace ROS2 template bool RaycastResults::IsFlagSatisfied(RaycastResultFlags requestedFlags) const { - return !IsFlagEnabled(requestedFlags) || IsFieldPresent(); + return !IsFlagEnabled(F, requestedFlags) || IsFieldPresent(); } template @@ -198,7 +197,7 @@ namespace ROS2 template void RaycastResults::EnsureFlagSatisfied(RaycastResultFlags flags, size_t count) { - if (!IsFlagEnabled(flags)) + if (!IsFlagEnabled(F, flags)) { return; } diff --git a/Gems/ROS2/Code/Source/Lidar/LidarCore.cpp b/Gems/ROS2/Code/Source/Lidar/LidarCore.cpp index 57361d681..f54f76155 100644 --- a/Gems/ROS2/Code/Source/Lidar/LidarCore.cpp +++ b/Gems/ROS2/Code/Source/Lidar/LidarCore.cpp @@ -192,10 +192,7 @@ namespace ROS2 return {}; } - if (results.GetValue().IsEmpty()) - { - AZ_TracePrintf("Lidar Sensor Component", "No results from raycast\n"); - } + AZ_Warning("Lidar Sensor Component", !results.GetValue().IsEmpty(), "No results from raycast\n"); UpdatePoints(results.GetValue()); From 9ee4ac6927032bd800de63097ea7497a7cad3fcf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Aleksander=20Kami=C5=84ski?= Date: Mon, 26 Aug 2024 09:42:21 +0200 Subject: [PATCH 3/3] Review changes MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Aleksander Kamiński --- .../Code/Include/ROS2/Lidar/RaycastResults.h | 9 --------- Gems/ROS2/Code/Source/Lidar/LidarCore.cpp | 2 +- .../Source/Lidar/PointCloudMessageBuilder.cpp | 19 ++++++------------- .../Source/Lidar/PointCloudMessageBuilder.h | 4 ++-- .../Source/Lidar/ROS2LidarSensorComponent.cpp | 2 +- .../ROS2/Code/Source/Lidar/RaycastResults.cpp | 12 +++++------- 6 files changed, 15 insertions(+), 33 deletions(-) diff --git a/Gems/ROS2/Code/Include/ROS2/Lidar/RaycastResults.h b/Gems/ROS2/Code/Include/ROS2/Lidar/RaycastResults.h index 8d2bb3f9e..bd4d67cb1 100644 --- a/Gems/ROS2/Code/Include/ROS2/Lidar/RaycastResults.h +++ b/Gems/ROS2/Code/Include/ROS2/Lidar/RaycastResults.h @@ -63,9 +63,6 @@ namespace ROS2 RaycastResults(RaycastResults&& other); [[nodiscard]] bool IsEmpty() const; - template - [[nodiscard]] bool IsFlagSatisfied(RaycastResultFlags requestedFlags) const; - [[nodiscard]] bool SatisfiesResultFlags(RaycastResultFlags flags) const; template [[nodiscard]] bool IsFieldPresent() const; @@ -109,12 +106,6 @@ namespace ROS2 FieldInternal m_intensities; }; - template - bool RaycastResults::IsFlagSatisfied(RaycastResultFlags requestedFlags) const - { - return !IsFlagEnabled(F, requestedFlags) || IsFieldPresent(); - } - template bool RaycastResults::IsFieldPresent() const { diff --git a/Gems/ROS2/Code/Source/Lidar/LidarCore.cpp b/Gems/ROS2/Code/Source/Lidar/LidarCore.cpp index f54f76155..e365700dc 100644 --- a/Gems/ROS2/Code/Source/Lidar/LidarCore.cpp +++ b/Gems/ROS2/Code/Source/Lidar/LidarCore.cpp @@ -183,7 +183,7 @@ namespace ROS2 AZ::ComponentApplicationBus::BroadcastResult(entity, &AZ::ComponentApplicationRequests::FindEntity, m_entityId); const auto entityTransform = entity->FindComponent(); - AZ::Outcome results = AZ::Failure("EBus failure occured."); + AZ::Outcome results = AZ::Failure("EBus failure occurred."); LidarRaycasterRequestBus::EventResult( results, m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::PerformRaycast, entityTransform->GetWorldTM()); if (!results.IsSuccess()) diff --git a/Gems/ROS2/Code/Source/Lidar/PointCloudMessageBuilder.cpp b/Gems/ROS2/Code/Source/Lidar/PointCloudMessageBuilder.cpp index b4bd534f9..8c80e8d6c 100644 --- a/Gems/ROS2/Code/Source/Lidar/PointCloudMessageBuilder.cpp +++ b/Gems/ROS2/Code/Source/Lidar/PointCloudMessageBuilder.cpp @@ -24,30 +24,23 @@ namespace ROS2 PointCloud2MessageBuilder& PointCloud2MessageBuilder::AddField(const char* name, uint8_t dataType, size_t count) { - AZ_Assert(m_message.has_value(), "Programmer error: Message builder was invalid. Double result retrieval."); - 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.value().fields.push_back(point_field); + m_message.fields.push_back(point_field); m_offset += point_field.count * sizeOfPointField(dataType); return *this; } - sensor_msgs::msg::PointCloud2 PointCloud2MessageBuilder::Build() + sensor_msgs::msg::PointCloud2 PointCloud2MessageBuilder::Get() { - AZ_Assert(m_message.has_value(), "Programmer error: Message builder was invalid. Double result retrieval."); - auto& message = m_message.value(); - - message.point_step = m_offset; - message.row_step = message.width * message.point_step; - message.data.resize(message.height * message.row_step); + 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); - sensor_msgs::msg::PointCloud2 result = m_message.value(); - m_message = {}; // Builder invalidated. - return result; + return m_message; } } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Lidar/PointCloudMessageBuilder.h b/Gems/ROS2/Code/Source/Lidar/PointCloudMessageBuilder.h index 95fffc7cf..776070375 100644 --- a/Gems/ROS2/Code/Source/Lidar/PointCloudMessageBuilder.h +++ b/Gems/ROS2/Code/Source/Lidar/PointCloudMessageBuilder.h @@ -17,10 +17,10 @@ namespace ROS2 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 Build(); + sensor_msgs::msg::PointCloud2 Get(); private: size_t m_offset = 0U; - AZStd::optional m_message; + sensor_msgs::msg::PointCloud2 m_message; }; } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp index e30a77b17..6bcee8925 100644 --- a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp @@ -165,7 +165,7 @@ namespace ROS2 { builder.AddField("intensity", sensor_msgs::msg::PointField::FLOAT32); } - sensor_msgs::msg::PointCloud2 message = builder.Build(); + sensor_msgs::msg::PointCloud2 message = builder.Get(); sensor_msgs::PointCloud2Iterator messageXIt(message, "x"); diff --git a/Gems/ROS2/Code/Source/Lidar/RaycastResults.cpp b/Gems/ROS2/Code/Source/Lidar/RaycastResults.cpp index d67abe9e1..56ad3e99a 100644 --- a/Gems/ROS2/Code/Source/Lidar/RaycastResults.cpp +++ b/Gems/ROS2/Code/Source/Lidar/RaycastResults.cpp @@ -18,8 +18,12 @@ namespace ROS2 } 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) } { - *this = other; + other.m_count = 0U; } void RaycastResults::Clear() @@ -60,12 +64,6 @@ namespace ROS2 return GetCount() == 0U; } - bool RaycastResults::SatisfiesResultFlags(RaycastResultFlags flags) const - { - return IsFlagSatisfied(flags) && IsFlagSatisfied(flags) && - IsFlagSatisfied(flags); - } - size_t RaycastResults::GetCount() const { return m_count;