From efa6ccef968865d0cd71555a283ffe1f4da8e9cf Mon Sep 17 00:00:00 2001 From: Jan Hanca Date: Wed, 26 Jul 2023 15:22:36 +0200 Subject: [PATCH 1/7] pass config to ROS2CameraSensorEditorComponent Signed-off-by: Jan Hanca --- .../ROS2CameraSensorEditorComponent.cpp | 29 ++++++++++++++----- .../Camera/ROS2CameraSensorEditorComponent.h | 6 +++- 2 files changed, 26 insertions(+), 9 deletions(-) diff --git a/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.cpp b/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.cpp index 5796f12b9..ed0842591 100644 --- a/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.cpp +++ b/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.cpp @@ -18,14 +18,15 @@ namespace ROS2 ROS2CameraSensorEditorComponent::ROS2CameraSensorEditorComponent() { m_sensorConfiguration.m_frequency = 10; - m_sensorConfiguration.m_publishersConfigurations.insert( - MakeTopicConfigurationPair("camera_image_color", CameraConstants::ImageMessageType, CameraConstants::ColorImageConfig)); - m_sensorConfiguration.m_publishersConfigurations.insert( - MakeTopicConfigurationPair("camera_image_depth", CameraConstants::ImageMessageType, CameraConstants::DepthImageConfig)); - m_sensorConfiguration.m_publishersConfigurations.insert( - MakeTopicConfigurationPair("color_camera_info", CameraConstants::CameraInfoMessageType, CameraConstants::ColorInfoConfig)); - m_sensorConfiguration.m_publishersConfigurations.insert( - MakeTopicConfigurationPair("depth_camera_info", CameraConstants::CameraInfoMessageType, CameraConstants::DepthInfoConfig)); + InitializeSensorTopics(); + } + + ROS2CameraSensorEditorComponent::ROS2CameraSensorEditorComponent( + const SensorConfiguration& sensorConfiguration, const CameraSensorConfiguration& cameraConfiguration) + : m_sensorConfiguration(sensorConfiguration) + , m_cameraSensorConfiguration(cameraConfiguration) + { + InitializeSensorTopics(); } void ROS2CameraSensorEditorComponent::Reflect(AZ::ReflectContext* context) @@ -203,4 +204,16 @@ namespace ROS2 return AZStd::make_pair(configName, config); } + void ROS2CameraSensorEditorComponent::InitializeSensorTopics() + { + m_sensorConfiguration.m_publishersConfigurations.insert( + MakeTopicConfigurationPair("camera_image_color", CameraConstants::ImageMessageType, CameraConstants::ColorImageConfig)); + m_sensorConfiguration.m_publishersConfigurations.insert( + MakeTopicConfigurationPair("camera_image_depth", CameraConstants::ImageMessageType, CameraConstants::DepthImageConfig)); + m_sensorConfiguration.m_publishersConfigurations.insert( + MakeTopicConfigurationPair("color_camera_info", CameraConstants::CameraInfoMessageType, CameraConstants::ColorInfoConfig)); + m_sensorConfiguration.m_publishersConfigurations.insert( + MakeTopicConfigurationPair("depth_camera_info", CameraConstants::CameraInfoMessageType, CameraConstants::DepthInfoConfig)); + } + } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.h b/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.h index d8f6a73b5..5bc80ba04 100644 --- a/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.h +++ b/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.h @@ -22,7 +22,7 @@ namespace ROS2 { //! ROS2 Camera Editor sensor component class //! Allows turning an entity into a camera sensor in Editor - //! Component draws camera frustrum in the Editor + //! Component draws camera frustum in the Editor class ROS2CameraSensorEditorComponent : public AzToolsFramework::Components::EditorComponentBase , public CameraCalibrationRequestBus::Handler @@ -30,6 +30,8 @@ namespace ROS2 { public: ROS2CameraSensorEditorComponent(); + ROS2CameraSensorEditorComponent( + const SensorConfiguration& sensorConfiguration, const CameraSensorConfiguration& cameraConfiguration); ~ROS2CameraSensorEditorComponent() override = default; AZ_EDITOR_COMPONENT(ROS2CameraSensorEditorComponent, "{3C2A86B2-AD58-4BF1-A5EF-71E0F94A2B42}"); static void Reflect(AZ::ReflectContext* context); @@ -55,6 +57,8 @@ namespace ROS2 AZStd::pair MakeTopicConfigurationPair( const AZStd::string& topic, const AZStd::string& messageType, const AZStd::string& configName) const; + void InitializeSensorTopics(); + SensorConfiguration m_sensorConfiguration; CameraSensorConfiguration m_cameraSensorConfiguration; }; From dd8f91e7ed69b64bba19aa8f14fc6024f595768d Mon Sep 17 00:00:00 2001 From: Jan Hanca Date: Wed, 26 Jul 2023 16:21:01 +0200 Subject: [PATCH 2/7] pass config to ROS2GNSSSensorComponent Signed-off-by: Jan Hanca --- .../ROS2CameraSensorEditorComponent.cpp | 22 ++++----- .../Source/GNSS/GNSSSensorConfiguration.cpp | 47 +++++++++++++++++++ .../Source/GNSS/GNSSSensorConfiguration.h | 26 ++++++++++ .../Source/GNSS/ROS2GNSSSensorComponent.cpp | 37 +++++++-------- .../Source/GNSS/ROS2GNSSSensorComponent.h | 7 +-- Gems/ROS2/Code/ros2_files.cmake | 2 + 6 files changed, 104 insertions(+), 37 deletions(-) create mode 100644 Gems/ROS2/Code/Source/GNSS/GNSSSensorConfiguration.cpp create mode 100644 Gems/ROS2/Code/Source/GNSS/GNSSSensorConfiguration.h diff --git a/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.cpp b/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.cpp index ed0842591..edee40e15 100644 --- a/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.cpp +++ b/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.cpp @@ -18,7 +18,14 @@ namespace ROS2 ROS2CameraSensorEditorComponent::ROS2CameraSensorEditorComponent() { m_sensorConfiguration.m_frequency = 10; - InitializeSensorTopics(); + m_sensorConfiguration.m_publishersConfigurations.insert( + MakeTopicConfigurationPair("camera_image_color", CameraConstants::ImageMessageType, CameraConstants::ColorImageConfig)); + m_sensorConfiguration.m_publishersConfigurations.insert( + MakeTopicConfigurationPair("camera_image_depth", CameraConstants::ImageMessageType, CameraConstants::DepthImageConfig)); + m_sensorConfiguration.m_publishersConfigurations.insert( + MakeTopicConfigurationPair("color_camera_info", CameraConstants::CameraInfoMessageType, CameraConstants::ColorInfoConfig)); + m_sensorConfiguration.m_publishersConfigurations.insert( + MakeTopicConfigurationPair("depth_camera_info", CameraConstants::CameraInfoMessageType, CameraConstants::DepthInfoConfig)); } ROS2CameraSensorEditorComponent::ROS2CameraSensorEditorComponent( @@ -26,7 +33,6 @@ namespace ROS2 : m_sensorConfiguration(sensorConfiguration) , m_cameraSensorConfiguration(cameraConfiguration) { - InitializeSensorTopics(); } void ROS2CameraSensorEditorComponent::Reflect(AZ::ReflectContext* context) @@ -204,16 +210,4 @@ namespace ROS2 return AZStd::make_pair(configName, config); } - void ROS2CameraSensorEditorComponent::InitializeSensorTopics() - { - m_sensorConfiguration.m_publishersConfigurations.insert( - MakeTopicConfigurationPair("camera_image_color", CameraConstants::ImageMessageType, CameraConstants::ColorImageConfig)); - m_sensorConfiguration.m_publishersConfigurations.insert( - MakeTopicConfigurationPair("camera_image_depth", CameraConstants::ImageMessageType, CameraConstants::DepthImageConfig)); - m_sensorConfiguration.m_publishersConfigurations.insert( - MakeTopicConfigurationPair("color_camera_info", CameraConstants::CameraInfoMessageType, CameraConstants::ColorInfoConfig)); - m_sensorConfiguration.m_publishersConfigurations.insert( - MakeTopicConfigurationPair("depth_camera_info", CameraConstants::CameraInfoMessageType, CameraConstants::DepthInfoConfig)); - } - } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/GNSS/GNSSSensorConfiguration.cpp b/Gems/ROS2/Code/Source/GNSS/GNSSSensorConfiguration.cpp new file mode 100644 index 000000000..fa518d3a4 --- /dev/null +++ b/Gems/ROS2/Code/Source/GNSS/GNSSSensorConfiguration.cpp @@ -0,0 +1,47 @@ +/* + * 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 "GNSSSensorConfiguration.h" +#include +#include + +namespace ROS2 +{ + void GNSSSensorConfiguration::Reflect(AZ::ReflectContext* context) + { + if (AZ::SerializeContext* serialize = azrtti_cast(context)) + { + serialize->Class() + ->Version(1) + ->Field("originLatitude", &GNSSSensorConfiguration::m_originLatitudeDeg) + ->Field("originLongitude", &GNSSSensorConfiguration::m_originLongitudeDeg) + ->Field("originAltitude", &GNSSSensorConfiguration::m_originAltitude); + + if (AZ::EditContext* ec = serialize->GetEditContext()) + { + ec->Class("ROS2 GNSS Sensor", "GNSS sensor component") + ->DataElement( + AZ::Edit::UIHandlers::Default, + &GNSSSensorConfiguration::m_originLatitudeDeg, + "Latitude offset", + "GNSS latitude position offset in degrees") + ->DataElement( + AZ::Edit::UIHandlers::Default, + &GNSSSensorConfiguration::m_originLongitudeDeg, + "Longitude offset", + "GNSS longitude position offset in degrees") + ->DataElement( + AZ::Edit::UIHandlers::Default, + &GNSSSensorConfiguration::m_originAltitude, + "Altitude offset", + "GNSS altitude position offset in meters"); + } + } + } + +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/GNSS/GNSSSensorConfiguration.h b/Gems/ROS2/Code/Source/GNSS/GNSSSensorConfiguration.h new file mode 100644 index 000000000..f8fd2dc9f --- /dev/null +++ b/Gems/ROS2/Code/Source/GNSS/GNSSSensorConfiguration.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 +#include + +namespace ROS2 +{ + //! A structure capturing configuration of a Global Navigation Satellite Systems (GNSS) sensor. + struct GNSSSensorConfiguration + { + AZ_TYPE_INFO(GNSSSensorConfiguration, "{8bc39c6b-2f2c-4612-bcc7-0cc7033001d4}"); + static void Reflect(AZ::ReflectContext* context); + + float m_originLatitudeDeg = 0.0f; + float m_originLongitudeDeg = 0.0f; + float m_originAltitude = 0.0f; + }; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp b/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp index 74c8f4904..3ab82b8f6 100644 --- a/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp @@ -28,13 +28,12 @@ namespace ROS2 void ROS2GNSSSensorComponent::Reflect(AZ::ReflectContext* context) { + GNSSSensorConfiguration::Reflect(context); + if (AZ::SerializeContext* serialize = azrtti_cast(context)) { - serialize->Class() - ->Version(1) - ->Field("gnssOriginLatitude", &ROS2GNSSSensorComponent::m_gnssOriginLatitudeDeg) - ->Field("gnssOriginLongitude", &ROS2GNSSSensorComponent::m_gnssOriginLongitudeDeg) - ->Field("gnssOriginAltitude", &ROS2GNSSSensorComponent::m_gnssOriginAltitude); + serialize->Class()->Version(2)->Field( + "gnssOriginLatitude", &ROS2GNSSSensorComponent::m_gnssConfiguration); if (AZ::EditContext* ec = serialize->GetEditContext()) { @@ -44,19 +43,9 @@ namespace ROS2 ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game")) ->DataElement( AZ::Edit::UIHandlers::Default, - &ROS2GNSSSensorComponent::m_gnssOriginLatitudeDeg, - "Latitude offset", - "GNSS latitude position offset in degrees") - ->DataElement( - AZ::Edit::UIHandlers::Default, - &ROS2GNSSSensorComponent::m_gnssOriginLongitudeDeg, - "Longitude offset", - "GNSS longitude position offset in degrees") - ->DataElement( - AZ::Edit::UIHandlers::Default, - &ROS2GNSSSensorComponent::m_gnssOriginAltitude, - "Altitude offset", - "GNSS altitude position offset in meters"); + &ROS2GNSSSensorComponent::m_gnssConfiguration, + "GNSS sensor configuration", + "GNSS sensor configuration"); } } } @@ -70,6 +59,13 @@ namespace ROS2 m_sensorConfiguration.m_publishersConfigurations.insert(AZStd::make_pair(Internal::kGNSSMsgType, pc)); } + ROS2GNSSSensorComponent::ROS2GNSSSensorComponent( + const SensorConfiguration& sensorConfiguration, const GNSSSensorConfiguration& gnssConfiguration) + : m_gnssConfiguration(gnssConfiguration) + { + m_sensorConfiguration = sensorConfiguration; + } + void ROS2GNSSSensorComponent::Activate() { ROS2SensorComponent::Activate(); @@ -92,8 +88,9 @@ namespace ROS2 void ROS2GNSSSensorComponent::FrequencyTick() { const AZ::Vector3 currentPosition = GetCurrentPose().GetTranslation(); - const AZ::Vector3 currentPositionECEF = - GNSS::ENUToECEF({ m_gnssOriginLatitudeDeg, m_gnssOriginLongitudeDeg, m_gnssOriginAltitude }, currentPosition); + const AZ::Vector3 currentPositionECEF = GNSS::ENUToECEF( + { m_gnssConfiguration.m_originLatitudeDeg, m_gnssConfiguration.m_originLongitudeDeg, m_gnssConfiguration.m_originAltitude }, + currentPosition); const AZ::Vector3 currentPositionWGS84 = GNSS::ECEFToWGS84(currentPositionECEF); m_gnssMsg.latitude = currentPositionWGS84.GetX(); diff --git a/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.h b/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.h index ada69e45d..d13b6e0bd 100644 --- a/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.h +++ b/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.h @@ -13,6 +13,8 @@ #include #include +#include "GNSSSensorConfiguration.h" + namespace ROS2 { //! Global Navigation Satellite Systems (GNSS) sensor component class @@ -24,6 +26,7 @@ namespace ROS2 public: AZ_COMPONENT(ROS2GNSSSensorComponent, "{55B4A299-7FA3-496A-88F0-764C75B0E9A7}", ROS2SensorComponent); ROS2GNSSSensorComponent(); + ROS2GNSSSensorComponent(const SensorConfiguration& sensorConfiguration, const GNSSSensorConfiguration& gnssConfiguration); ~ROS2GNSSSensorComponent() = default; static void Reflect(AZ::ReflectContext* context); ////////////////////////////////////////////////////////////////////////// @@ -33,9 +36,7 @@ namespace ROS2 ////////////////////////////////////////////////////////////////////////// private: - float m_gnssOriginLatitudeDeg = 0.0f; - float m_gnssOriginLongitudeDeg = 0.0f; - float m_gnssOriginAltitude = 0.0f; + GNSSSensorConfiguration m_gnssConfiguration; ////////////////////////////////////////////////////////////////////////// // ROS2SensorComponent overrides diff --git a/Gems/ROS2/Code/ros2_files.cmake b/Gems/ROS2/Code/ros2_files.cmake index ee6cfd0db..727379ee3 100644 --- a/Gems/ROS2/Code/ros2_files.cmake +++ b/Gems/ROS2/Code/ros2_files.cmake @@ -42,6 +42,8 @@ set(FILES Source/Gripper/VacuumGripperComponent.cpp Source/GNSS/GNSSFormatConversions.cpp Source/GNSS/GNSSFormatConversions.h + Source/GNSS/GNSSSensorConfiguration.cpp + Source/GNSS/GNSSSensorConfiguration.h Source/GNSS/ROS2GNSSSensorComponent.cpp Source/GNSS/ROS2GNSSSensorComponent.h Source/Imu/ROS2ImuSensorComponent.cpp From 0b20104097ab4390f4464c38c8c8ee938192c805 Mon Sep 17 00:00:00 2001 From: Jan Hanca Date: Thu, 27 Jul 2023 10:28:26 +0200 Subject: [PATCH 3/7] pass config to ROS2LidarSensorComponent and ROS2Lidar2DSensorComponent Signed-off-by: Jan Hanca --- .../Camera/ROS2CameraSensorEditorComponent.h | 1 - .../Lidar/Lidar2DSensorConfiguration.cpp | 138 +++++++++++++ .../Source/Lidar/Lidar2DSensorConfiguration.h | 54 +++++ .../Source/Lidar/LidarSensorConfiguration.cpp | 142 +++++++++++++ .../Source/Lidar/LidarSensorConfiguration.h | 54 +++++ Gems/ROS2/Code/Source/Lidar/LidarTemplate.h | 2 +- .../Lidar/ROS2Lidar2DSensorComponent.cpp | 193 +++++------------- .../Source/Lidar/ROS2Lidar2DSensorComponent.h | 36 +--- .../Source/Lidar/ROS2LidarSensorComponent.cpp | 193 +++++------------- .../Source/Lidar/ROS2LidarSensorComponent.h | 36 +--- Gems/ROS2/Code/ros2_files.cmake | 4 + 11 files changed, 516 insertions(+), 337 deletions(-) create mode 100644 Gems/ROS2/Code/Source/Lidar/Lidar2DSensorConfiguration.cpp create mode 100644 Gems/ROS2/Code/Source/Lidar/Lidar2DSensorConfiguration.h create mode 100644 Gems/ROS2/Code/Source/Lidar/LidarSensorConfiguration.cpp create mode 100644 Gems/ROS2/Code/Source/Lidar/LidarSensorConfiguration.h diff --git a/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.h b/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.h index 5bc80ba04..878faec5a 100644 --- a/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.h +++ b/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.h @@ -57,7 +57,6 @@ namespace ROS2 AZStd::pair MakeTopicConfigurationPair( const AZStd::string& topic, const AZStd::string& messageType, const AZStd::string& configName) const; - void InitializeSensorTopics(); SensorConfiguration m_sensorConfiguration; CameraSensorConfiguration m_cameraSensorConfiguration; diff --git a/Gems/ROS2/Code/Source/Lidar/Lidar2DSensorConfiguration.cpp b/Gems/ROS2/Code/Source/Lidar/Lidar2DSensorConfiguration.cpp new file mode 100644 index 000000000..8247bead3 --- /dev/null +++ b/Gems/ROS2/Code/Source/Lidar/Lidar2DSensorConfiguration.cpp @@ -0,0 +1,138 @@ +/* + * 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("IgnoreLayer", &Lidar2DSensorConfiguration::m_ignoreLayer) + ->Field("IgnoredLayerIndex", &Lidar2DSensorConfiguration::m_ignoredLayerIndex) + ->Field("ExcludedEntities", &Lidar2DSensorConfiguration::m_excludedEntities) + ->Field("PointsAtMax", &Lidar2DSensorConfiguration::m_addPointsAtMax); + + if (AZ::EditContext* ec = serializeContext->GetEditContext()) + { + ec->Class("Lidar 2D Sensor configuration", "ROS2 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::ComboBox, + &Lidar2DSensorConfiguration::m_ignoreLayer, + "Ignore layer", + "Should we ignore selected layer index") + ->Attribute(AZ::Edit::Attributes::Visibility, &Lidar2DSensorConfiguration::IsIgnoredLayerConfigurationVisible) + ->DataElement( + AZ::Edit::UIHandlers::Default, + &Lidar2DSensorConfiguration::m_ignoredLayerIndex, + "Ignored layer index", + "Layer index 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 new file mode 100644 index 000000000..8672ae444 --- /dev/null +++ b/Gems/ROS2/Code/Source/Lidar/Lidar2DSensorConfiguration.h @@ -0,0 +1,54 @@ +/* + * 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); + + bool m_ignoreLayer = false; + AZ::u32 m_ignoredLayerIndex = 0; + 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/LidarSensorConfiguration.cpp b/Gems/ROS2/Code/Source/Lidar/LidarSensorConfiguration.cpp new file mode 100644 index 000000000..53a8a3dd7 --- /dev/null +++ b/Gems/ROS2/Code/Source/Lidar/LidarSensorConfiguration.cpp @@ -0,0 +1,142 @@ +/* + * 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 "LidarSensorConfiguration.h" +#include +#include + +namespace ROS2 +{ + void LidarSensorConfiguration::Reflect(AZ::ReflectContext* context) + { + if (auto serializeContext = azrtti_cast(context)) + { + serializeContext->Class() + ->Version(1) + ->Field("lidarModel", &LidarSensorConfiguration::m_lidarModel) + ->Field("lidarImplementation", &LidarSensorConfiguration::m_lidarSystem) + ->Field("LidarParameters", &LidarSensorConfiguration::m_lidarParameters) + ->Field("IgnoreLayer", &LidarSensorConfiguration::m_ignoreLayer) + ->Field("IgnoredLayerIndex", &LidarSensorConfiguration::m_ignoredLayerIndex) + ->Field("ExcludedEntities", &LidarSensorConfiguration::m_excludedEntities) + ->Field("PointsAtMax", &LidarSensorConfiguration::m_addPointsAtMax); + + if (AZ::EditContext* ec = serializeContext->GetEditContext()) + { + ec->Class("Lidar Sensor configuration", "ROS2 Lidar sensor configuration") + ->DataElement(AZ::Edit::UIHandlers::ComboBox, &LidarSensorConfiguration::m_lidarModel, "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") + ->DataElement( + AZ::Edit::UIHandlers::ComboBox, + &LidarSensorConfiguration::m_lidarSystem, + "Lidar Implementation", + "Select a lidar implementation out of registered ones.") + ->Attribute(AZ::Edit::Attributes::ChangeNotify, &LidarSensorConfiguration::OnLidarImplementationSelected) + ->Attribute(AZ::Edit::Attributes::StringList, &LidarSensorConfiguration::FetchLidarSystemList) + ->DataElement( + AZ::Edit::UIHandlers::EntityId, + &LidarSensorConfiguration::m_lidarParameters, + "Lidar parameters", + "Configuration of Custom lidar") + ->Attribute(AZ::Edit::Attributes::Visibility, &LidarSensorConfiguration::IsConfigurationVisible) + ->DataElement( + AZ::Edit::UIHandlers::ComboBox, + &LidarSensorConfiguration::m_ignoreLayer, + "Ignore layer", + "Should we ignore selected layer index") + ->Attribute(AZ::Edit::Attributes::Visibility, &LidarSensorConfiguration::IsIgnoredLayerConfigurationVisible) + ->DataElement( + AZ::Edit::UIHandlers::Default, + &LidarSensorConfiguration::m_ignoredLayerIndex, + "Ignored layer index", + "Layer index to ignore") + ->Attribute(AZ::Edit::Attributes::Visibility, &LidarSensorConfiguration::IsIgnoredLayerConfigurationVisible) + ->DataElement( + AZ::Edit::UIHandlers::Default, + &LidarSensorConfiguration::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, &LidarSensorConfiguration::IsEntityExclusionVisible) + ->DataElement( + AZ::Edit::UIHandlers::Default, + &LidarSensorConfiguration::m_addPointsAtMax, + "Points at Max", + "If set true LiDAR will produce points at max range for free space") + ->Attribute(AZ::Edit::Attributes::Visibility, &LidarSensorConfiguration::IsMaxPointsConfigurationVisible); + } + } + } + + void LidarSensorConfiguration::FetchLidarImplementationFeatures() + { + if (m_lidarSystem.empty()) + { + m_lidarSystem = Details::GetDefaultLidarSystem(); + } + const auto* lidarMetaData = LidarRegistrarInterface::Get()->GetLidarSystemMetaData(m_lidarSystem); + AZ_Warning("LidarSensorConfiguration", lidarMetaData, "No metadata for \"%s\"", m_lidarSystem.c_str()); + if (lidarMetaData) + { + m_lidarSystemFeatures = LidarRegistrarInterface::Get()->GetLidarSystemMetaData(m_lidarSystem)->m_features; + } + } + + bool LidarSensorConfiguration::IsConfigurationVisible() const + { + return m_lidarModel == LidarTemplate::LidarModel::Custom3DLidar; + } + + bool LidarSensorConfiguration::IsIgnoredLayerConfigurationVisible() const + { + return m_lidarSystemFeatures & LidarSystemFeatures::CollisionLayers; + } + + bool LidarSensorConfiguration::IsEntityExclusionVisible() const + { + return m_lidarSystemFeatures & LidarSystemFeatures::EntityExclusion; + } + + bool LidarSensorConfiguration::IsMaxPointsConfigurationVisible() const + { + return m_lidarSystemFeatures & LidarSystemFeatures::MaxRangePoints; + } + + AZ::Crc32 LidarSensorConfiguration::OnLidarModelSelected() + { + m_lidarParameters = LidarTemplateUtils::GetTemplate(m_lidarModel); + UpdateShowNoise(); + return AZ::Edit::PropertyRefreshLevels::EntireTree; + } + + AZ::Crc32 LidarSensorConfiguration::OnLidarImplementationSelected() + { + FetchLidarImplementationFeatures(); + UpdateShowNoise(); + return AZ::Edit::PropertyRefreshLevels::EntireTree; + } + + AZStd::vector LidarSensorConfiguration::FetchLidarSystemList() + { + FetchLidarImplementationFeatures(); + UpdateShowNoise(); + return LidarRegistrarInterface::Get()->GetRegisteredLidarSystems(); + } + + void LidarSensorConfiguration::UpdateShowNoise() + { + m_lidarParameters.m_showNoiseConfig = m_lidarSystemFeatures & LidarSystemFeatures::Noise; + } +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Lidar/LidarSensorConfiguration.h b/Gems/ROS2/Code/Source/Lidar/LidarSensorConfiguration.h new file mode 100644 index 000000000..cea0c6fe7 --- /dev/null +++ b/Gems/ROS2/Code/Source/Lidar/LidarSensorConfiguration.h @@ -0,0 +1,54 @@ +/* + * 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 ROS2LidarSensorComponent). + class LidarSensorConfiguration + { + public: + AZ_TYPE_INFO(LidarSensorConfiguration, "{e46e75f4-1e0e-48ca-a22f-43afc8f25101}"); + static void Reflect(AZ::ReflectContext* context); + + void FetchLidarImplementationFeatures(); + + LidarSystemFeatures m_lidarSystemFeatures; + + AZStd::string m_lidarSystem; + LidarTemplate::LidarModel m_lidarModel = LidarTemplate::LidarModel::Custom3DLidar; + LidarTemplate m_lidarParameters = LidarTemplateUtils::GetTemplate(LidarTemplate::LidarModel::Custom3DLidar); + + bool m_ignoreLayer = false; + AZ::u32 m_ignoredLayerIndex = 0; + 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/LidarTemplate.h b/Gems/ROS2/Code/Source/Lidar/LidarTemplate.h index d4df791c5..e249faa57 100644 --- a/Gems/ROS2/Code/Source/Lidar/LidarTemplate.h +++ b/Gems/ROS2/Code/Source/Lidar/LidarTemplate.h @@ -46,7 +46,7 @@ namespace ROS2 float m_angularNoiseStdDev = 0.0f; //! Distance noise standard deviation base value, in meters float m_distanceNoiseStdDevBase = 0.0f; - //! Distance noise standard deviation increase per meter distance travelled, in meters + //! Distance noise standard deviation increase per meter distance traveled, in meters float m_distanceNoiseStdDevRisePerMeter = 0.0f; }; diff --git a/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp b/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp index 09acbe79f..a3bec6b5e 100644 --- a/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp @@ -9,11 +9,11 @@ #include #include #include +#include #include #include #include #include -#include namespace ROS2 { @@ -24,17 +24,12 @@ namespace ROS2 void ROS2Lidar2DSensorComponent::Reflect(AZ::ReflectContext* context) { + Lidar2DSensorConfiguration::Reflect(context); + if (auto* serialize = azrtti_cast(context)) { - serialize->Class() - ->Version(1) - ->Field("lidarModel", &ROS2Lidar2DSensorComponent::m_lidarModel) - ->Field("lidarImplementation", &ROS2Lidar2DSensorComponent::m_lidarSystem) - ->Field("LidarParameters", &ROS2Lidar2DSensorComponent::m_lidarParameters) - ->Field("IgnoreLayer", &ROS2Lidar2DSensorComponent::m_ignoreLayer) - ->Field("IgnoredLayerIndex", &ROS2Lidar2DSensorComponent::m_ignoredLayerIndex) - ->Field("ExcludedEntities", &ROS2Lidar2DSensorComponent::m_excludedEntities) - ->Field("PointsAtMax", &ROS2Lidar2DSensorComponent::m_addPointsAtMax); + serialize->Class()->Version(2)->Field( + "lidarConfiguration", &ROS2Lidar2DSensorComponent::m_lidarConfiguration); if (AZ::EditContext* ec = serialize->GetEditContext()) { @@ -42,116 +37,19 @@ namespace ROS2 ->ClassElement(AZ::Edit::ClassElements::EditorData, "") ->Attribute(AZ::Edit::Attributes::Category, "ROS2") ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game")) - ->DataElement(AZ::Edit::UIHandlers::ComboBox, &ROS2Lidar2DSensorComponent::m_lidarModel, "Lidar Model", "Lidar model") - ->Attribute(AZ::Edit::Attributes::ChangeNotify, &ROS2Lidar2DSensorComponent::OnLidarModelSelected) - ->EnumAttribute(LidarTemplate::LidarModel::Custom2DLidar, "Custom Lidar 2D") - ->EnumAttribute(LidarTemplate::LidarModel::Slamtec_RPLIDAR_S1, "Slamtec RPLIDAR S1") - ->DataElement( - AZ::Edit::UIHandlers::ComboBox, - &ROS2Lidar2DSensorComponent::m_lidarSystem, - "Lidar Implementation", - "Select a lidar implementation out of registered ones.") - ->Attribute(AZ::Edit::Attributes::ChangeNotify, &ROS2Lidar2DSensorComponent::OnLidarImplementationSelected) - ->Attribute(AZ::Edit::Attributes::StringList, &ROS2Lidar2DSensorComponent::FetchLidarSystemList) - ->DataElement( - AZ::Edit::UIHandlers::EntityId, - &ROS2Lidar2DSensorComponent::m_lidarParameters, - "Lidar parameters", - "Configuration of Custom lidar") - ->Attribute(AZ::Edit::Attributes::Visibility, &ROS2Lidar2DSensorComponent::IsConfigurationVisible) ->DataElement( AZ::Edit::UIHandlers::ComboBox, - &ROS2Lidar2DSensorComponent::m_ignoreLayer, - "Ignore layer", - "Should we ignore selected layer index") - ->Attribute(AZ::Edit::Attributes::Visibility, &ROS2Lidar2DSensorComponent::IsIgnoredLayerConfigurationVisible) - ->DataElement( - AZ::Edit::UIHandlers::Default, - &ROS2Lidar2DSensorComponent::m_ignoredLayerIndex, - "Ignored layer index", - "Layer index to ignore") - ->Attribute(AZ::Edit::Attributes::Visibility, &ROS2Lidar2DSensorComponent::IsIgnoredLayerConfigurationVisible) - ->DataElement( - AZ::Edit::UIHandlers::Default, - &ROS2Lidar2DSensorComponent::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, &ROS2Lidar2DSensorComponent::IsEntityExclusionVisible) - ->DataElement( - AZ::Edit::UIHandlers::Default, - &ROS2Lidar2DSensorComponent::m_addPointsAtMax, - "Points at Max", - "If set true LiDAR will produce points at max range for free space") - ->Attribute(AZ::Edit::Attributes::Visibility, &ROS2Lidar2DSensorComponent::IsMaxPointsConfigurationVisible); + &ROS2Lidar2DSensorComponent::m_lidarConfiguration, + "Lidar configuration", + "Lidar configuration"); } } } - void ROS2Lidar2DSensorComponent::UpdateShowNoise() - { - m_lidarParameters.m_showNoiseConfig = m_lidarSystemFeatures & LidarSystemFeatures::Noise; - } - - void ROS2Lidar2DSensorComponent::FetchLidarImplementationFeatures() - { - if (m_lidarSystem.empty()) - { - m_lidarSystem = Details::GetDefaultLidarSystem(); - } - const auto* lidarMetaData = LidarRegistrarInterface::Get()->GetLidarSystemMetaData(m_lidarSystem); - AZ_Warning("ROS2Lidar2DSensorComponent", lidarMetaData, "No metadata for \"%s\"", m_lidarSystem.c_str()); - if (lidarMetaData) - { - m_lidarSystemFeatures = LidarRegistrarInterface::Get()->GetLidarSystemMetaData(m_lidarSystem)->m_features; - } - } - - bool ROS2Lidar2DSensorComponent::IsConfigurationVisible() const - { - return m_lidarModel == LidarTemplate::LidarModel::Custom2DLidar; - } - - bool ROS2Lidar2DSensorComponent::IsIgnoredLayerConfigurationVisible() const - { - return m_lidarSystemFeatures & LidarSystemFeatures::CollisionLayers; - } - - bool ROS2Lidar2DSensorComponent::IsEntityExclusionVisible() const - { - return m_lidarSystemFeatures & LidarSystemFeatures::EntityExclusion; - } - - bool ROS2Lidar2DSensorComponent::IsMaxPointsConfigurationVisible() const - { - return m_lidarSystemFeatures & LidarSystemFeatures::MaxRangePoints; - } - - AZStd::vector ROS2Lidar2DSensorComponent::FetchLidarSystemList() - { - FetchLidarImplementationFeatures(); - UpdateShowNoise(); - return LidarRegistrarInterface::Get()->GetRegisteredLidarSystems(); - } - - AZ::Crc32 ROS2Lidar2DSensorComponent::OnLidarModelSelected() - { - m_lidarParameters = LidarTemplateUtils::GetTemplate(m_lidarModel); - UpdateShowNoise(); - return AZ::Edit::PropertyRefreshLevels::EntireTree; - } - - AZ::Crc32 ROS2Lidar2DSensorComponent::OnLidarImplementationSelected() - { - FetchLidarImplementationFeatures(); - UpdateShowNoise(); - return AZ::Edit::PropertyRefreshLevels::EntireTree; - } - void ROS2Lidar2DSensorComponent::ConnectToLidarRaycaster() { - if (auto raycasterId = m_implementationToRaycasterMap.find(m_lidarSystem); raycasterId != m_implementationToRaycasterMap.end()) + if (auto raycasterId = m_implementationToRaycasterMap.find(m_lidarConfiguration.m_lidarSystem); + raycasterId != m_implementationToRaycasterMap.end()) { m_lidarRaycasterId = raycasterId->second; return; @@ -159,29 +57,32 @@ namespace ROS2 m_lidarRaycasterId = LidarId::CreateNull(); LidarSystemRequestBus::EventResult( - m_lidarRaycasterId, AZ_CRC(m_lidarSystem), &LidarSystemRequestBus::Events::CreateLidar, GetEntityId()); + 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_lidarSystem, m_lidarRaycasterId); + m_implementationToRaycasterMap.emplace(m_lidarConfiguration.m_lidarSystem, m_lidarRaycasterId); } void ROS2Lidar2DSensorComponent::ConfigureLidarRaycaster() { - FetchLidarImplementationFeatures(); + m_lidarConfiguration.FetchLidarImplementationFeatures(); LidarRaycasterRequestBus::Event(m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRayOrientations, m_lastRotations); LidarRaycasterRequestBus::Event( - m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRayRange, m_lidarParameters.m_maxRange); + m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRayRange, m_lidarConfiguration.m_lidarParameters.m_maxRange); LidarRaycasterRequestBus::Event( - m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureMinimumRayRange, m_lidarParameters.m_minRange); + m_lidarRaycasterId, + &LidarRaycasterRequestBus::Events::ConfigureMinimumRayRange, + m_lidarConfiguration.m_lidarParameters.m_minRange); - if (m_lidarSystemFeatures & LidarSystemFeatures::Noise && m_lidarParameters.m_isNoiseEnabled) + if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::Noise && + m_lidarConfiguration.m_lidarParameters.m_isNoiseEnabled) { LidarRaycasterRequestBus::Event( m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureNoiseParameters, - m_lidarParameters.m_noiseParameters.m_angularNoiseStdDev, - m_lidarParameters.m_noiseParameters.m_distanceNoiseStdDevBase, - m_lidarParameters.m_noiseParameters.m_distanceNoiseStdDevRisePerMeter); + 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; @@ -191,21 +92,27 @@ namespace ROS2 } LidarRaycasterRequestBus::Event(m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRaycastResultFlags, requestedFlags); - if (m_lidarSystemFeatures & LidarSystemFeatures::CollisionLayers) + if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::CollisionLayers) { LidarRaycasterRequestBus::Event( - m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureLayerIgnoring, m_ignoreLayer, m_ignoredLayerIndex); + m_lidarRaycasterId, + &LidarRaycasterRequestBus::Events::ConfigureLayerIgnoring, + m_lidarConfiguration.m_ignoreLayer, + m_lidarConfiguration.m_ignoredLayerIndex); } - if (m_lidarSystemFeatures & LidarSystemFeatures::EntityExclusion) + if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::EntityExclusion) { - LidarRaycasterRequestBus::Event(m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ExcludeEntities, m_excludedEntities); + LidarRaycasterRequestBus::Event( + m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ExcludeEntities, m_lidarConfiguration.m_excludedEntities); } - if (m_lidarSystemFeatures & LidarSystemFeatures::MaxRangePoints) + if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::MaxRangePoints) { LidarRaycasterRequestBus::Event( - m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureMaxRangePointAddition, m_addPointsAtMax); + m_lidarRaycasterId, + &LidarRaycasterRequestBus::Events::ConfigureMaxRangePointAddition, + m_lidarConfiguration.m_addPointsAtMax); } } @@ -219,9 +126,16 @@ namespace ROS2 m_sensorConfiguration.m_publishersConfigurations.insert(AZStd::make_pair(type, ls)); } + ROS2Lidar2DSensorComponent::ROS2Lidar2DSensorComponent( + const SensorConfiguration& sensorConfiguration, const Lidar2DSensorConfiguration& lidarConfiguration) + : m_lidarConfiguration(lidarConfiguration) + { + m_sensorConfiguration = sensorConfiguration; + } + void ROS2Lidar2DSensorComponent::Visualise() { - if (m_visualisationPoints.empty()) + if (m_visualizationPoints.empty()) { return; } @@ -230,8 +144,8 @@ namespace ROS2 { const uint8_t pixelSize = 2; AZ::RPI::AuxGeomDraw::AuxGeomDynamicDrawArguments drawArgs; - drawArgs.m_verts = m_visualisationPoints.data(); - drawArgs.m_vertCount = m_visualisationPoints.size(); + 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; @@ -255,9 +169,9 @@ namespace ROS2 m_drawQueue = AZ::RPI::AuxGeomFeatureProcessorInterface::GetDrawQueueForScene(entityScene); } - m_lastRotations = LidarTemplateUtils::PopulateRayRotations(m_lidarParameters); + m_lastRotations = LidarTemplateUtils::PopulateRayRotations(m_lidarConfiguration.m_lidarParameters); - FetchLidarImplementationFeatures(); + m_lidarConfiguration.FetchLidarImplementationFeatures(); ConnectToLidarRaycaster(); ConfigureLidarRaycaster(); @@ -288,20 +202,21 @@ namespace ROS2 } if (m_sensorConfiguration.m_visualise) - { // Store points for visualisation purposes, in global frame - m_visualisationPoints = m_lastScanResults.m_points; + { // Store points for visualization purposes, in global frame + m_visualizationPoints = m_lastScanResults.m_points; } 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_lidarParameters.m_minHAngle); - message.angle_max = AZ::DegToRad(m_lidarParameters.m_maxHAngle); - message.angle_increment = (message.angle_max - message.angle_min) / aznumeric_cast(m_lidarParameters.m_numberOfIncrements); + 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.range_min = m_lidarParameters.m_minRange; - message.range_max = m_lidarParameters.m_maxRange; + message.range_min = m_lidarConfiguration.m_lidarParameters.m_minRange; + message.range_max = m_lidarConfiguration.m_lidarParameters.m_maxRange; message.scan_time = 1.f / m_sensorConfiguration.m_frequency; message.time_increment = 0.0f; diff --git a/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.h b/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.h index 84143498f..20abcd1c6 100644 --- a/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.h +++ b/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.h @@ -9,15 +9,15 @@ #include #include -#include -#include -#include #include #include #include #include #include +#include "Lidar2DSensorConfiguration.h" +#include "LidarRaycaster.h" + namespace ROS2 { //! Lidar 2D sensor Component. @@ -29,6 +29,7 @@ namespace ROS2 public: AZ_COMPONENT(ROS2Lidar2DSensorComponent, "{F4C2D970-1D69-40F2-9D4D-B52DCFDD2704}", ROS2SensorComponent); ROS2Lidar2DSensorComponent(); + ROS2Lidar2DSensorComponent(const SensorConfiguration& sensorConfiguration, const Lidar2DSensorConfiguration& lidarConfiguration); ~ROS2Lidar2DSensorComponent() = default; static void Reflect(AZ::ReflectContext* context); ////////////////////////////////////////////////////////////////////////// @@ -43,40 +44,21 @@ namespace ROS2 void FrequencyTick() override; void Visualise() override; - bool IsConfigurationVisible() const; - bool IsIgnoredLayerConfigurationVisible() const; - bool IsEntityExclusionVisible() const; - bool IsMaxPointsConfigurationVisible() const; - - AZ::Crc32 OnLidarModelSelected(); - AZ::Crc32 OnLidarImplementationSelected(); - void UpdateShowNoise(); - void FetchLidarImplementationFeatures(); - AZStd::vector FetchLidarSystemList(); void ConnectToLidarRaycaster(); void ConfigureLidarRaycaster(); - LidarSystemFeatures m_lidarSystemFeatures; - LidarTemplate::LidarModel m_lidarModel = LidarTemplate::LidarModel::Custom2DLidar; - LidarTemplate m_lidarParameters = LidarTemplateUtils::GetTemplate(LidarTemplate::LidarModel::Custom2DLidar); - AZStd::vector m_lastRotations; - - AZStd::string m_lidarSystem; // 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 visualisation is on - points differ since they are in global transform as opposed to local - AZStd::vector m_visualisationPoints; + // 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; - RaycastResult m_lastScanResults; - - AZ::u32 m_ignoredLayerIndex = 0; - bool m_ignoreLayer = false; - AZStd::vector m_excludedEntities; + Lidar2DSensorConfiguration m_lidarConfiguration; - bool m_addPointsAtMax = false; + AZStd::vector m_lastRotations; + RaycastResult m_lastScanResults; }; } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp index 4a8ec1222..495914a0d 100644 --- a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp @@ -9,11 +9,11 @@ #include #include #include +#include #include #include #include #include -#include namespace ROS2 { @@ -24,17 +24,12 @@ namespace ROS2 void ROS2LidarSensorComponent::Reflect(AZ::ReflectContext* context) { + LidarSensorConfiguration::Reflect(context); + if (AZ::SerializeContext* serialize = azrtti_cast(context)) { - serialize->Class() - ->Version(1) - ->Field("lidarModel", &ROS2LidarSensorComponent::m_lidarModel) - ->Field("lidarImplementation", &ROS2LidarSensorComponent::m_lidarSystem) - ->Field("LidarParameters", &ROS2LidarSensorComponent::m_lidarParameters) - ->Field("IgnoreLayer", &ROS2LidarSensorComponent::m_ignoreLayer) - ->Field("IgnoredLayerIndex", &ROS2LidarSensorComponent::m_ignoredLayerIndex) - ->Field("ExcludedEntities", &ROS2LidarSensorComponent::m_excludedEntities) - ->Field("PointsAtMax", &ROS2LidarSensorComponent::m_addPointsAtMax); + serialize->Class()->Version(2)->Field( + "lidarConfiguration", &ROS2LidarSensorComponent::m_lidarConfiguration); if (AZ::EditContext* ec = serialize->GetEditContext()) { @@ -42,120 +37,19 @@ namespace ROS2 ->ClassElement(AZ::Edit::ClassElements::EditorData, "") ->Attribute(AZ::Edit::Attributes::Category, "ROS2") ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game")) - ->DataElement(AZ::Edit::UIHandlers::ComboBox, &ROS2LidarSensorComponent::m_lidarModel, "Lidar Model", "Lidar model") - ->Attribute(AZ::Edit::Attributes::ChangeNotify, &ROS2LidarSensorComponent::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") - ->DataElement( - AZ::Edit::UIHandlers::ComboBox, - &ROS2LidarSensorComponent::m_lidarSystem, - "Lidar Implementation", - "Select a lidar implementation out of registered ones.") - ->Attribute(AZ::Edit::Attributes::ChangeNotify, &ROS2LidarSensorComponent::OnLidarImplementationSelected) - ->Attribute(AZ::Edit::Attributes::StringList, &ROS2LidarSensorComponent::FetchLidarSystemList) - ->DataElement( - AZ::Edit::UIHandlers::EntityId, - &ROS2LidarSensorComponent::m_lidarParameters, - "Lidar parameters", - "Configuration of Custom lidar") - ->Attribute(AZ::Edit::Attributes::Visibility, &ROS2LidarSensorComponent::IsConfigurationVisible) ->DataElement( AZ::Edit::UIHandlers::ComboBox, - &ROS2LidarSensorComponent::m_ignoreLayer, - "Ignore layer", - "Should we ignore selected layer index") - ->Attribute(AZ::Edit::Attributes::Visibility, &ROS2LidarSensorComponent::IsIgnoredLayerConfigurationVisible) - ->DataElement( - AZ::Edit::UIHandlers::Default, - &ROS2LidarSensorComponent::m_ignoredLayerIndex, - "Ignored layer index", - "Layer index to ignore") - ->Attribute(AZ::Edit::Attributes::Visibility, &ROS2LidarSensorComponent::IsIgnoredLayerConfigurationVisible) - ->DataElement( - AZ::Edit::UIHandlers::Default, - &ROS2LidarSensorComponent::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, &ROS2LidarSensorComponent::IsEntityExclusionVisible) - ->DataElement( - AZ::Edit::UIHandlers::Default, - &ROS2LidarSensorComponent::m_addPointsAtMax, - "Points at Max", - "If set true LiDAR will produce points at max range for free space") - ->Attribute(AZ::Edit::Attributes::Visibility, &ROS2LidarSensorComponent::IsMaxPointsConfigurationVisible); + &ROS2LidarSensorComponent::m_lidarConfiguration, + "Lidar configuration", + "Lidar configuration"); } } } - void ROS2LidarSensorComponent::UpdateShowNoise() - { - m_lidarParameters.m_showNoiseConfig = m_lidarSystemFeatures & LidarSystemFeatures::Noise; - } - - void ROS2LidarSensorComponent::FetchLidarImplementationFeatures() - { - if (m_lidarSystem.empty()) - { - m_lidarSystem = Details::GetDefaultLidarSystem(); - } - const auto* lidarMetaData = LidarRegistrarInterface::Get()->GetLidarSystemMetaData(m_lidarSystem); - AZ_Warning("ROS2LidarSensorComponent", lidarMetaData, "No metadata for \"%s\"", m_lidarSystem.c_str()); - if (lidarMetaData) - { - m_lidarSystemFeatures = LidarRegistrarInterface::Get()->GetLidarSystemMetaData(m_lidarSystem)->m_features; - } - } - - bool ROS2LidarSensorComponent::IsConfigurationVisible() const - { - return m_lidarModel == LidarTemplate::LidarModel::Custom3DLidar; - } - - bool ROS2LidarSensorComponent::IsIgnoredLayerConfigurationVisible() const - { - return m_lidarSystemFeatures & LidarSystemFeatures::CollisionLayers; - } - - bool ROS2LidarSensorComponent::IsEntityExclusionVisible() const - { - return m_lidarSystemFeatures & LidarSystemFeatures::EntityExclusion; - } - - bool ROS2LidarSensorComponent::IsMaxPointsConfigurationVisible() const - { - return m_lidarSystemFeatures & LidarSystemFeatures::MaxRangePoints; - } - - AZStd::vector ROS2LidarSensorComponent::FetchLidarSystemList() - { - FetchLidarImplementationFeatures(); - UpdateShowNoise(); - return LidarRegistrarInterface::Get()->GetRegisteredLidarSystems(); - } - - AZ::Crc32 ROS2LidarSensorComponent::OnLidarModelSelected() - { - m_lidarParameters = LidarTemplateUtils::GetTemplate(m_lidarModel); - UpdateShowNoise(); - return AZ::Edit::PropertyRefreshLevels::EntireTree; - } - - AZ::Crc32 ROS2LidarSensorComponent::OnLidarImplementationSelected() - { - FetchLidarImplementationFeatures(); - UpdateShowNoise(); - return AZ::Edit::PropertyRefreshLevels::EntireTree; - } - void ROS2LidarSensorComponent::ConnectToLidarRaycaster() { - if (auto raycasterId = m_implementationToRaycasterMap.find(m_lidarSystem); raycasterId != m_implementationToRaycasterMap.end()) + if (auto raycasterId = m_implementationToRaycasterMap.find(m_lidarConfiguration.m_lidarSystem); + raycasterId != m_implementationToRaycasterMap.end()) { m_lidarRaycasterId = raycasterId->second; return; @@ -163,51 +57,60 @@ namespace ROS2 m_lidarRaycasterId = LidarId::CreateNull(); LidarSystemRequestBus::EventResult( - m_lidarRaycasterId, AZ_CRC(m_lidarSystem), &LidarSystemRequestBus::Events::CreateLidar, GetEntityId()); + 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_lidarSystem, m_lidarRaycasterId); + m_implementationToRaycasterMap.emplace(m_lidarConfiguration.m_lidarSystem, m_lidarRaycasterId); } void ROS2LidarSensorComponent::ConfigureLidarRaycaster() { - FetchLidarImplementationFeatures(); + m_lidarConfiguration.FetchLidarImplementationFeatures(); LidarRaycasterRequestBus::Event(m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRayOrientations, m_lastRotations); LidarRaycasterRequestBus::Event( - m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureMinimumRayRange, m_lidarParameters.m_minRange); + m_lidarRaycasterId, + &LidarRaycasterRequestBus::Events::ConfigureMinimumRayRange, + m_lidarConfiguration.m_lidarParameters.m_minRange); LidarRaycasterRequestBus::Event( - m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRayRange, m_lidarParameters.m_maxRange); + m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRayRange, m_lidarConfiguration.m_lidarParameters.m_maxRange); LidarRaycasterRequestBus::Event( m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRaycastResultFlags, RaycastResultFlags::Points); - if ((m_lidarSystemFeatures & LidarSystemFeatures::Noise) && m_lidarParameters.m_isNoiseEnabled) + if ((m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::Noise) && + m_lidarConfiguration.m_lidarParameters.m_isNoiseEnabled) { LidarRaycasterRequestBus::Event( m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureNoiseParameters, - m_lidarParameters.m_noiseParameters.m_angularNoiseStdDev, - m_lidarParameters.m_noiseParameters.m_distanceNoiseStdDevBase, - m_lidarParameters.m_noiseParameters.m_distanceNoiseStdDevRisePerMeter); + 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_lidarSystemFeatures & LidarSystemFeatures::CollisionLayers) + if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::CollisionLayers) { LidarRaycasterRequestBus::Event( - m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureLayerIgnoring, m_ignoreLayer, m_ignoredLayerIndex); + m_lidarRaycasterId, + &LidarRaycasterRequestBus::Events::ConfigureLayerIgnoring, + m_lidarConfiguration.m_ignoreLayer, + m_lidarConfiguration.m_ignoredLayerIndex); } - if (m_lidarSystemFeatures & LidarSystemFeatures::EntityExclusion) + if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::EntityExclusion) { - LidarRaycasterRequestBus::Event(m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ExcludeEntities, m_excludedEntities); + LidarRaycasterRequestBus::Event( + m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ExcludeEntities, m_lidarConfiguration.m_excludedEntities); } - if (m_lidarSystemFeatures & LidarSystemFeatures::MaxRangePoints) + if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::MaxRangePoints) { LidarRaycasterRequestBus::Event( - m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureMaxRangePointAddition, m_addPointsAtMax); + m_lidarRaycasterId, + &LidarRaycasterRequestBus::Events::ConfigureMaxRangePointAddition, + m_lidarConfiguration.m_addPointsAtMax); } - if (m_lidarSystemFeatures & LidarSystemFeatures::PointcloudPublishing) + if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::PointcloudPublishing) { const TopicConfiguration& publisherConfig = m_sensorConfiguration.m_publishersConfigurations[Internal::kPointCloudType]; auto* ros2Frame = Utils::GetGameOrEditorComponent(GetEntity()); @@ -231,9 +134,16 @@ namespace ROS2 m_sensorConfiguration.m_publishersConfigurations.insert(AZStd::make_pair(type, pc)); } + ROS2LidarSensorComponent::ROS2LidarSensorComponent( + const SensorConfiguration& sensorConfiguration, const LidarSensorConfiguration& lidarConfiguration) + : m_lidarConfiguration(lidarConfiguration) + { + m_sensorConfiguration = sensorConfiguration; + } + void ROS2LidarSensorComponent::Visualise() { - if (m_visualisationPoints.empty()) + if (m_visualizationPoints.empty()) { return; } @@ -242,8 +152,8 @@ namespace ROS2 { const uint8_t pixelSize = 2; AZ::RPI::AuxGeomDraw::AuxGeomDynamicDrawArguments drawArgs; - drawArgs.m_verts = m_visualisationPoints.data(); - drawArgs.m_vertCount = m_visualisationPoints.size(); + 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; @@ -260,14 +170,14 @@ namespace ROS2 m_drawQueue = AZ::RPI::AuxGeomFeatureProcessorInterface::GetDrawQueueForScene(entityScene); } - m_lastRotations = LidarTemplateUtils::PopulateRayRotations(m_lidarParameters); + m_lastRotations = LidarTemplateUtils::PopulateRayRotations(m_lidarConfiguration.m_lidarParameters); - FetchLidarImplementationFeatures(); + m_lidarConfiguration.FetchLidarImplementationFeatures(); ConnectToLidarRaycaster(); ConfigureLidarRaycaster(); m_canRaycasterPublish = false; - if (m_lidarSystemFeatures & LidarSystemFeatures::PointcloudPublishing) + if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::PointcloudPublishing) { LidarRaycasterRequestBus::EventResult( m_canRaycasterPublish, m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::CanHandlePublishing); @@ -309,8 +219,7 @@ namespace ROS2 LidarRaycasterRequestBus::Event( m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::UpdatePublisherTimestamp, - aznumeric_cast(timestamp.sec) * aznumeric_cast(1.0e9f) + timestamp.nanosec - ); + aznumeric_cast(timestamp.sec) * aznumeric_cast(1.0e9f) + timestamp.nanosec); } LidarRaycasterRequestBus::EventResult( @@ -322,8 +231,8 @@ namespace ROS2 } if (m_sensorConfiguration.m_visualise) - { // Store points for visualisation purposes, in global frame - m_visualisationPoints = m_lastScanResults.m_points; + { // Store points for visualization purposes, in global frame + m_visualizationPoints = m_lastScanResults.m_points; } if (m_canRaycasterPublish) diff --git a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.h b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.h index 9d27b292f..fc0a3df18 100644 --- a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.h +++ b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.h @@ -9,15 +9,15 @@ #include #include -#include -#include -#include #include #include #include #include #include +#include "LidarRaycaster.h" +#include "LidarSensorConfiguration.h" + namespace ROS2 { //! Lidar sensor Component. @@ -29,6 +29,7 @@ namespace ROS2 public: AZ_COMPONENT(ROS2LidarSensorComponent, "{502A955F-7742-4E23-AD77-5E4063739DCA}", ROS2SensorComponent); ROS2LidarSensorComponent(); + ROS2LidarSensorComponent(const SensorConfiguration& sensorConfiguration, const LidarSensorConfiguration& lidarConfiguration); ~ROS2LidarSensorComponent() = default; static void Reflect(AZ::ReflectContext* context); ////////////////////////////////////////////////////////////////////////// @@ -43,41 +44,22 @@ namespace ROS2 void FrequencyTick() override; void Visualise() override; - bool IsConfigurationVisible() const; - bool IsIgnoredLayerConfigurationVisible() const; - bool IsEntityExclusionVisible() const; - bool IsMaxPointsConfigurationVisible() const; - - AZ::Crc32 OnLidarModelSelected(); - AZ::Crc32 OnLidarImplementationSelected(); - void UpdateShowNoise(); - void FetchLidarImplementationFeatures(); - AZStd::vector FetchLidarSystemList(); void ConnectToLidarRaycaster(); void ConfigureLidarRaycaster(); - LidarSystemFeatures m_lidarSystemFeatures; - LidarTemplate::LidarModel m_lidarModel = LidarTemplate::LidarModel::Custom3DLidar; - LidarTemplate m_lidarParameters = LidarTemplateUtils::GetTemplate(LidarTemplate::LidarModel::Custom3DLidar); - AZStd::vector m_lastRotations; - - AZStd::string m_lidarSystem; // 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 visualisation is on - points differ since they are in global transform as opposed to local - AZStd::vector m_visualisationPoints; + // 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; - RaycastResult m_lastScanResults; - - AZ::u32 m_ignoredLayerIndex = 0; - bool m_ignoreLayer = false; - AZStd::vector m_excludedEntities; + LidarSensorConfiguration m_lidarConfiguration; - bool m_addPointsAtMax = false; + AZStd::vector m_lastRotations; + RaycastResult m_lastScanResults; }; } // namespace ROS2 diff --git a/Gems/ROS2/Code/ros2_files.cmake b/Gems/ROS2/Code/ros2_files.cmake index 727379ee3..962f4f12e 100644 --- a/Gems/ROS2/Code/ros2_files.cmake +++ b/Gems/ROS2/Code/ros2_files.cmake @@ -52,6 +52,10 @@ 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 Source/Lidar/LidarSystem.h Source/Lidar/LidarTemplate.cpp From 68d1c2dc3bdd407c9a1c48178333506f745df21e Mon Sep 17 00:00:00 2001 From: Jan Hanca Date: Thu, 27 Jul 2023 11:05:25 +0200 Subject: [PATCH 4/7] pass config to ROS2ImuSensorComponent Signed-off-by: Jan Hanca --- .../Source/GNSS/ROS2GNSSSensorComponent.cpp | 2 +- .../Source/Imu/ImuSensorConfiguration.cpp | 67 ++++++++++++++++++ .../Code/Source/Imu/ImuSensorConfiguration.h | 38 ++++++++++ .../Source/Imu/ROS2ImuSensorComponent.cpp | 70 ++++++------------- .../Code/Source/Imu/ROS2ImuSensorComponent.h | 19 ++--- .../Lidar/Lidar2DSensorConfiguration.cpp | 2 +- .../Source/Lidar/LidarSensorConfiguration.cpp | 2 +- Gems/ROS2/Code/ros2_files.cmake | 2 + 8 files changed, 137 insertions(+), 65 deletions(-) create mode 100644 Gems/ROS2/Code/Source/Imu/ImuSensorConfiguration.cpp create mode 100644 Gems/ROS2/Code/Source/Imu/ImuSensorConfiguration.h diff --git a/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp b/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp index 3ab82b8f6..2b965c27d 100644 --- a/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp @@ -33,7 +33,7 @@ namespace ROS2 if (AZ::SerializeContext* serialize = azrtti_cast(context)) { serialize->Class()->Version(2)->Field( - "gnssOriginLatitude", &ROS2GNSSSensorComponent::m_gnssConfiguration); + "gnssSensorConfiguration", &ROS2GNSSSensorComponent::m_gnssConfiguration); if (AZ::EditContext* ec = serialize->GetEditContext()) { diff --git a/Gems/ROS2/Code/Source/Imu/ImuSensorConfiguration.cpp b/Gems/ROS2/Code/Source/Imu/ImuSensorConfiguration.cpp new file mode 100644 index 000000000..df6cce9b9 --- /dev/null +++ b/Gems/ROS2/Code/Source/Imu/ImuSensorConfiguration.cpp @@ -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 + * + */ + +#include "ImuSensorConfiguration.h" +#include +#include + +namespace ROS2 +{ + void ImuSensorConfiguration::Reflect(AZ::ReflectContext* context) + { + if (AZ::SerializeContext* serialize = azrtti_cast(context)) + { + serialize->Class() + ->Version(1) + ->Field("FilterSize", &ImuSensorConfiguration::m_filterSize) + ->Field("IncludeGravity", &ImuSensorConfiguration::m_includeGravity) + ->Field("AbsoluteRotation", &ImuSensorConfiguration::m_absoluteRotation) + ->Field("AccelerationVariance", &ImuSensorConfiguration::m_linearAccelerationVariance) + ->Field("AngularVelocityVariance", &ImuSensorConfiguration::m_angularVelocityVariance) + ->Field("OrientationVariance", &ImuSensorConfiguration::m_orientationVariance); + + if (AZ::EditContext* ec = serialize->GetEditContext()) + { + ec->Class("ROS2 IMU sensor configuration", "IMU sensor configuration") + ->DataElement( + AZ::Edit::UIHandlers::Slider, + &ImuSensorConfiguration::m_filterSize, + "Filter Length", + "Filter Length, Large value reduce numeric noise but increase lag") + ->Attribute(AZ::Edit::Attributes::Max, &ImuSensorConfiguration::m_maxFilterSize) + ->Attribute(AZ::Edit::Attributes::Min, &ImuSensorConfiguration::m_minFilterSize) + ->DataElement( + AZ::Edit::UIHandlers::Default, + &ImuSensorConfiguration::m_includeGravity, + "Include Gravitation", + "Enable accelerometer to observe gravity force.") + ->DataElement( + AZ::Edit::UIHandlers::Default, + &ImuSensorConfiguration::m_absoluteRotation, + "Absolute Rotation", + "Include Absolute rotation in message.") + ->DataElement( + AZ::Edit::UIHandlers::Default, + &ImuSensorConfiguration::m_linearAccelerationVariance, + "Linear Acceleration Variance", + "Variance of linear acceleration.") + ->DataElement( + AZ::Edit::UIHandlers::Default, + &ImuSensorConfiguration::m_angularVelocityVariance, + "Angular Velocity Variance", + "Variance of angular velocity.") + ->DataElement( + AZ::Edit::UIHandlers::Default, + &ImuSensorConfiguration::m_orientationVariance, + "Orientation Variance", + "Variance of orientation."); + } + } + } + +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Imu/ImuSensorConfiguration.h b/Gems/ROS2/Code/Source/Imu/ImuSensorConfiguration.h new file mode 100644 index 000000000..6051e3aed --- /dev/null +++ b/Gems/ROS2/Code/Source/Imu/ImuSensorConfiguration.h @@ -0,0 +1,38 @@ +/* + * 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 + +namespace ROS2 +{ + //! A structure capturing configuration of a IMU sensor. + struct ImuSensorConfiguration + { + AZ_TYPE_INFO(ImuSensorConfiguration, "{6788e84f-b985-4413-8e2b-46fbfb667c95}"); + static void Reflect(AZ::ReflectContext* context); + + //! Length of filter that removes numerical noise + int m_filterSize = 10; + int m_minFilterSize = 1; + int m_maxFilterSize = 200; + + //! Include gravity acceleration + bool m_includeGravity = true; + + //! Measure also absolute rotation + bool m_absoluteRotation = true; + + AZ::Vector3 m_orientationVariance = AZ::Vector3::CreateZero(); + AZ::Vector3 m_angularVelocityVariance = AZ::Vector3::CreateZero(); + AZ::Vector3 m_linearAccelerationVariance = AZ::Vector3::CreateZero(); + }; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.cpp b/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.cpp index bc4ec46a2..4ce42b280 100644 --- a/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.cpp @@ -31,16 +31,12 @@ namespace ROS2 void ROS2ImuSensorComponent::Reflect(AZ::ReflectContext* context) { + ImuSensorConfiguration::Reflect(context); + if (AZ::SerializeContext* serialize = azrtti_cast(context)) { - serialize->Class() - ->Version(1) - ->Field("FilterSize", &ROS2ImuSensorComponent::m_filterSize) - ->Field("IncludeGravity", &ROS2ImuSensorComponent::m_includeGravity) - ->Field("AbsoluteRotation", &ROS2ImuSensorComponent::m_absoluteRotation) - ->Field("AccelerationVariance", &ROS2ImuSensorComponent::m_linearAccelerationVariance) - ->Field("AngularVelocityVariance", &ROS2ImuSensorComponent::m_angularVelocityVariance) - ->Field("OrientationVariance", &ROS2ImuSensorComponent::m_orientationVariance); + serialize->Class()->Version(2)->Field( + "imuSensorConfiguration", &ROS2ImuSensorComponent::m_imuConfiguration); if (AZ::EditContext* ec = serialize->GetEditContext()) { @@ -48,38 +44,11 @@ namespace ROS2 ->ClassElement(AZ::Edit::ClassElements::EditorData, "") ->Attribute(AZ::Edit::Attributes::Category, "ROS2") ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game")) - ->DataElement( - AZ::Edit::UIHandlers::Slider, - &ROS2ImuSensorComponent::m_filterSize, - "Filter Length", - "Filter Length, Large value reduce numeric noise but increase lag") - ->Attribute(AZ::Edit::Attributes::Max, &ROS2ImuSensorComponent::m_maxFilterSize) - ->Attribute(AZ::Edit::Attributes::Min, &ROS2ImuSensorComponent::m_minFilterSize) - ->DataElement( - AZ::Edit::UIHandlers::Default, - &ROS2ImuSensorComponent::m_includeGravity, - "Include Gravitation", - "Enable accelerometer to observe gravity force.") - ->DataElement( - AZ::Edit::UIHandlers::Default, - &ROS2ImuSensorComponent::m_absoluteRotation, - "Absolute Rotation", - "Include Absolute rotation in message.") - ->DataElement( - AZ::Edit::UIHandlers::Default, - &ROS2ImuSensorComponent::m_linearAccelerationVariance, - "Linear Acceleration Variance", - "Variance of linear acceleration.") ->DataElement( AZ::Edit::UIHandlers::Default, - &ROS2ImuSensorComponent::m_angularVelocityVariance, - "Angular Velocity Variance", - "Variance of angular velocity.") - ->DataElement( - AZ::Edit::UIHandlers::Default, - &ROS2ImuSensorComponent::m_orientationVariance, - "Orientation Variance", - "Variance of orientation."); + &ROS2ImuSensorComponent::m_imuConfiguration, + "Imu sensor configuration", + "Imu sensor configuration"); } } } @@ -94,6 +63,13 @@ namespace ROS2 m_sensorConfiguration.m_publishersConfigurations.insert(AZStd::make_pair(type, pc)); } + ROS2ImuSensorComponent::ROS2ImuSensorComponent( + const SensorConfiguration& sensorConfiguration, const ImuSensorConfiguration& imuConfiguration) + : m_imuConfiguration(imuConfiguration) + { + m_sensorConfiguration = sensorConfiguration; + } + void ROS2ImuSensorComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required) { required.push_back(AZ_CRC_CE("PhysicsRigidBodyService")); @@ -117,7 +93,7 @@ namespace ROS2 m_filterAcceleration.push_back(linearVelocity); const auto angularVelocity = inv.TransformVector(rigidbody->GetAngularVelocity()); m_filterAngularVelocity.push_back(angularVelocity); - if (m_filterAcceleration.size() > m_filterSize) + if (m_filterAcceleration.size() > m_imuConfiguration.m_filterSize) { m_filterAcceleration.pop_front(); m_filterAngularVelocity.pop_front(); @@ -135,16 +111,16 @@ namespace ROS2 m_previousLinearVelocity = linearVelocityFilter; m_acceleration = -acc + angularRateFiltered.Cross(linearVelocityFilter); - if (m_includeGravity) + if (m_imuConfiguration.m_includeGravity) { m_acceleration += inv.TransformVector(gravity); } m_imuMsg.linear_acceleration = ROS2Conversions::ToROS2Vector3(m_acceleration); - m_imuMsg.linear_acceleration_covariance = ROS2Conversions::ToROS2Covariance(m_linearAccelerationCovariance); + m_imuMsg.linear_acceleration_covariance = ROS2Conversions::ToROS2Covariance(m_linearAccelerationCovariance); m_imuMsg.angular_velocity = ROS2Conversions::ToROS2Vector3(angularRateFiltered); m_imuMsg.angular_velocity_covariance = ROS2Conversions::ToROS2Covariance(m_angularVelocityCovariance); - if (m_absoluteRotation) + if (m_imuConfiguration.m_absoluteRotation) { m_imuMsg.orientation = ROS2Conversions::ToROS2Quaternion(rigidbody->GetTransform().GetRotation()); m_imuMsg.orientation_covariance = ROS2Conversions::ToROS2Covariance(m_orientationCovariance); @@ -162,11 +138,11 @@ namespace ROS2 const auto publisherConfig = m_sensorConfiguration.m_publishersConfigurations[Internal::kImuMsgType]; const auto fullTopic = ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic); m_imuPublisher = ros2Node->create_publisher(fullTopic.data(), publisherConfig.GetQoS()); - - m_linearAccelerationCovariance = ToDiagonalCovarianceMatrix(m_linearAccelerationVariance); - m_angularVelocityCovariance = ToDiagonalCovarianceMatrix(m_angularVelocityVariance); - m_orientationCovariance = ToDiagonalCovarianceMatrix(m_orientationVariance); - + + m_linearAccelerationCovariance = ToDiagonalCovarianceMatrix(m_imuConfiguration.m_linearAccelerationVariance); + m_angularVelocityCovariance = ToDiagonalCovarianceMatrix(m_imuConfiguration.m_angularVelocityVariance); + m_orientationCovariance = ToDiagonalCovarianceMatrix(m_imuConfiguration.m_orientationVariance); + ROS2SensorComponent::Activate(); } diff --git a/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.h b/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.h index 99849dbc1..23e14d13d 100644 --- a/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.h +++ b/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.h @@ -17,6 +17,8 @@ #include #include +#include "ImuSensorConfiguration.h" + namespace ROS2 { //! An IMU (Inertial Measurement Unit) sensor Component. @@ -29,6 +31,7 @@ namespace ROS2 public: AZ_COMPONENT(ROS2ImuSensorComponent, "{502A955E-7742-4E23-AD77-5E4063739DCA}", ROS2SensorComponent); ROS2ImuSensorComponent(); + ROS2ImuSensorComponent(const SensorConfiguration& sensorConfiguration, const ImuSensorConfiguration& imuConfiguration); ~ROS2ImuSensorComponent() = default; static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required); static void Reflect(AZ::ReflectContext* context); @@ -39,17 +42,6 @@ namespace ROS2 ////////////////////////////////////////////////////////////////////////// private: - //! Length of filter that removes numerical noise - int m_filterSize{ 10 }; - int m_minFilterSize{ 1 }; - int m_maxFilterSize{ 200 }; - - //! Include gravity acceleration - bool m_includeGravity{ true }; - - //! Measure also absolute rotation - bool m_absoluteRotation{ true }; - std::shared_ptr> m_imuPublisher; sensor_msgs::msg::Imu m_imuMsg; AZ::Vector3 m_previousLinearVelocity = AZ::Vector3::CreateZero(); @@ -58,10 +50,7 @@ namespace ROS2 AZStd::deque m_filterAcceleration; AZStd::deque m_filterAngularVelocity; - AZ::Vector3 m_orientationVariance = AZ::Vector3::CreateZero(); - AZ::Vector3 m_angularVelocityVariance = AZ::Vector3::CreateZero(); - AZ::Vector3 m_linearAccelerationVariance = AZ::Vector3::CreateZero(); - + ImuSensorConfiguration m_imuConfiguration; AZ::Matrix3x3 m_orientationCovariance = AZ::Matrix3x3::CreateZero(); AZ::Matrix3x3 m_angularVelocityCovariance = AZ::Matrix3x3::CreateZero(); diff --git a/Gems/ROS2/Code/Source/Lidar/Lidar2DSensorConfiguration.cpp b/Gems/ROS2/Code/Source/Lidar/Lidar2DSensorConfiguration.cpp index 8247bead3..156b08cc7 100644 --- a/Gems/ROS2/Code/Source/Lidar/Lidar2DSensorConfiguration.cpp +++ b/Gems/ROS2/Code/Source/Lidar/Lidar2DSensorConfiguration.cpp @@ -28,7 +28,7 @@ namespace ROS2 if (AZ::EditContext* ec = serializeContext->GetEditContext()) { - ec->Class("Lidar 2D Sensor configuration", "ROS2 Lidar 2D sensor configuration") + 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") diff --git a/Gems/ROS2/Code/Source/Lidar/LidarSensorConfiguration.cpp b/Gems/ROS2/Code/Source/Lidar/LidarSensorConfiguration.cpp index 53a8a3dd7..fcac5c602 100644 --- a/Gems/ROS2/Code/Source/Lidar/LidarSensorConfiguration.cpp +++ b/Gems/ROS2/Code/Source/Lidar/LidarSensorConfiguration.cpp @@ -28,7 +28,7 @@ namespace ROS2 if (AZ::EditContext* ec = serializeContext->GetEditContext()) { - ec->Class("Lidar Sensor configuration", "ROS2 Lidar sensor configuration") + ec->Class("ROS2 Lidar Sensor configuration", "Lidar sensor configuration") ->DataElement(AZ::Edit::UIHandlers::ComboBox, &LidarSensorConfiguration::m_lidarModel, "Lidar Model", "Lidar model") ->Attribute(AZ::Edit::Attributes::ChangeNotify, &LidarSensorConfiguration::OnLidarModelSelected) ->EnumAttribute(LidarTemplate::LidarModel::Custom3DLidar, "Custom Lidar") diff --git a/Gems/ROS2/Code/ros2_files.cmake b/Gems/ROS2/Code/ros2_files.cmake index 962f4f12e..61e336f2a 100644 --- a/Gems/ROS2/Code/ros2_files.cmake +++ b/Gems/ROS2/Code/ros2_files.cmake @@ -46,6 +46,8 @@ set(FILES Source/GNSS/GNSSSensorConfiguration.h Source/GNSS/ROS2GNSSSensorComponent.cpp Source/GNSS/ROS2GNSSSensorComponent.h + Source/Imu/ImuSensorConfiguration.cpp + Source/Imu/ImuSensorConfiguration.h Source/Imu/ROS2ImuSensorComponent.cpp Source/Imu/ROS2ImuSensorComponent.h Source/Lidar/LidarRaycaster.cpp From 52d70aa71f2c6216b10221484dca8c267a794edf Mon Sep 17 00:00:00 2001 From: Jan Hanca Date: Thu, 3 Aug 2023 17:25:34 +0200 Subject: [PATCH 5/7] modify editor view: ShowChildrenOnly Signed-off-by: Jan Hanca --- Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp | 3 ++- Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.cpp | 3 ++- Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp | 3 ++- Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp | 3 ++- 4 files changed, 8 insertions(+), 4 deletions(-) diff --git a/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp b/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp index 2b965c27d..65f889c14 100644 --- a/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp @@ -45,7 +45,8 @@ namespace ROS2 AZ::Edit::UIHandlers::Default, &ROS2GNSSSensorComponent::m_gnssConfiguration, "GNSS sensor configuration", - "GNSS sensor configuration"); + "GNSS sensor configuration") + ->Attribute(AZ::Edit::Attributes::Visibility, AZ::Edit::PropertyVisibility::ShowChildrenOnly); } } } diff --git a/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.cpp b/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.cpp index 4ce42b280..54c1deddc 100644 --- a/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.cpp @@ -48,7 +48,8 @@ namespace ROS2 AZ::Edit::UIHandlers::Default, &ROS2ImuSensorComponent::m_imuConfiguration, "Imu sensor configuration", - "Imu sensor configuration"); + "Imu sensor configuration") + ->Attribute(AZ::Edit::Attributes::Visibility, AZ::Edit::PropertyVisibility::ShowChildrenOnly); } } } diff --git a/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp b/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp index a3bec6b5e..01d106904 100644 --- a/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp @@ -41,7 +41,8 @@ namespace ROS2 AZ::Edit::UIHandlers::ComboBox, &ROS2Lidar2DSensorComponent::m_lidarConfiguration, "Lidar configuration", - "Lidar configuration"); + "Lidar configuration") + ->Attribute(AZ::Edit::Attributes::Visibility, AZ::Edit::PropertyVisibility::ShowChildrenOnly); } } } diff --git a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp index 495914a0d..9aed4ad86 100644 --- a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp @@ -41,7 +41,8 @@ namespace ROS2 AZ::Edit::UIHandlers::ComboBox, &ROS2LidarSensorComponent::m_lidarConfiguration, "Lidar configuration", - "Lidar configuration"); + "Lidar configuration") + ->Attribute(AZ::Edit::Attributes::Visibility, AZ::Edit::PropertyVisibility::ShowChildrenOnly); } } } From e6aafb902c13c92570ff0533f9b985a0ecd1592a Mon Sep 17 00:00:00 2001 From: Jan Hanca Date: Fri, 4 Aug 2023 10:44:32 +0200 Subject: [PATCH 6/7] update prefabs to new format Signed-off-by: Jan Hanca --- .../Assets/ROSBot_slamtec.prefab | 30 +++++++------ .../Assets/ROSbot_velodyne.prefab | 44 ++++++++++--------- 2 files changed, 39 insertions(+), 35 deletions(-) diff --git a/Gems/RosRobotSample/Assets/ROSBot_slamtec.prefab b/Gems/RosRobotSample/Assets/ROSBot_slamtec.prefab index f6de3a1eb..d5b3f919a 100755 --- a/Gems/RosRobotSample/Assets/ROSBot_slamtec.prefab +++ b/Gems/RosRobotSample/Assets/ROSBot_slamtec.prefab @@ -616,20 +616,22 @@ } } }, - "lidarModel": 1, - "lidarImplementation": "Scene Queries", - "LidarParameters": { - "Name": "CustomLidar2D", - "Points per layer": 921, - "Min horizontal angle": -180.0, - "Max horizontal angle": 180.0, - "Min range": 0.10000000149011612, - "Max range": 40.0 - }, - "IgnoreLayer": true, - "IgnoredLayerIndex": 1, - "ExcludedEntities": [], - "PointsAtMax": false + "lidarConfiguration": { + "lidarModel": 7, + "lidarImplementation": "Scene Queries", + "LidarParameters": { + "Name": "Slamtec RPLIDAR S1", + "Points per layer": 921, + "Min horizontal angle": -180.0, + "Max horizontal angle": 180.0, + "Min range": 0.10000000149011612, + "Max range": 40.0 + }, + "IgnoreLayer": true, + "IgnoredLayerIndex": 1, + "ExcludedEntities": [], + "PointsAtMax": false + } } }, "TransformComponent": { diff --git a/Gems/RosRobotSample/Assets/ROSbot_velodyne.prefab b/Gems/RosRobotSample/Assets/ROSbot_velodyne.prefab index 9f1ec0886..5e725cc2a 100755 --- a/Gems/RosRobotSample/Assets/ROSbot_velodyne.prefab +++ b/Gems/RosRobotSample/Assets/ROSbot_velodyne.prefab @@ -478,27 +478,29 @@ } } }, - "lidarModel": 4, - "lidarImplementation": "Scene Queries", - "LidarParameters": { - "Name": "Velodyne Puck (VLP-16)", - "Layers": 16, - "Points per layer": 1800, - "Min horizontal angle": -180.0, - "Max horizontal angle": 180.0, - "Min vertical angle": 15.0, - "Max vertical angle": -15.0, - "Max range": 100.0, - "Noise Parameters": { - "Angular noise standard deviation": 0.0, - "Distance noise standard deviation base": 0.029999999329447743, - "Distance noise standard deviation slope": 0.0010000000474974513 - } - }, - "IgnoreLayer": true, - "IgnoredLayerIndex": 1, - "ExcludedEntities": [], - "PointsAtMax": false + "lidarConfiguration": { + "lidarModel": 4, + "lidarImplementation": "Scene Queries", + "LidarParameters": { + "Name": "Velodyne Puck (VLP-16)", + "Layers": 16, + "Points per layer": 1800, + "Min horizontal angle": -180.0, + "Max horizontal angle": 180.0, + "Min vertical angle": 15.0, + "Max vertical angle": -15.0, + "Max range": 100.0, + "Noise Parameters": { + "Angular noise standard deviation": 0.0, + "Distance noise standard deviation base": 0.029999999329447743, + "Distance noise standard deviation slope": 0.0010000000474974513 + } + }, + "IgnoreLayer": true, + "IgnoredLayerIndex": 1, + "ExcludedEntities": [], + "PointsAtMax": false + } } }, "TransformComponent": { From 990f32c44b4355612803750dba65a97e3de2bb89 Mon Sep 17 00:00:00 2001 From: Jan Hanca Date: Fri, 4 Aug 2023 11:43:00 +0200 Subject: [PATCH 7/7] EN_UK -> EN_US: visualise -> visualize Signed-off-by: Jan Hanca --- .../Code/Include/ROS2/Sensor/ROS2SensorComponent.h | 6 +++--- .../Code/Include/ROS2/Sensor/SensorConfiguration.h | 4 ++-- .../Source/Camera/ROS2CameraSensorEditorComponent.cpp | 2 +- .../Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp | 10 +++++----- .../Code/Source/Lidar/ROS2Lidar2DSensorComponent.h | 2 +- .../Code/Source/Lidar/ROS2LidarSensorComponent.cpp | 6 +++--- Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.h | 2 +- .../Source/ProximitySensor/ROS2ProximitySensor.cpp | 4 ++-- .../Code/Source/ProximitySensor/ROS2ProximitySensor.h | 2 +- Gems/ROS2/Code/Source/Sensor/ROS2SensorComponent.cpp | 2 +- Gems/ROS2/Code/Source/Sensor/SensorConfiguration.cpp | 4 ++-- Gems/RosRobotSample/Assets/ROSBot_slamtec.prefab | 2 +- Gems/RosRobotSample/Assets/ROSbot_velodyne.prefab | 2 +- 13 files changed, 24 insertions(+), 24 deletions(-) diff --git a/Gems/ROS2/Code/Include/ROS2/Sensor/ROS2SensorComponent.h b/Gems/ROS2/Code/Include/ROS2/Sensor/ROS2SensorComponent.h index 28afa20f4..400c27b89 100644 --- a/Gems/ROS2/Code/Include/ROS2/Sensor/ROS2SensorComponent.h +++ b/Gems/ROS2/Code/Include/ROS2/Sensor/ROS2SensorComponent.h @@ -59,10 +59,10 @@ namespace ROS2 virtual void FrequencyTick(){}; private: - //! Visualise sensor operation. + //! Visualize sensor operation. //! For example, draw points or rays for a lidar, viewport for a camera, etc. - //! Visualisation can be turned on or off in SensorConfiguration. - virtual void Visualise(){}; + //! Visualization can be turned on or off in SensorConfiguration. + virtual void Visualize(){}; //! The number of ticks that are expected to pass to trigger next measurement. AZ::s32 m_tickCountDown{ 0 }; diff --git a/Gems/ROS2/Code/Include/ROS2/Sensor/SensorConfiguration.h b/Gems/ROS2/Code/Include/ROS2/Sensor/SensorConfiguration.h index dea9cb284..4f3c6223d 100644 --- a/Gems/ROS2/Code/Include/ROS2/Sensor/SensorConfiguration.h +++ b/Gems/ROS2/Code/Include/ROS2/Sensor/SensorConfiguration.h @@ -17,7 +17,7 @@ namespace ROS2 { //! General configuration for sensors. //! All sensors can be set to a certain frequency, have their data published or not, - //! and visualised or not. + //! and visualized or not. struct SensorConfiguration { public: @@ -34,7 +34,7 @@ namespace ROS2 float m_frequency = 10; bool m_publishingEnabled = true; //!< Determines whether the sensor is publishing (sending data to ROS 2 ecosystem). - bool m_visualise = true; //!< Determines whether the sensor is visualised in O3DE (for example, point cloud is drawn for LIDAR). + bool m_visualize = true; //!< Determines whether the sensor is visualized in O3DE (for example, point cloud is drawn for LIDAR). private: // Frequency limit is once per day. static constexpr float m_minFrequency = AZStd::numeric_limits::epsilon(); diff --git a/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.cpp b/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.cpp index edee40e15..c9061f904 100644 --- a/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.cpp +++ b/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.cpp @@ -126,7 +126,7 @@ namespace ROS2 void ROS2CameraSensorEditorComponent::DisplayEntityViewport( [[maybe_unused]] const AzFramework::ViewportInfo& viewportInfo, AzFramework::DebugDisplayRequests& debugDisplay) { - if (!m_sensorConfiguration.m_visualise) + if (!m_sensorConfiguration.m_visualize) { return; } diff --git a/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp b/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp index 01d106904..6d021b070 100644 --- a/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp @@ -87,7 +87,7 @@ namespace ROS2 } RaycastResultFlags requestedFlags = RaycastResultFlags::Ranges; - if (m_sensorConfiguration.m_visualise) + if (m_sensorConfiguration.m_visualize) { requestedFlags |= RaycastResultFlags::Points; } @@ -134,7 +134,7 @@ namespace ROS2 m_sensorConfiguration = sensorConfiguration; } - void ROS2Lidar2DSensorComponent::Visualise() + void ROS2Lidar2DSensorComponent::Visualize() { if (m_visualizationPoints.empty()) { @@ -164,7 +164,7 @@ namespace ROS2 AZStd::string fullTopic = ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic); m_laserScanPublisher = ros2Node->create_publisher(fullTopic.data(), publisherConfig.GetQoS()); - if (m_sensorConfiguration.m_visualise) + if (m_sensorConfiguration.m_visualize) { auto* entityScene = AZ::RPI::Scene::GetSceneForEntityId(GetEntityId()); m_drawQueue = AZ::RPI::AuxGeomFeatureProcessorInterface::GetDrawQueueForScene(entityScene); @@ -190,7 +190,7 @@ namespace ROS2 auto entityTransform = GetEntity()->FindComponent(); RaycastResultFlags requestedFlags = RaycastResultFlags::Ranges; - if (m_sensorConfiguration.m_visualise) + if (m_sensorConfiguration.m_visualize) { requestedFlags |= RaycastResultFlags::Points; } @@ -202,7 +202,7 @@ namespace ROS2 return; } - if (m_sensorConfiguration.m_visualise) + if (m_sensorConfiguration.m_visualize) { // Store points for visualization purposes, in global frame m_visualizationPoints = m_lastScanResults.m_points; } diff --git a/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.h b/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.h index 20abcd1c6..97b954201 100644 --- a/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.h +++ b/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.h @@ -42,7 +42,7 @@ namespace ROS2 ////////////////////////////////////////////////////////////////////////// // ROS2SensorComponent overrides void FrequencyTick() override; - void Visualise() override; + void Visualize() override; void ConnectToLidarRaycaster(); void ConfigureLidarRaycaster(); diff --git a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp index 9aed4ad86..ba5e748b5 100644 --- a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp @@ -142,7 +142,7 @@ namespace ROS2 m_sensorConfiguration = sensorConfiguration; } - void ROS2LidarSensorComponent::Visualise() + void ROS2LidarSensorComponent::Visualize() { if (m_visualizationPoints.empty()) { @@ -165,7 +165,7 @@ namespace ROS2 void ROS2LidarSensorComponent::Activate() { - if (m_sensorConfiguration.m_visualise) + if (m_sensorConfiguration.m_visualize) { auto* entityScene = AZ::RPI::Scene::GetSceneForEntityId(GetEntityId()); m_drawQueue = AZ::RPI::AuxGeomFeatureProcessorInterface::GetDrawQueueForScene(entityScene); @@ -231,7 +231,7 @@ namespace ROS2 return; } - if (m_sensorConfiguration.m_visualise) + if (m_sensorConfiguration.m_visualize) { // Store points for visualization purposes, in global frame m_visualizationPoints = m_lastScanResults.m_points; } diff --git a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.h b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.h index fc0a3df18..f441667e2 100644 --- a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.h +++ b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.h @@ -42,7 +42,7 @@ namespace ROS2 ////////////////////////////////////////////////////////////////////////// // ROS2SensorComponent overrides void FrequencyTick() override; - void Visualise() override; + void Visualize() override; void ConnectToLidarRaycaster(); void ConfigureLidarRaycaster(); diff --git a/Gems/ROS2/Code/Source/ProximitySensor/ROS2ProximitySensor.cpp b/Gems/ROS2/Code/Source/ProximitySensor/ROS2ProximitySensor.cpp index 5b5a9ed49..1c8aa84f3 100644 --- a/Gems/ROS2/Code/Source/ProximitySensor/ROS2ProximitySensor.cpp +++ b/Gems/ROS2/Code/Source/ProximitySensor/ROS2ProximitySensor.cpp @@ -78,7 +78,7 @@ namespace ROS2 AZStd::string fullTopic = ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic); m_detectionPublisher = ros2Node->create_publisher(fullTopic.data(), publisherConfig.GetQoS()); - if (m_sensorConfiguration.m_visualise) + if (m_sensorConfiguration.m_visualize) { auto* entityScene = AZ::RPI::Scene::GetSceneForEntityId(GetEntityId()); m_drawQueue = AZ::RPI::AuxGeomFeatureProcessorInterface::GetDrawQueueForScene(entityScene); @@ -99,7 +99,7 @@ namespace ROS2 ROS2SensorComponent::Deactivate(); } - void ROS2ProximitySensor::Visualise() + void ROS2ProximitySensor::Visualize() { if (m_drawQueue) { diff --git a/Gems/ROS2/Code/Source/ProximitySensor/ROS2ProximitySensor.h b/Gems/ROS2/Code/Source/ProximitySensor/ROS2ProximitySensor.h index 273bb1891..8aca4971c 100644 --- a/Gems/ROS2/Code/Source/ProximitySensor/ROS2ProximitySensor.h +++ b/Gems/ROS2/Code/Source/ProximitySensor/ROS2ProximitySensor.h @@ -35,7 +35,7 @@ namespace ROS2 ////////////////////////////////////////////////////////////////////////// // ROS2SensorComponent overrides void FrequencyTick() override; - void Visualise() override; + void Visualize() override; ////////////////////////////////////////////////////////////////////////// AZ::Vector3 m_detectionDirection = AZ::Vector3::CreateAxisX(); diff --git a/Gems/ROS2/Code/Source/Sensor/ROS2SensorComponent.cpp b/Gems/ROS2/Code/Source/Sensor/ROS2SensorComponent.cpp index b213edd70..861441bad 100644 --- a/Gems/ROS2/Code/Source/Sensor/ROS2SensorComponent.cpp +++ b/Gems/ROS2/Code/Source/Sensor/ROS2SensorComponent.cpp @@ -70,7 +70,7 @@ namespace ROS2 void ROS2SensorComponent::OnTick([[maybe_unused]] float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time) { - Visualise(); // each frame + Visualize(); // each frame if (m_onTickCall) { m_onTickCall(); diff --git a/Gems/ROS2/Code/Source/Sensor/SensorConfiguration.cpp b/Gems/ROS2/Code/Source/Sensor/SensorConfiguration.cpp index 776ff4a71..50bc5857e 100644 --- a/Gems/ROS2/Code/Source/Sensor/SensorConfiguration.cpp +++ b/Gems/ROS2/Code/Source/Sensor/SensorConfiguration.cpp @@ -20,7 +20,7 @@ namespace ROS2 serializeContext->RegisterGenericType>>(); serializeContext->Class() ->Version(2) - ->Field("Visualise", &SensorConfiguration::m_visualise) + ->Field("Visualize", &SensorConfiguration::m_visualize) ->Field("Publishing Enabled", &SensorConfiguration::m_publishingEnabled) ->Field("Frequency (HZ)", &SensorConfiguration::m_frequency) ->Field("Publishers", &SensorConfiguration::m_publishersConfigurations); @@ -29,7 +29,7 @@ namespace ROS2 { ec->Class("ROS2 Sensor Configuration", "Sensor configuration") ->ClassElement(AZ::Edit::ClassElements::EditorData, "") - ->DataElement(AZ::Edit::UIHandlers::Default, &SensorConfiguration::m_visualise, "Visualise", "Visualise") + ->DataElement(AZ::Edit::UIHandlers::Default, &SensorConfiguration::m_visualize, "Visualize", "Visualize") ->DataElement( AZ::Edit::UIHandlers::Default, &SensorConfiguration::m_publishingEnabled, diff --git a/Gems/RosRobotSample/Assets/ROSBot_slamtec.prefab b/Gems/RosRobotSample/Assets/ROSBot_slamtec.prefab index d5b3f919a..722da9f35 100755 --- a/Gems/RosRobotSample/Assets/ROSBot_slamtec.prefab +++ b/Gems/RosRobotSample/Assets/ROSBot_slamtec.prefab @@ -601,7 +601,7 @@ "$type": "ROS2Lidar2DSensorComponent", "Id": 0, "SensorConfiguration": { - "Visualise": true, + "Visualize": true, "Publishing Enabled": true, "Frequency (HZ)": 10.0, "Publishers": { diff --git a/Gems/RosRobotSample/Assets/ROSbot_velodyne.prefab b/Gems/RosRobotSample/Assets/ROSbot_velodyne.prefab index 5e725cc2a..e9cdbc8db 100755 --- a/Gems/RosRobotSample/Assets/ROSbot_velodyne.prefab +++ b/Gems/RosRobotSample/Assets/ROSbot_velodyne.prefab @@ -463,7 +463,7 @@ "$type": "ROS2LidarSensorComponent", "Id": 0, "SensorConfiguration": { - "Visualise": true, + "Visualize": true, "Publishing Enabled": true, "Frequency (HZ)": 10.0, "Publishers": {