From b5ce30ecd0900daa9c0d7914d47da844fe4d4912 Mon Sep 17 00:00:00 2001 From: Antoni-Robotec <138497503+Antoni-Robotec@users.noreply.github.com> Date: Wed, 6 Sep 2023 11:59:50 +0200 Subject: [PATCH] Lidar component refactor (#463) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Lidar Refactor --------- Signed-off-by: Antoni Puch Signed-off-by: Antoni-Robotec <138497503+Antoni-Robotec@users.noreply.github.com> Co-authored-by: Adam Dąbrowski --- .../Lidar/Lidar2DSensorConfiguration.cpp | 131 ------------- .../Source/Lidar/Lidar2DSensorConfiguration.h | 53 ------ Gems/ROS2/Code/Source/Lidar/LidarCore.cpp | 178 ++++++++++++++++++ Gems/ROS2/Code/Source/Lidar/LidarCore.h | 67 +++++++ .../ROS2/Code/Source/Lidar/LidarRaycaster.cpp | 4 +- .../Lidar/LidarRegistrarSystemComponent.cpp | 14 +- .../Source/Lidar/LidarSensorConfiguration.cpp | 55 ++++-- .../Source/Lidar/LidarSensorConfiguration.h | 20 +- Gems/ROS2/Code/Source/Lidar/LidarTemplate.cpp | 12 +- Gems/ROS2/Code/Source/Lidar/LidarTemplate.h | 3 + .../Code/Source/Lidar/LidarTemplateUtils.cpp | 47 ++++- .../Code/Source/Lidar/LidarTemplateUtils.h | 8 + .../Lidar/ROS2Lidar2DSensorComponent.cpp | 156 +++------------ .../Source/Lidar/ROS2Lidar2DSensorComponent.h | 19 +- .../Source/Lidar/ROS2LidarSensorComponent.cpp | 170 ++++------------- .../Source/Lidar/ROS2LidarSensorComponent.h | 16 +- Gems/ROS2/Code/Source/ROS2SystemComponent.cpp | 2 + .../Source/Sensor/ROS2SensorComponent.cpp | 5 +- Gems/ROS2/Code/ros2_files.cmake | 4 +- 19 files changed, 443 insertions(+), 521 deletions(-) delete mode 100644 Gems/ROS2/Code/Source/Lidar/Lidar2DSensorConfiguration.cpp delete mode 100644 Gems/ROS2/Code/Source/Lidar/Lidar2DSensorConfiguration.h create mode 100644 Gems/ROS2/Code/Source/Lidar/LidarCore.cpp create mode 100644 Gems/ROS2/Code/Source/Lidar/LidarCore.h diff --git a/Gems/ROS2/Code/Source/Lidar/Lidar2DSensorConfiguration.cpp b/Gems/ROS2/Code/Source/Lidar/Lidar2DSensorConfiguration.cpp deleted file mode 100644 index d4d42049d..000000000 --- a/Gems/ROS2/Code/Source/Lidar/Lidar2DSensorConfiguration.cpp +++ /dev/null @@ -1,131 +0,0 @@ -/* - * 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 "Lidar2DSensorConfiguration.h" -#include -#include - -namespace ROS2 -{ - void Lidar2DSensorConfiguration::Reflect(AZ::ReflectContext* context) - { - if (auto serializeContext = azrtti_cast(context)) - { - serializeContext->Class() - ->Version(1) - ->Field("lidarModel", &Lidar2DSensorConfiguration::m_lidarModel) - ->Field("lidarImplementation", &Lidar2DSensorConfiguration::m_lidarSystem) - ->Field("LidarParameters", &Lidar2DSensorConfiguration::m_lidarParameters) - ->Field("IgnoredLayerIndices", &Lidar2DSensorConfiguration::m_ignoredCollisionLayers) - ->Field("ExcludedEntities", &Lidar2DSensorConfiguration::m_excludedEntities) - ->Field("PointsAtMax", &Lidar2DSensorConfiguration::m_addPointsAtMax); - - if (AZ::EditContext* ec = serializeContext->GetEditContext()) - { - ec->Class("ROS2 Lidar 2D Sensor configuration", "Lidar 2D sensor configuration") - ->DataElement(AZ::Edit::UIHandlers::ComboBox, &Lidar2DSensorConfiguration::m_lidarModel, "Lidar Model", "Lidar model") - ->Attribute(AZ::Edit::Attributes::ChangeNotify, &Lidar2DSensorConfiguration::OnLidarModelSelected) - ->EnumAttribute(LidarTemplate::LidarModel::Custom2DLidar, "Custom Lidar 2D") - ->EnumAttribute(LidarTemplate::LidarModel::Slamtec_RPLIDAR_S1, "Slamtec RPLIDAR S1") - ->DataElement( - AZ::Edit::UIHandlers::ComboBox, - &Lidar2DSensorConfiguration::m_lidarSystem, - "Lidar Implementation", - "Select a lidar implementation out of registered ones.") - ->Attribute(AZ::Edit::Attributes::ChangeNotify, &Lidar2DSensorConfiguration::OnLidarImplementationSelected) - ->Attribute(AZ::Edit::Attributes::StringList, &Lidar2DSensorConfiguration::FetchLidarSystemList) - ->DataElement( - AZ::Edit::UIHandlers::EntityId, - &Lidar2DSensorConfiguration::m_lidarParameters, - "Lidar parameters", - "Configuration of Custom lidar") - ->Attribute(AZ::Edit::Attributes::Visibility, &Lidar2DSensorConfiguration::IsConfigurationVisible) - ->DataElement( - AZ::Edit::UIHandlers::Default, - &Lidar2DSensorConfiguration::m_ignoredCollisionLayers, - "Ignored collision layers", - "Indices of collision layers to ignore") - ->Attribute(AZ::Edit::Attributes::Visibility, &Lidar2DSensorConfiguration::IsIgnoredLayerConfigurationVisible) - ->DataElement( - AZ::Edit::UIHandlers::Default, - &Lidar2DSensorConfiguration::m_excludedEntities, - "Excluded Entities", - "List of entities excluded from raycasting.") - ->Attribute(AZ::Edit::Attributes::AutoExpand, true) - ->Attribute(AZ::Edit::Attributes::ContainerCanBeModified, true) - ->Attribute(AZ::Edit::Attributes::Visibility, &Lidar2DSensorConfiguration::IsEntityExclusionVisible) - ->DataElement( - AZ::Edit::UIHandlers::Default, - &Lidar2DSensorConfiguration::m_addPointsAtMax, - "Points at Max", - "If set true LiDAR will produce points at max range for free space") - ->Attribute(AZ::Edit::Attributes::Visibility, &Lidar2DSensorConfiguration::IsMaxPointsConfigurationVisible); - } - } - } - - void Lidar2DSensorConfiguration::FetchLidarImplementationFeatures() - { - if (m_lidarSystem.empty()) - { - m_lidarSystem = Details::GetDefaultLidarSystem(); - } - const auto* lidarMetaData = LidarRegistrarInterface::Get()->GetLidarSystemMetaData(m_lidarSystem); - AZ_Warning("Lidar2DSensorConfiguration", lidarMetaData, "No metadata for \"%s\"", m_lidarSystem.c_str()); - if (lidarMetaData) - { - m_lidarSystemFeatures = LidarRegistrarInterface::Get()->GetLidarSystemMetaData(m_lidarSystem)->m_features; - } - } - - bool Lidar2DSensorConfiguration::IsConfigurationVisible() const - { - return m_lidarModel == LidarTemplate::LidarModel::Custom2DLidar; - } - - bool Lidar2DSensorConfiguration::IsIgnoredLayerConfigurationVisible() const - { - return m_lidarSystemFeatures & LidarSystemFeatures::CollisionLayers; - } - - bool Lidar2DSensorConfiguration::IsEntityExclusionVisible() const - { - return m_lidarSystemFeatures & LidarSystemFeatures::EntityExclusion; - } - - bool Lidar2DSensorConfiguration::IsMaxPointsConfigurationVisible() const - { - return m_lidarSystemFeatures & LidarSystemFeatures::MaxRangePoints; - } - - AZ::Crc32 Lidar2DSensorConfiguration::OnLidarModelSelected() - { - m_lidarParameters = LidarTemplateUtils::GetTemplate(m_lidarModel); - UpdateShowNoise(); - return AZ::Edit::PropertyRefreshLevels::EntireTree; - } - - AZ::Crc32 Lidar2DSensorConfiguration::OnLidarImplementationSelected() - { - FetchLidarImplementationFeatures(); - UpdateShowNoise(); - return AZ::Edit::PropertyRefreshLevels::EntireTree; - } - - AZStd::vector Lidar2DSensorConfiguration::FetchLidarSystemList() - { - FetchLidarImplementationFeatures(); - UpdateShowNoise(); - return LidarRegistrarInterface::Get()->GetRegisteredLidarSystems(); - } - - void Lidar2DSensorConfiguration::UpdateShowNoise() - { - m_lidarParameters.m_showNoiseConfig = m_lidarSystemFeatures & LidarSystemFeatures::Noise; - } -} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Lidar/Lidar2DSensorConfiguration.h b/Gems/ROS2/Code/Source/Lidar/Lidar2DSensorConfiguration.h deleted file mode 100644 index 4b89c6287..000000000 --- a/Gems/ROS2/Code/Source/Lidar/Lidar2DSensorConfiguration.h +++ /dev/null @@ -1,53 +0,0 @@ -/* - * 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 -#include -#include - -#include "LidarRegistrarSystemComponent.h" -#include "LidarTemplate.h" -#include "LidarTemplateUtils.h" - -namespace ROS2 -{ - //! A structure capturing configuration of a lidar sensor (to be used with ROS2Lidar2DSensorComponent). - class Lidar2DSensorConfiguration - { - public: - AZ_TYPE_INFO(Lidar2DSensorConfiguration, "{ed89dd3c-81ef-4636-9907-7ea641f8fbb0}"); - static void Reflect(AZ::ReflectContext* context); - - void FetchLidarImplementationFeatures(); - - LidarSystemFeatures m_lidarSystemFeatures; - - AZStd::string m_lidarSystem; - LidarTemplate::LidarModel m_lidarModel = LidarTemplate::LidarModel::Custom2DLidar; - LidarTemplate m_lidarParameters = LidarTemplateUtils::GetTemplate(LidarTemplate::LidarModel::Custom2DLidar); - - AZStd::unordered_set m_ignoredCollisionLayers; - AZStd::vector m_excludedEntities; - - bool m_addPointsAtMax = false; - - private: - bool IsConfigurationVisible() const; - bool IsIgnoredLayerConfigurationVisible() const; - bool IsEntityExclusionVisible() const; - bool IsMaxPointsConfigurationVisible() const; - - AZ::Crc32 OnLidarModelSelected(); - AZ::Crc32 OnLidarImplementationSelected(); - AZStd::vector FetchLidarSystemList(); - - void UpdateShowNoise(); - }; -} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Lidar/LidarCore.cpp b/Gems/ROS2/Code/Source/Lidar/LidarCore.cpp new file mode 100644 index 000000000..0bb932296 --- /dev/null +++ b/Gems/ROS2/Code/Source/Lidar/LidarCore.cpp @@ -0,0 +1,178 @@ +/* + * 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 "LidarCore.h" +#include +#include +#include +#include +#include +#include +#include + +namespace ROS2 +{ + + void LidarCore::Reflect(AZ::ReflectContext* context) + { + LidarSensorConfiguration::Reflect(context); + + if (auto serialize = azrtti_cast(context)) + { + serialize->Class()->Version(1)->Field("lidarConfiguration", &LidarCore::m_lidarConfiguration); + + if (AZ::EditContext* ec = serialize->GetEditContext()) + { + ec->Class("ROS2 Lidar Sensor", "Lidar sensor component") + ->DataElement( + AZ::Edit::UIHandlers::ComboBox, &LidarCore::m_lidarConfiguration, "Lidar configuration", "Lidar configuration") + ->Attribute(AZ::Edit::Attributes::Visibility, AZ::Edit::PropertyVisibility::ShowChildrenOnly); + } + } + } + + void LidarCore::ConnectToLidarRaycaster() + { + if (auto raycasterId = m_implementationToRaycasterMap.find(m_lidarConfiguration.m_lidarSystem); + raycasterId != m_implementationToRaycasterMap.end()) + { + m_lidarRaycasterId = raycasterId->second; + return; + } + + m_lidarRaycasterId = LidarId::CreateNull(); + LidarSystemRequestBus::EventResult( + m_lidarRaycasterId, AZ_CRC(m_lidarConfiguration.m_lidarSystem), &LidarSystemRequestBus::Events::CreateLidar, m_entityId); + AZ_Assert(!m_lidarRaycasterId.IsNull(), "Could not access selected Lidar System."); + + m_implementationToRaycasterMap.emplace(m_lidarConfiguration.m_lidarSystem, m_lidarRaycasterId); + } + + void LidarCore::ConfigureLidarRaycaster() + { + LidarRaycasterRequestBus::Event(m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRayOrientations, m_lastRotations); + LidarRaycasterRequestBus::Event( + m_lidarRaycasterId, + &LidarRaycasterRequestBus::Events::ConfigureMinimumRayRange, + m_lidarConfiguration.m_lidarParameters.m_minRange); + LidarRaycasterRequestBus::Event( + m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRayRange, m_lidarConfiguration.m_lidarParameters.m_maxRange); + + if ((m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::Noise) && + m_lidarConfiguration.m_lidarParameters.m_isNoiseEnabled) + { + LidarRaycasterRequestBus::Event( + m_lidarRaycasterId, + &LidarRaycasterRequestBus::Events::ConfigureNoiseParameters, + m_lidarConfiguration.m_lidarParameters.m_noiseParameters.m_angularNoiseStdDev, + m_lidarConfiguration.m_lidarParameters.m_noiseParameters.m_distanceNoiseStdDevBase, + m_lidarConfiguration.m_lidarParameters.m_noiseParameters.m_distanceNoiseStdDevRisePerMeter); + } + + RaycastResultFlags requestedFlags = RaycastResultFlags::Ranges | RaycastResultFlags::Points; + + LidarRaycasterRequestBus::Event(m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRaycastResultFlags, requestedFlags); + + if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::CollisionLayers) + { + LidarRaycasterRequestBus::Event( + m_lidarRaycasterId, + &LidarRaycasterRequestBus::Events::ConfigureIgnoredCollisionLayers, + m_lidarConfiguration.m_ignoredCollisionLayers); + } + + if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::EntityExclusion) + { + LidarRaycasterRequestBus::Event( + m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ExcludeEntities, m_lidarConfiguration.m_excludedEntities); + } + + if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::MaxRangePoints) + { + LidarRaycasterRequestBus::Event( + m_lidarRaycasterId, + &LidarRaycasterRequestBus::Events::ConfigureMaxRangePointAddition, + m_lidarConfiguration.m_addPointsAtMax); + } + } + + LidarCore::LidarCore(const AZStd::vector& availableModels) + : m_lidarConfiguration(availableModels) + { + } + + LidarCore::LidarCore(const LidarSensorConfiguration& lidarConfiguration) + : m_lidarConfiguration(lidarConfiguration) + { + } + + void LidarCore::VisualizeResults() const + { + if (m_lastScanResults.m_points.empty()) + { + return; + } + + if (m_drawQueue) + { + 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_colors = &AZ::Colors::Red; + drawArgs.m_colorCount = 1; + drawArgs.m_opacityType = AZ::RPI::AuxGeomDraw::OpacityType::Opaque; + drawArgs.m_size = pixelSize; + m_drawQueue->DrawPoints(drawArgs); + } + } + + void LidarCore::Init(AZ::EntityId entityId) + { + m_entityId = entityId; + + auto* entityScene = AZ::RPI::Scene::GetSceneForEntityId(m_entityId); + m_drawQueue = AZ::RPI::AuxGeomFeatureProcessorInterface::GetDrawQueueForScene(entityScene); + + m_lastRotations = LidarTemplateUtils::PopulateRayRotations(m_lidarConfiguration.m_lidarParameters); + + ConnectToLidarRaycaster(); + ConfigureLidarRaycaster(); + } + + void LidarCore::Deinit() + { + for (auto& [implementation, raycasterId] : m_implementationToRaycasterMap) + { + LidarSystemRequestBus::Event(AZ_CRC(implementation), &LidarSystemRequestBus::Events::DestroyLidar, raycasterId); + } + + m_implementationToRaycasterMap.clear(); + } + + LidarId LidarCore::GetLidarRaycasterId() const + { + return m_lidarRaycasterId; + } + + RaycastResult LidarCore::PerformRaycast() + { + AZ::Entity* entity = nullptr; + AZ::ComponentApplicationBus::BroadcastResult(entity, &AZ::ComponentApplicationRequests::FindEntity, m_entityId); + const auto entityTransform = entity->FindComponent(); + + LidarRaycasterRequestBus::EventResult( + m_lastScanResults, m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::PerformRaycast, entityTransform->GetWorldTM()); + if (m_lastScanResults.m_points.empty()) + { + AZ_TracePrintf("Lidar Sensor Component", "No results from raycast\n"); + return RaycastResult(); + } + return m_lastScanResults; + } +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Lidar/LidarCore.h b/Gems/ROS2/Code/Source/Lidar/LidarCore.h new file mode 100644 index 000000000..f517deb80 --- /dev/null +++ b/Gems/ROS2/Code/Source/Lidar/LidarCore.h @@ -0,0 +1,67 @@ +/* + * 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 +#include +#include +#include +#include + +#include "LidarRaycaster.h" +#include "LidarSensorConfiguration.h" + +namespace ROS2 +{ + //! A class for executing lidar operations, such as data acquisition and visualization. + class LidarCore + { + public: + AZ_TYPE_INFO(LidarCore, "{e46126a2-7a86-bb65-367a-416f2cab393c}"); + static void Reflect(AZ::ReflectContext* context); + + LidarCore(const AZStd::vector& availableModels = {}); + LidarCore(const LidarSensorConfiguration& lidarConfiguration); + ~LidarCore() = default; + + //! Initialize when activating the lidar. + //! @param entityId Entity from which the rays are sent. + void Init(AZ::EntityId entityId); + //! Deinitialize when deactivating the lidar. + void Deinit(); + + //! Perform a raycast. + //! @return Results of the raycast. + RaycastResult PerformRaycast(); + //! Visualize the results of the last performed raycast. + void VisualizeResults() const; + + //! Get the raycaster used by this lidar. + //! @return Used raycaster's id. + LidarId GetLidarRaycasterId() const; + + //! Configuration according to which the lidar performs its raycasts. + LidarSensorConfiguration m_lidarConfiguration; + + private: + void ConnectToLidarRaycaster(); + void ConfigureLidarRaycaster(); + + //! An unordered map of lidar implementations to their raycasters created by this LidarSensorComponent. + AZStd::unordered_map m_implementationToRaycasterMap; + LidarId m_lidarRaycasterId; + + AZ::RPI::AuxGeomDrawPtr m_drawQueue; + + AZStd::vector m_lastRotations; + RaycastResult m_lastScanResults; + + AZ::EntityId m_entityId; + }; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Lidar/LidarRaycaster.cpp b/Gems/ROS2/Code/Source/Lidar/LidarRaycaster.cpp index 2bd083b40..369828ce2 100644 --- a/Gems/ROS2/Code/Source/Lidar/LidarRaycaster.cpp +++ b/Gems/ROS2/Code/Source/Lidar/LidarRaycaster.cpp @@ -99,8 +99,8 @@ namespace ROS2 request->m_distance = m_range; request->m_reportMultipleHits = false; - request->m_filterCallback = - [ignoredCollisionLayers = this->m_ignoredCollisionLayers](const AzPhysics::SimulatedBody* simBody, const Physics::Shape* shape) + request->m_filterCallback = [ignoredCollisionLayers = this->m_ignoredCollisionLayers]( + const AzPhysics::SimulatedBody* simBody, const Physics::Shape* shape) { if (ignoredCollisionLayers.contains(shape->GetCollisionLayer().GetIndex())) { diff --git a/Gems/ROS2/Code/Source/Lidar/LidarRegistrarSystemComponent.cpp b/Gems/ROS2/Code/Source/Lidar/LidarRegistrarSystemComponent.cpp index ce6edc253..a9c71262d 100644 --- a/Gems/ROS2/Code/Source/Lidar/LidarRegistrarSystemComponent.cpp +++ b/Gems/ROS2/Code/Source/Lidar/LidarRegistrarSystemComponent.cpp @@ -113,14 +113,14 @@ namespace ROS2 } AZStd::string Details::GetDefaultLidarSystem() + { + const auto& lidarSystemList = LidarRegistrarInterface::Get()->GetRegisteredLidarSystems(); + if (lidarSystemList.empty()) { - const auto& lidarSystemList = LidarRegistrarInterface::Get()->GetRegisteredLidarSystems(); - if (lidarSystemList.empty()) - { - AZ_Warning("ROS2LidarSensorComponent", false, "No LIDAR system for the sensor to use."); - return {}; - } - return lidarSystemList.front(); + AZ_Warning("ROS2LidarSensorComponent", false, "No LIDAR system for the sensor to use."); + return {}; } + return lidarSystemList.front(); + } } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Lidar/LidarSensorConfiguration.cpp b/Gems/ROS2/Code/Source/Lidar/LidarSensorConfiguration.cpp index 24adf8be6..c3a122431 100644 --- a/Gems/ROS2/Code/Source/Lidar/LidarSensorConfiguration.cpp +++ b/Gems/ROS2/Code/Source/Lidar/LidarSensorConfiguration.cpp @@ -18,7 +18,7 @@ namespace ROS2 { serializeContext->Class() ->Version(1) - ->Field("lidarModel", &LidarSensorConfiguration::m_lidarModel) + ->Field("lidarModelName", &LidarSensorConfiguration::m_lidarModelName) ->Field("lidarImplementation", &LidarSensorConfiguration::m_lidarSystem) ->Field("LidarParameters", &LidarSensorConfiguration::m_lidarParameters) ->Field("IgnoredLayerIndices", &LidarSensorConfiguration::m_ignoredCollisionLayers) @@ -27,15 +27,10 @@ namespace ROS2 if (AZ::EditContext* ec = serializeContext->GetEditContext()) { - ec->Class("ROS2 Lidar Sensor configuration", "Lidar sensor configuration") - ->DataElement(AZ::Edit::UIHandlers::ComboBox, &LidarSensorConfiguration::m_lidarModel, "Lidar Model", "Lidar model") + ec->Class("Lidar Sensor configuration", "Lidar sensor configuration") + ->DataElement(AZ::Edit::UIHandlers::ComboBox, &LidarSensorConfiguration::m_lidarModelName, "Lidar Model", "Lidar model") ->Attribute(AZ::Edit::Attributes::ChangeNotify, &LidarSensorConfiguration::OnLidarModelSelected) - ->EnumAttribute(LidarTemplate::LidarModel::Custom3DLidar, "Custom Lidar") - ->EnumAttribute(LidarTemplate::LidarModel::Ouster_OS0_64, "Ouster OS0-64") - ->EnumAttribute(LidarTemplate::LidarModel::Ouster_OS1_64, "Ouster OS1-64") - ->EnumAttribute(LidarTemplate::LidarModel::Ouster_OS2_64, "Ouster OS2-64") - ->EnumAttribute(LidarTemplate::LidarModel::Velodyne_Puck, "Velodyne Puck (VLP-16)") - ->EnumAttribute(LidarTemplate::LidarModel::Velodyne_HDL_32E, "Velodyne HDL-32E") + ->Attribute(AZ::Edit::Attributes::StringList, &LidarSensorConfiguration::GetAvailableModels) ->DataElement( AZ::Edit::UIHandlers::ComboBox, &LidarSensorConfiguration::m_lidarSystem, @@ -73,6 +68,19 @@ namespace ROS2 } } + LidarSensorConfiguration::LidarSensorConfiguration(AZStd::vector availableModels) + : m_availableModels(AZStd::move(availableModels)) + { + if (m_availableModels.empty()) + { + AZ_Warning("LidarSensorConfiguration", false, "Lidar configuration created with an empty models list"); + return; + } + m_lidarModel = m_availableModels.front(); + m_lidarParameters = LidarTemplateUtils::GetTemplate(m_lidarModel); + m_lidarModelName = m_lidarParameters.m_name; + } + void LidarSensorConfiguration::FetchLidarImplementationFeatures() { if (m_lidarSystem.empty()) @@ -87,9 +95,34 @@ namespace ROS2 } } + AZStd::vector LidarSensorConfiguration::GetAvailableModels() const + { + AZStd::vector result; + for (const auto model : m_availableModels) + { + auto templ = LidarTemplateUtils::GetTemplate(model); + result.push_back({ templ.m_name }); + } + return result; + } + + void LidarSensorConfiguration::FetchLidarModelConfiguration() + { + for (const auto model : m_availableModels) + { + auto templ = LidarTemplateUtils::GetTemplate(model); + if (m_lidarModelName == templ.m_name) + { + m_lidarModel = model; + break; + } + } + m_lidarParameters = LidarTemplateUtils::GetTemplate(m_lidarModel); + } + bool LidarSensorConfiguration::IsConfigurationVisible() const { - return m_lidarModel == LidarTemplate::LidarModel::Custom3DLidar; + return m_lidarModel == LidarTemplate::LidarModel::Custom3DLidar || m_lidarModel == LidarTemplate::LidarModel::Custom2DLidar; } bool LidarSensorConfiguration::IsIgnoredLayerConfigurationVisible() const @@ -109,7 +142,7 @@ namespace ROS2 AZ::Crc32 LidarSensorConfiguration::OnLidarModelSelected() { - m_lidarParameters = LidarTemplateUtils::GetTemplate(m_lidarModel); + FetchLidarModelConfiguration(); UpdateShowNoise(); return AZ::Edit::PropertyRefreshLevels::EntireTree; } diff --git a/Gems/ROS2/Code/Source/Lidar/LidarSensorConfiguration.h b/Gems/ROS2/Code/Source/Lidar/LidarSensorConfiguration.h index dd68b3550..00b2fbf83 100644 --- a/Gems/ROS2/Code/Source/Lidar/LidarSensorConfiguration.h +++ b/Gems/ROS2/Code/Source/Lidar/LidarSensorConfiguration.h @@ -18,20 +18,20 @@ namespace ROS2 { - //! A structure capturing configuration of a lidar sensor (to be used with ROS2LidarSensorComponent). + //! A structure capturing configuration of a lidar sensor (to be used with LidarCore). class LidarSensorConfiguration { public: AZ_TYPE_INFO(LidarSensorConfiguration, "{e46e75f4-1e0e-48ca-a22f-43afc8f25101}"); static void Reflect(AZ::ReflectContext* context); - void FetchLidarImplementationFeatures(); + LidarSensorConfiguration(AZStd::vector availableModels = {}); LidarSystemFeatures m_lidarSystemFeatures; AZStd::string m_lidarSystem; - LidarTemplate::LidarModel m_lidarModel = LidarTemplate::LidarModel::Custom3DLidar; - LidarTemplate m_lidarParameters = LidarTemplateUtils::GetTemplate(LidarTemplate::LidarModel::Custom3DLidar); + LidarTemplate::LidarModel m_lidarModel = LidarTemplate::LidarModel::Custom2DLidar; + LidarTemplate m_lidarParameters = LidarTemplateUtils::GetTemplate(LidarTemplate::LidarModel::Custom2DLidar); AZStd::unordered_set m_ignoredCollisionLayers; AZStd::vector m_excludedEntities; @@ -44,10 +44,22 @@ namespace ROS2 bool IsEntityExclusionVisible() const; bool IsMaxPointsConfigurationVisible() const; + //! Update the lidar configuration based on the current lidar model selected. + void FetchLidarModelConfiguration(); + //! Update the lidar system features based on the current lidar system selected. + void FetchLidarImplementationFeatures(); + AZ::Crc32 OnLidarModelSelected(); AZ::Crc32 OnLidarImplementationSelected(); + + //! Get all models this configuration can be set to (for example all 2D lidar models). + AZStd::vector GetAvailableModels() const; + //! Get all available lidar systems. AZStd::vector FetchLidarSystemList(); + const AZStd::vector m_availableModels; + AZStd::string m_lidarModelName = "CustomLidar2D"; + void UpdateShowNoise(); }; } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Lidar/LidarTemplate.cpp b/Gems/ROS2/Code/Source/Lidar/LidarTemplate.cpp index 1445a2552..1bcd10f28 100644 --- a/Gems/ROS2/Code/Source/Lidar/LidarTemplate.cpp +++ b/Gems/ROS2/Code/Source/Lidar/LidarTemplate.cpp @@ -49,7 +49,7 @@ namespace ROS2 bool LidarTemplate::IsLayersVisible() const { - return m_model != LidarTemplate::LidarModel::Custom2DLidar; + return !m_is2D; } void LidarTemplate::Reflect(AZ::ReflectContext* context) @@ -89,12 +89,14 @@ namespace ROS2 ->Attribute(AZ::Edit::Attributes::Max, 180.0f) ->DataElement( AZ::Edit::UIHandlers::Default, &LidarTemplate::m_minVAngle, "Min vertical angle [Deg]", "Downwards reach of fov") - ->Attribute(AZ::Edit::Attributes::Min, -180.0f) - ->Attribute(AZ::Edit::Attributes::Max, 180.0f) + ->Attribute(AZ::Edit::Attributes::Min, -90.0f) + ->Attribute(AZ::Edit::Attributes::Max, 90.0f) + ->Attribute(AZ::Edit::Attributes::Visibility, &LidarTemplate::IsLayersVisible) ->DataElement( AZ::Edit::UIHandlers::Default, &LidarTemplate::m_maxVAngle, "Max vertical angle [Deg]", "Upwards reach of fov") - ->Attribute(AZ::Edit::Attributes::Min, -180.0f) - ->Attribute(AZ::Edit::Attributes::Max, 180.0f) + ->Attribute(AZ::Edit::Attributes::Min, -90.0f) + ->Attribute(AZ::Edit::Attributes::Max, 90.0f) + ->Attribute(AZ::Edit::Attributes::Visibility, &LidarTemplate::IsLayersVisible) ->DataElement(AZ::Edit::UIHandlers::Default, &LidarTemplate::m_minRange, "Min range", "Minimum beam range [m]") ->Attribute(AZ::Edit::Attributes::Min, 0.0f) ->Attribute(AZ::Edit::Attributes::Max, 1000.0f) diff --git a/Gems/ROS2/Code/Source/Lidar/LidarTemplate.h b/Gems/ROS2/Code/Source/Lidar/LidarTemplate.h index e249faa57..867d7df3d 100644 --- a/Gems/ROS2/Code/Source/Lidar/LidarTemplate.h +++ b/Gems/ROS2/Code/Source/Lidar/LidarTemplate.h @@ -54,6 +54,9 @@ namespace ROS2 //! Name of lidar template AZStd::string m_name; + //! Whether the template is for a 2D Lidar. + //! This causes vertical parameters of the Lidar to be unmodifiable (m_minVAngle, m_maxVAngle, m_layers). + bool m_is2D = false; //! Minimum horizontal angle (altitude of the ray), in degrees float m_minHAngle = 0.0f; //! Maximum horizontal angle (altitude of the ray), in degrees diff --git a/Gems/ROS2/Code/Source/Lidar/LidarTemplateUtils.cpp b/Gems/ROS2/Code/Source/Lidar/LidarTemplateUtils.cpp index c5c6d758d..af5823def 100644 --- a/Gems/ROS2/Code/Source/Lidar/LidarTemplateUtils.cpp +++ b/Gems/ROS2/Code/Source/Lidar/LidarTemplateUtils.cpp @@ -12,16 +12,17 @@ namespace ROS2 { - LidarTemplate LidarTemplateUtils::GetTemplate(LidarTemplate::LidarModel model) + namespace { using Model = LidarTemplate::LidarModel; - static const std::unordered_map templates = { + static const AZStd::map templates = { { Model::Custom3DLidar, { /*.m_model = */ Model::Custom3DLidar, /*.m_name = */ "CustomLidar", + /*.m_is2D = */ false, /*.m_minHAngle = */ -180.0f, /*.m_maxHAngle = */ 180.0f, /*.m_minVAngle = */ -35.0f, @@ -43,6 +44,7 @@ namespace ROS2 { /*.m_model = */ Model::Ouster_OS0_64, /*.m_name = */ "Ouster OS0-64", + /*.m_is2D = */ false, /*.m_minHAngle = */ -180.0f, /*.m_maxHAngle = */ 180.0f, /*.m_minVAngle = */ -45.0f, @@ -64,6 +66,7 @@ namespace ROS2 { /*.m_model = */ Model::Ouster_OS1_64, /*.m_name = */ "Ouster OS1-64", + /*.m_is2D = */ false, /*.m_minHAngle = */ -180.0f, /*.m_maxHAngle = */ 180.0f, /*.m_minVAngle = */ 22.5f, @@ -83,8 +86,9 @@ namespace ROS2 { Model::Ouster_OS2_64, { - /*.m_model = */ Model::Ouster_OS1_64, - /*.m_name = */ "Ouster OS1-64", + /*.m_model = */ Model::Ouster_OS2_64, + /*.m_name = */ "Ouster OS2-64", + /*.m_is2D = */ false, /*.m_minHAngle = */ -180.0f, /*.m_maxHAngle = */ 180.0f, /*.m_minVAngle = */ 11.25f, @@ -106,6 +110,7 @@ namespace ROS2 { /*.m_model = */ Model::Velodyne_Puck, /*.m_name = */ "Velodyne Puck (VLP-16)", + /*.m_is2D = */ false, /*.m_minHAngle = */ -180.0f, /*.m_maxHAngle = */ 180.0f, /*.m_minVAngle = */ 15.0f, @@ -127,6 +132,7 @@ namespace ROS2 { /*.m_model = */ Model::Velodyne_HDL_32E, /*.m_name = */ "Velodyne HDL-32E", + /*.m_is2D = */ false, /*.m_minHAngle = */ -180.0f, /*.m_maxHAngle = */ 180.0f, /*.m_minVAngle = */ 10.67f, @@ -148,6 +154,7 @@ namespace ROS2 { /*.m_model = */ Model::Custom2DLidar, /*.m_name = */ "CustomLidar2D", + /*.m_is2D = */ true, /*.m_minHAngle = */ -180.0f, /*.m_maxHAngle = */ 180.0f, /*.m_minVAngle = */ 0.f, @@ -169,6 +176,7 @@ namespace ROS2 { /*.m_model = */ Model::Slamtec_RPLIDAR_S1, /*.m_name = */ "Slamtec RPLIDAR S1", + /*.m_is2D = */ true, /*.m_minHAngle = */ -180.0f, /*.m_maxHAngle = */ 180.0f, /*.m_minVAngle = */ 0.f, @@ -186,7 +194,10 @@ namespace ROS2 }, } }; + } // namespace + LidarTemplate LidarTemplateUtils::GetTemplate(LidarTemplate::LidarModel model) + { auto it = templates.find(model); if (it == templates.end()) { @@ -196,6 +207,34 @@ namespace ROS2 return it->second; } + AZStd::vector LidarTemplateUtils::Get2DModels() + { + AZStd::vector result; + + for (const auto& it : templates) + { + if (it.second.m_is2D) + { + result.push_back(it.first); + } + } + return result; + } + + AZStd::vector LidarTemplateUtils::Get3DModels() + { + AZStd::vector result; + + for (const auto& it : templates) + { + if (!it.second.m_is2D) + { + result.push_back(it.first); + } + } + return result; + } + size_t LidarTemplateUtils::TotalPointCount(const LidarTemplate& t) { return t.m_layers * t.m_numberOfIncrements; diff --git a/Gems/ROS2/Code/Source/Lidar/LidarTemplateUtils.h b/Gems/ROS2/Code/Source/Lidar/LidarTemplateUtils.h index 54837fbff..440e35815 100644 --- a/Gems/ROS2/Code/Source/Lidar/LidarTemplateUtils.h +++ b/Gems/ROS2/Code/Source/Lidar/LidarTemplateUtils.h @@ -21,6 +21,14 @@ namespace ROS2 //! @return the matching template which describes parameters for the model. LidarTemplate GetTemplate(LidarTemplate::LidarModel model); + //! Get all 2D lidar models. + //! @return 2D lidar models. + AZStd::vector Get2DModels(); + + //! Get all 3D lidar models. + //! @return 3D lidar models. + AZStd::vector Get3DModels(); + //! Get total point count for a given template. //! @param t lidar template. //! @return total count of points that the lidar specified by the template would produce on each scan. diff --git a/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp b/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp index 06b75be59..835f94969 100644 --- a/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp @@ -17,19 +17,17 @@ namespace ROS2 { - namespace Internal + namespace { - const char* kLaserScanType = "sensor_msgs::msg::LaserScan"; + const char* LaserScanType = "sensor_msgs::msg::LaserScan"; } void ROS2Lidar2DSensorComponent::Reflect(AZ::ReflectContext* context) { - Lidar2DSensorConfiguration::Reflect(context); - if (auto* serialize = azrtti_cast(context)) { serialize->Class()->Version(2)->Field( - "lidarConfiguration", &ROS2Lidar2DSensorComponent::m_lidarConfiguration); + "lidarCore", &ROS2Lidar2DSensorComponent::m_lidarCore); if (AZ::EditContext* ec = serialize->GetEditContext()) { @@ -39,7 +37,7 @@ namespace ROS2 ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game")) ->DataElement( AZ::Edit::UIHandlers::ComboBox, - &ROS2Lidar2DSensorComponent::m_lidarConfiguration, + &ROS2Lidar2DSensorComponent::m_lidarCore, "Lidar configuration", "Lidar configuration") ->Attribute(AZ::Edit::Attributes::Visibility, AZ::Edit::PropertyVisibility::ShowChildrenOnly); @@ -47,132 +45,39 @@ namespace ROS2 } } - void ROS2Lidar2DSensorComponent::ConnectToLidarRaycaster() - { - if (auto raycasterId = m_implementationToRaycasterMap.find(m_lidarConfiguration.m_lidarSystem); - raycasterId != m_implementationToRaycasterMap.end()) - { - m_lidarRaycasterId = raycasterId->second; - return; - } - - m_lidarRaycasterId = LidarId::CreateNull(); - LidarSystemRequestBus::EventResult( - m_lidarRaycasterId, AZ_CRC(m_lidarConfiguration.m_lidarSystem), &LidarSystemRequestBus::Events::CreateLidar, GetEntityId()); - AZ_Assert(!m_lidarRaycasterId.IsNull(), "Could not access selected Lidar System."); - - m_implementationToRaycasterMap.emplace(m_lidarConfiguration.m_lidarSystem, m_lidarRaycasterId); - } - - void ROS2Lidar2DSensorComponent::ConfigureLidarRaycaster() - { - m_lidarConfiguration.FetchLidarImplementationFeatures(); - LidarRaycasterRequestBus::Event(m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRayOrientations, m_lastRotations); - LidarRaycasterRequestBus::Event( - m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRayRange, m_lidarConfiguration.m_lidarParameters.m_maxRange); - LidarRaycasterRequestBus::Event( - m_lidarRaycasterId, - &LidarRaycasterRequestBus::Events::ConfigureMinimumRayRange, - m_lidarConfiguration.m_lidarParameters.m_minRange); - - if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::Noise && - m_lidarConfiguration.m_lidarParameters.m_isNoiseEnabled) - { - LidarRaycasterRequestBus::Event( - m_lidarRaycasterId, - &LidarRaycasterRequestBus::Events::ConfigureNoiseParameters, - m_lidarConfiguration.m_lidarParameters.m_noiseParameters.m_angularNoiseStdDev, - m_lidarConfiguration.m_lidarParameters.m_noiseParameters.m_distanceNoiseStdDevBase, - m_lidarConfiguration.m_lidarParameters.m_noiseParameters.m_distanceNoiseStdDevRisePerMeter); - } - - RaycastResultFlags requestedFlags = RaycastResultFlags::Ranges; - if (m_sensorConfiguration.m_visualize) - { - requestedFlags |= RaycastResultFlags::Points; - } - LidarRaycasterRequestBus::Event(m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRaycastResultFlags, requestedFlags); - - if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::CollisionLayers) - { - LidarRaycasterRequestBus::Event( - m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureIgnoredCollisionLayers, m_lidarConfiguration.m_ignoredCollisionLayers); - } - - if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::EntityExclusion) - { - LidarRaycasterRequestBus::Event( - m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ExcludeEntities, m_lidarConfiguration.m_excludedEntities); - } - - if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::MaxRangePoints) - { - LidarRaycasterRequestBus::Event( - m_lidarRaycasterId, - &LidarRaycasterRequestBus::Events::ConfigureMaxRangePointAddition, - m_lidarConfiguration.m_addPointsAtMax); - } - } - ROS2Lidar2DSensorComponent::ROS2Lidar2DSensorComponent() + : m_lidarCore(LidarTemplateUtils::Get2DModels()) { TopicConfiguration ls; - AZStd::string type = Internal::kLaserScanType; + AZStd::string type = LaserScanType; ls.m_type = type; ls.m_topic = "ls"; - m_sensorConfiguration.m_frequency = 10; + m_sensorConfiguration.m_frequency = 10.f; m_sensorConfiguration.m_publishersConfigurations.insert(AZStd::make_pair(type, ls)); } - ROS2Lidar2DSensorComponent::ROS2Lidar2DSensorComponent( - const SensorConfiguration& sensorConfiguration, const Lidar2DSensorConfiguration& lidarConfiguration) - : m_lidarConfiguration(lidarConfiguration) + const SensorConfiguration& sensorConfiguration, const LidarSensorConfiguration& lidarConfiguration) + : m_lidarCore(lidarConfiguration) { m_sensorConfiguration = sensorConfiguration; } void ROS2Lidar2DSensorComponent::Visualize() { - if (m_visualizationPoints.empty()) - { - return; - } - - if (m_drawQueue) - { - const uint8_t pixelSize = 2; - AZ::RPI::AuxGeomDraw::AuxGeomDynamicDrawArguments drawArgs; - drawArgs.m_verts = m_visualizationPoints.data(); - drawArgs.m_vertCount = m_visualizationPoints.size(); - drawArgs.m_colors = &AZ::Colors::Red; - drawArgs.m_colorCount = 1; - drawArgs.m_opacityType = AZ::RPI::AuxGeomDraw::OpacityType::Opaque; - drawArgs.m_size = pixelSize; - m_drawQueue->DrawPoints(drawArgs); - } + m_lidarCore.VisualizeResults(); } void ROS2Lidar2DSensorComponent::Activate() { + m_lidarCore.Init(GetEntityId()); + auto ros2Node = ROS2Interface::Get()->GetNode(); AZ_Assert(m_sensorConfiguration.m_publishersConfigurations.size() == 1, "Invalid configuration of publishers for lidar sensor"); - const TopicConfiguration& publisherConfig = m_sensorConfiguration.m_publishersConfigurations[Internal::kLaserScanType]; + const TopicConfiguration& publisherConfig = m_sensorConfiguration.m_publishersConfigurations[LaserScanType]; AZStd::string fullTopic = ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic); m_laserScanPublisher = ros2Node->create_publisher(fullTopic.data(), publisherConfig.GetQoS()); - if (m_sensorConfiguration.m_visualize) - { - auto* entityScene = AZ::RPI::Scene::GetSceneForEntityId(GetEntityId()); - m_drawQueue = AZ::RPI::AuxGeomFeatureProcessorInterface::GetDrawQueueForScene(entityScene); - } - - m_lastRotations = LidarTemplateUtils::PopulateRayRotations(m_lidarConfiguration.m_lidarParameters); - - m_lidarConfiguration.FetchLidarImplementationFeatures(); - ConnectToLidarRaycaster(); - ConfigureLidarRaycaster(); - ROS2SensorComponent::Activate(); } @@ -180,45 +85,28 @@ namespace ROS2 { ROS2SensorComponent::Deactivate(); m_laserScanPublisher.reset(); + m_lidarCore.Deinit(); } void ROS2Lidar2DSensorComponent::FrequencyTick() { - auto entityTransform = GetEntity()->FindComponent(); - - RaycastResultFlags requestedFlags = RaycastResultFlags::Ranges; - if (m_sensorConfiguration.m_visualize) - { - requestedFlags |= RaycastResultFlags::Points; - } - LidarRaycasterRequestBus::EventResult( - m_lastScanResults, m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::PerformRaycast, entityTransform->GetWorldTM()); - if (m_lastScanResults.m_ranges.empty()) - { - AZ_TracePrintf("Lidar Sensor Component", "No results from raycast\n"); - return; - } - - if (m_sensorConfiguration.m_visualize) - { // Store points for visualization purposes, in global frame - m_visualizationPoints = m_lastScanResults.m_points; - } + RaycastResult lastScanResults = m_lidarCore.PerformRaycast(); auto* ros2Frame = Utils::GetGameOrEditorComponent(GetEntity()); auto message = sensor_msgs::msg::LaserScan(); message.header.frame_id = ros2Frame->GetFrameID().data(); message.header.stamp = ROS2Interface::Get()->GetROSTimestamp(); - message.angle_min = AZ::DegToRad(m_lidarConfiguration.m_lidarParameters.m_minHAngle); - message.angle_max = AZ::DegToRad(m_lidarConfiguration.m_lidarParameters.m_maxHAngle); - message.angle_increment = - (message.angle_max - message.angle_min) / aznumeric_cast(m_lidarConfiguration.m_lidarParameters.m_numberOfIncrements); + message.angle_min = AZ::DegToRad(m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_minHAngle); + message.angle_max = AZ::DegToRad(m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_maxHAngle); + message.angle_increment = (message.angle_max - message.angle_min) / + aznumeric_cast(m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_numberOfIncrements); - message.range_min = m_lidarConfiguration.m_lidarParameters.m_minRange; - message.range_max = m_lidarConfiguration.m_lidarParameters.m_maxRange; + message.range_min = m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_minRange; + message.range_max = m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_maxRange; message.scan_time = 1.f / m_sensorConfiguration.m_frequency; message.time_increment = 0.0f; - message.ranges.assign(m_lastScanResults.m_ranges.begin(), m_lastScanResults.m_ranges.end()); + message.ranges.assign(lastScanResults.m_ranges.begin(), lastScanResults.m_ranges.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 97b954201..31de80487 100644 --- a/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.h +++ b/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.h @@ -15,7 +15,7 @@ #include #include -#include "Lidar2DSensorConfiguration.h" +#include "LidarCore.h" #include "LidarRaycaster.h" namespace ROS2 @@ -29,7 +29,7 @@ namespace ROS2 public: AZ_COMPONENT(ROS2Lidar2DSensorComponent, "{F4C2D970-1D69-40F2-9D4D-B52DCFDD2704}", ROS2SensorComponent); ROS2Lidar2DSensorComponent(); - ROS2Lidar2DSensorComponent(const SensorConfiguration& sensorConfiguration, const Lidar2DSensorConfiguration& lidarConfiguration); + ROS2Lidar2DSensorComponent(const SensorConfiguration& sensorConfiguration, const LidarSensorConfiguration& lidarConfiguration); ~ROS2Lidar2DSensorComponent() = default; static void Reflect(AZ::ReflectContext* context); ////////////////////////////////////////////////////////////////////////// @@ -44,21 +44,8 @@ namespace ROS2 void FrequencyTick() override; void Visualize() override; - void ConnectToLidarRaycaster(); - void ConfigureLidarRaycaster(); - - // A structure that maps each lidar implementation busId to the busId of a raycaster created by this LidarSensorComponent. - AZStd::unordered_map m_implementationToRaycasterMap; - LidarId m_lidarRaycasterId; std::shared_ptr> m_laserScanPublisher; - // Used only when visualization is on - points differ since they are in global transform as opposed to local - AZStd::vector m_visualizationPoints; - AZ::RPI::AuxGeomDrawPtr m_drawQueue; - - Lidar2DSensorConfiguration m_lidarConfiguration; - - AZStd::vector m_lastRotations; - RaycastResult m_lastScanResults; + LidarCore m_lidarCore; }; } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp index ca04e4309..641bc0d0c 100644 --- a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp @@ -17,19 +17,17 @@ namespace ROS2 { - namespace Internal + namespace { - const char* kPointCloudType = "sensor_msgs::msg::PointCloud2"; + const char* PointCloudType = "sensor_msgs::msg::PointCloud2"; } void ROS2LidarSensorComponent::Reflect(AZ::ReflectContext* context) { - LidarSensorConfiguration::Reflect(context); - if (AZ::SerializeContext* serialize = azrtti_cast(context)) { serialize->Class()->Version(2)->Field( - "lidarConfiguration", &ROS2LidarSensorComponent::m_lidarConfiguration); + "lidarCore", &ROS2LidarSensorComponent::m_lidarCore); if (AZ::EditContext* ec = serialize->GetEditContext()) { @@ -39,7 +37,7 @@ namespace ROS2 ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game")) ->DataElement( AZ::Edit::UIHandlers::ComboBox, - &ROS2LidarSensorComponent::m_lidarConfiguration, + &ROS2LidarSensorComponent::m_lidarCore, "Lidar configuration", "Lidar configuration") ->Attribute(AZ::Edit::Attributes::Visibility, AZ::Edit::PropertyVisibility::ShowChildrenOnly); @@ -47,146 +45,59 @@ namespace ROS2 } } - void ROS2LidarSensorComponent::ConnectToLidarRaycaster() - { - if (auto raycasterId = m_implementationToRaycasterMap.find(m_lidarConfiguration.m_lidarSystem); - raycasterId != m_implementationToRaycasterMap.end()) - { - m_lidarRaycasterId = raycasterId->second; - return; - } - - m_lidarRaycasterId = LidarId::CreateNull(); - LidarSystemRequestBus::EventResult( - m_lidarRaycasterId, AZ_CRC(m_lidarConfiguration.m_lidarSystem), &LidarSystemRequestBus::Events::CreateLidar, GetEntityId()); - AZ_Assert(!m_lidarRaycasterId.IsNull(), "Could not access selected Lidar System."); - - m_implementationToRaycasterMap.emplace(m_lidarConfiguration.m_lidarSystem, m_lidarRaycasterId); - } - - void ROS2LidarSensorComponent::ConfigureLidarRaycaster() - { - m_lidarConfiguration.FetchLidarImplementationFeatures(); - LidarRaycasterRequestBus::Event(m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRayOrientations, m_lastRotations); - LidarRaycasterRequestBus::Event( - m_lidarRaycasterId, - &LidarRaycasterRequestBus::Events::ConfigureMinimumRayRange, - m_lidarConfiguration.m_lidarParameters.m_minRange); - LidarRaycasterRequestBus::Event( - m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRayRange, m_lidarConfiguration.m_lidarParameters.m_maxRange); - LidarRaycasterRequestBus::Event( - m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRaycastResultFlags, RaycastResultFlags::Points); - - if ((m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::Noise) && - m_lidarConfiguration.m_lidarParameters.m_isNoiseEnabled) - { - LidarRaycasterRequestBus::Event( - m_lidarRaycasterId, - &LidarRaycasterRequestBus::Events::ConfigureNoiseParameters, - m_lidarConfiguration.m_lidarParameters.m_noiseParameters.m_angularNoiseStdDev, - m_lidarConfiguration.m_lidarParameters.m_noiseParameters.m_distanceNoiseStdDevBase, - m_lidarConfiguration.m_lidarParameters.m_noiseParameters.m_distanceNoiseStdDevRisePerMeter); - } - - if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::CollisionLayers) - { - LidarRaycasterRequestBus::Event( - m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureIgnoredCollisionLayers, m_lidarConfiguration.m_ignoredCollisionLayers); - } - - if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::EntityExclusion) - { - LidarRaycasterRequestBus::Event( - m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ExcludeEntities, m_lidarConfiguration.m_excludedEntities); - } - - if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::MaxRangePoints) - { - LidarRaycasterRequestBus::Event( - m_lidarRaycasterId, - &LidarRaycasterRequestBus::Events::ConfigureMaxRangePointAddition, - m_lidarConfiguration.m_addPointsAtMax); - } - - if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::PointcloudPublishing) - { - const TopicConfiguration& publisherConfig = m_sensorConfiguration.m_publishersConfigurations[Internal::kPointCloudType]; - auto* ros2Frame = Utils::GetGameOrEditorComponent(GetEntity()); - - LidarRaycasterRequestBus::Event( - m_lidarRaycasterId, - &LidarRaycasterRequestBus::Events::ConfigurePointCloudPublisher, - ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic), - ros2Frame->GetFrameID().data(), - publisherConfig.GetQoS()); - } - } - ROS2LidarSensorComponent::ROS2LidarSensorComponent() + : m_lidarCore(LidarTemplateUtils::Get3DModels()) { TopicConfiguration pc; - AZStd::string type = Internal::kPointCloudType; + AZStd::string type = PointCloudType; pc.m_type = type; pc.m_topic = "pc"; - m_sensorConfiguration.m_frequency = 10; + m_sensorConfiguration.m_frequency = 10.f; m_sensorConfiguration.m_publishersConfigurations.insert(AZStd::make_pair(type, pc)); } ROS2LidarSensorComponent::ROS2LidarSensorComponent( const SensorConfiguration& sensorConfiguration, const LidarSensorConfiguration& lidarConfiguration) - : m_lidarConfiguration(lidarConfiguration) + : m_lidarCore(lidarConfiguration) { m_sensorConfiguration = sensorConfiguration; } void ROS2LidarSensorComponent::Visualize() { - if (m_visualizationPoints.empty()) - { - return; - } - - if (m_drawQueue) - { - const uint8_t pixelSize = 2; - AZ::RPI::AuxGeomDraw::AuxGeomDynamicDrawArguments drawArgs; - drawArgs.m_verts = m_visualizationPoints.data(); - drawArgs.m_vertCount = m_visualizationPoints.size(); - drawArgs.m_colors = &AZ::Colors::Red; - drawArgs.m_colorCount = 1; - drawArgs.m_opacityType = AZ::RPI::AuxGeomDraw::OpacityType::Opaque; - drawArgs.m_size = pixelSize; - m_drawQueue->DrawPoints(drawArgs); - } + m_lidarCore.VisualizeResults(); } void ROS2LidarSensorComponent::Activate() { - if (m_sensorConfiguration.m_visualize) - { - auto* entityScene = AZ::RPI::Scene::GetSceneForEntityId(GetEntityId()); - m_drawQueue = AZ::RPI::AuxGeomFeatureProcessorInterface::GetDrawQueueForScene(entityScene); - } - - m_lastRotations = LidarTemplateUtils::PopulateRayRotations(m_lidarConfiguration.m_lidarParameters); - - m_lidarConfiguration.FetchLidarImplementationFeatures(); - ConnectToLidarRaycaster(); - ConfigureLidarRaycaster(); + m_lidarCore.Init(GetEntityId()); + m_lidarRaycasterId = m_lidarCore.GetLidarRaycasterId(); m_canRaycasterPublish = false; - if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::PointcloudPublishing) + if (m_lidarCore.m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::PointcloudPublishing) { LidarRaycasterRequestBus::EventResult( m_canRaycasterPublish, m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::CanHandlePublishing); } - if (!m_canRaycasterPublish) + if (m_canRaycasterPublish) + { + const TopicConfiguration& publisherConfig = m_sensorConfiguration.m_publishersConfigurations[PointCloudType]; + auto* ros2Frame = Utils::GetGameOrEditorComponent(GetEntity()); + + LidarRaycasterRequestBus::Event( + m_lidarRaycasterId, + &LidarRaycasterRequestBus::Events::ConfigurePointCloudPublisher, + ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic), + ros2Frame->GetFrameID().data(), + publisherConfig.GetQoS()); + } + else { auto ros2Node = ROS2Interface::Get()->GetNode(); AZ_Assert(m_sensorConfiguration.m_publishersConfigurations.size() == 1, "Invalid configuration of publishers for lidar sensor"); - const TopicConfiguration& publisherConfig = m_sensorConfiguration.m_publishersConfigurations[Internal::kPointCloudType]; + const TopicConfiguration& publisherConfig = m_sensorConfiguration.m_publishersConfigurations[PointCloudType]; AZStd::string fullTopic = ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic); m_pointCloudPublisher = ros2Node->create_publisher(fullTopic.data(), publisherConfig.GetQoS()); } @@ -198,13 +109,7 @@ namespace ROS2 { ROS2SensorComponent::Deactivate(); m_pointCloudPublisher.reset(); - - for (auto& [implementation, raycasterId] : m_implementationToRaycasterMap) - { - LidarSystemRequestBus::Event(AZ_CRC(implementation), &LidarSystemRequestBus::Events::DestroyLidar, raycasterId); - } - - m_implementationToRaycasterMap.clear(); + m_lidarCore.Deinit(); } void ROS2LidarSensorComponent::FrequencyTick() @@ -220,18 +125,7 @@ namespace ROS2 aznumeric_cast(timestamp.sec) * aznumeric_cast(1.0e9f) + timestamp.nanosec); } - LidarRaycasterRequestBus::EventResult( - m_lastScanResults, m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::PerformRaycast, entityTransform->GetWorldTM()); - if (m_lastScanResults.m_points.empty()) - { - AZ_TracePrintf("Lidar Sensor Component", "No results from raycast\n"); - return; - } - - if (m_sensorConfiguration.m_visualize) - { // Store points for visualization purposes, in global frame - m_visualizationPoints = m_lastScanResults.m_points; - } + auto lastScanResults = m_lidarCore.PerformRaycast(); if (m_canRaycasterPublish) { // Skip publishing when it can be handled by the raycaster. @@ -239,7 +133,7 @@ namespace ROS2 } const auto inverseLidarTM = entityTransform->GetWorldTM().GetInverse(); - for (auto& point : m_lastScanResults.m_points) + for (auto& point : lastScanResults.m_points) { point = inverseLidarTM.TransformPoint(point); } @@ -249,7 +143,7 @@ namespace ROS2 message.header.frame_id = ros2Frame->GetFrameID().data(); message.header.stamp = ROS2Interface::Get()->GetROSTimestamp(); message.height = 1; - message.width = m_lastScanResults.m_points.size(); + message.width = lastScanResults.m_points.size(); message.point_step = sizeof(AZ::Vector3); message.row_step = message.width * message.point_step; @@ -264,10 +158,10 @@ namespace ROS2 message.fields.push_back(pf); } - size_t sizeInBytes = m_lastScanResults.m_points.size() * sizeof(AZ::Vector3); + size_t sizeInBytes = lastScanResults.m_points.size() * sizeof(AZ::Vector3); message.data.resize(sizeInBytes); AZ_Assert(message.row_step * message.height == sizeInBytes, "Inconsistency in the size of point cloud data"); - memcpy(message.data.data(), m_lastScanResults.m_points.data(), sizeInBytes); + memcpy(message.data.data(), lastScanResults.m_points.data(), sizeInBytes); m_pointCloudPublisher->publish(message); } } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.h b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.h index f441667e2..33edb0ce3 100644 --- a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.h +++ b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.h @@ -15,6 +15,7 @@ #include #include +#include "LidarCore.h" #include "LidarRaycaster.h" #include "LidarSensorConfiguration.h" @@ -44,22 +45,11 @@ namespace ROS2 void FrequencyTick() override; void Visualize() override; - void ConnectToLidarRaycaster(); - void ConfigureLidarRaycaster(); - - // A structure that maps each lidar implementation busId to the busId of a raycaster created by this LidarSensorComponent. - AZStd::unordered_map m_implementationToRaycasterMap; bool m_canRaycasterPublish = false; - LidarId m_lidarRaycasterId; std::shared_ptr> m_pointCloudPublisher; - // Used only when visualization is on - points differ since they are in global transform as opposed to local - AZStd::vector m_visualizationPoints; - AZ::RPI::AuxGeomDrawPtr m_drawQueue; + LidarCore m_lidarCore; - LidarSensorConfiguration m_lidarConfiguration; - - AZStd::vector m_lastRotations; - RaycastResult m_lastScanResults; + LidarId m_lidarRaycasterId; }; } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/ROS2SystemComponent.cpp b/Gems/ROS2/Code/Source/ROS2SystemComponent.cpp index 546b4f478..d040b253c 100644 --- a/Gems/ROS2/Code/Source/ROS2SystemComponent.cpp +++ b/Gems/ROS2/Code/Source/ROS2SystemComponent.cpp @@ -5,6 +5,7 @@ * SPDX-License-Identifier: Apache-2.0 OR MIT * */ +#include #include #include #include @@ -34,6 +35,7 @@ namespace ROS2 QoS::Reflect(context); TopicConfiguration::Reflect(context); PublisherConfiguration::Reflect(context); + LidarCore::Reflect(context); VehicleDynamics::VehicleModelComponent::Reflect(context); ROS2::Controllers::PidConfiguration::Reflect(context); if (AZ::SerializeContext* serialize = azrtti_cast(context)) diff --git a/Gems/ROS2/Code/Source/Sensor/ROS2SensorComponent.cpp b/Gems/ROS2/Code/Source/Sensor/ROS2SensorComponent.cpp index 861441bad..b9cb44b81 100644 --- a/Gems/ROS2/Code/Source/Sensor/ROS2SensorComponent.cpp +++ b/Gems/ROS2/Code/Source/Sensor/ROS2SensorComponent.cpp @@ -70,7 +70,10 @@ namespace ROS2 void ROS2SensorComponent::OnTick([[maybe_unused]] float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time) { - Visualize(); // each frame + if (m_sensorConfiguration.m_visualize) + { + Visualize(); // each frame + } if (m_onTickCall) { m_onTickCall(); diff --git a/Gems/ROS2/Code/ros2_files.cmake b/Gems/ROS2/Code/ros2_files.cmake index 2aa385604..642676b33 100644 --- a/Gems/ROS2/Code/ros2_files.cmake +++ b/Gems/ROS2/Code/ros2_files.cmake @@ -54,8 +54,6 @@ set(FILES Source/Lidar/LidarRaycaster.h Source/Lidar/LidarRegistrarSystemComponent.cpp Source/Lidar/LidarRegistrarSystemComponent.h - Source/Lidar/Lidar2DSensorConfiguration.cpp - Source/Lidar/Lidar2DSensorConfiguration.h Source/Lidar/LidarSensorConfiguration.cpp Source/Lidar/LidarSensorConfiguration.h Source/Lidar/LidarSystem.cpp @@ -64,6 +62,8 @@ set(FILES Source/Lidar/LidarTemplate.h Source/Lidar/LidarTemplateUtils.cpp Source/Lidar/LidarTemplateUtils.h + Source/Lidar/LidarCore.cpp + Source/Lidar/LidarCore.h Source/Lidar/ROS2Lidar2DSensorComponent.cpp Source/Lidar/ROS2Lidar2DSensorComponent.h Source/Lidar/ROS2LidarSensorComponent.cpp