From 603f29e0c1e175ea346f13dd6a6bfade0f094897 Mon Sep 17 00:00:00 2001 From: Jan Hanca <134940295+jhanca-robotecai@users.noreply.github.com> Date: Mon, 4 Sep 2023 11:28:01 +0200 Subject: [PATCH] Add SDFormat importer hooks (#453) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Add sensors SDF importer hooks Signed-off-by: Jan Hanca Signed-off-by: Michał Pełka --- .../Code/Source/Lidar/LidarTemplateUtils.cpp | 50 ++-- .../ROS2RobotControlComponent.cpp | 2 +- .../RobotControl/ROS2RobotControlComponent.h | 2 +- .../ROS2CameraSensorHook.cpp} | 55 ++-- .../SDFormat/Hooks/ROS2GNSSSensorHook.cpp | 50 ++++ .../SDFormat/Hooks/ROS2ImuSensorHook.cpp | 101 +++++++ .../SDFormat/Hooks/ROS2LidarSensorHook.cpp | 87 ++++++ .../RobotImporter/SDFormat/ROS2SensorHooks.h | 11 + .../SDFormat/ROS2SensorHooksUtils.cpp | 28 ++ .../SDFormat/ROS2SensorHooksUtils.h | 78 +++++ Gems/ROS2/Code/Tests/SdfParserTest.cpp | 272 +++++++++++++----- Gems/ROS2/Code/ros2_editor_files.cmake | 7 +- 12 files changed, 616 insertions(+), 127 deletions(-) rename Gems/ROS2/Code/Source/RobotImporter/SDFormat/{ROS2SensorHooks.cpp => Hooks/ROS2CameraSensorHook.cpp} (74%) create mode 100644 Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2GNSSSensorHook.cpp create mode 100644 Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2ImuSensorHook.cpp create mode 100644 Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2LidarSensorHook.cpp create mode 100644 Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooksUtils.cpp create mode 100644 Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooksUtils.h diff --git a/Gems/ROS2/Code/Source/Lidar/LidarTemplateUtils.cpp b/Gems/ROS2/Code/Source/Lidar/LidarTemplateUtils.cpp index a6d7f66c8..c5c6d758d 100644 --- a/Gems/ROS2/Code/Source/Lidar/LidarTemplateUtils.cpp +++ b/Gems/ROS2/Code/Source/Lidar/LidarTemplateUtils.cpp @@ -148,43 +148,43 @@ namespace ROS2 { /*.m_model = */ Model::Custom2DLidar, /*.m_name = */ "CustomLidar2D", - /*.m_minHAngle = */ -180.0f, - /*.m_maxHAngle = */ 180.0f, - /*.m_minVAngle = */ 0.f, - /*.m_maxVAngle = */ 0.f, - /*.m_layers = */ 1, - /*.m_numberOfIncrements = */ 924, - /*.m_minRange = */ 0.0f, - /*.m_maxRange = */ 100.0f, - /*.m_noiseParameters = */ - { - /*.m_angularNoiseStdDev = */ 0.0f, + /*.m_minHAngle = */ -180.0f, + /*.m_maxHAngle = */ 180.0f, + /*.m_minVAngle = */ 0.f, + /*.m_maxVAngle = */ 0.f, + /*.m_layers = */ 1, + /*.m_numberOfIncrements = */ 924, + /*.m_minRange = */ 0.0f, + /*.m_maxRange = */ 100.0f, + /*.m_noiseParameters = */ + { + /*.m_angularNoiseStdDev = */ 0.0f, /*.m_distanceNoiseStdDevBase = */ 0.02f, /*.m_distanceNoiseStdDevRisePerMeter = */ 0.001f, }, - } + }, }, { Model::Slamtec_RPLIDAR_S1, { /*.m_model = */ Model::Slamtec_RPLIDAR_S1, /*.m_name = */ "Slamtec RPLIDAR S1", - /*.m_minHAngle = */ -180.0f, - /*.m_maxHAngle = */ 180.0f, - /*.m_minVAngle = */ 0.f, - /*.m_maxVAngle = */ 0.f, - /*.m_layers = */ 1, - /*.m_numberOfIncrements = */ 921, - /*.m_minRange = */ 0.1f, - /*.m_maxRange = */ 40.0f, - /*.m_noiseParameters = */ - { - /*.m_angularNoiseStdDev = */ 0.0f, + /*.m_minHAngle = */ -180.0f, + /*.m_maxHAngle = */ 180.0f, + /*.m_minVAngle = */ 0.f, + /*.m_maxVAngle = */ 0.f, + /*.m_layers = */ 1, + /*.m_numberOfIncrements = */ 921, + /*.m_minRange = */ 0.1f, + /*.m_maxRange = */ 40.0f, + /*.m_noiseParameters = */ + { + /*.m_angularNoiseStdDev = */ 0.0f, /*.m_distanceNoiseStdDevBase = */ 0.02f, /*.m_distanceNoiseStdDevRisePerMeter = */ 0.001f, }, - } - }, + }, + } }; auto it = templates.find(model); diff --git a/Gems/ROS2/Code/Source/RobotControl/ROS2RobotControlComponent.cpp b/Gems/ROS2/Code/Source/RobotControl/ROS2RobotControlComponent.cpp index 719a9dbee..680f382d7 100644 --- a/Gems/ROS2/Code/Source/RobotControl/ROS2RobotControlComponent.cpp +++ b/Gems/ROS2/Code/Source/RobotControl/ROS2RobotControlComponent.cpp @@ -89,7 +89,7 @@ namespace ROS2 return m_controlConfiguration; } - const TopicConfiguration& ROS2RobotControlComponent::GetSubscriberConfigration() const + const TopicConfiguration& ROS2RobotControlComponent::GetSubscriberConfiguration() const { return m_subscriberConfiguration; } diff --git a/Gems/ROS2/Code/Source/RobotControl/ROS2RobotControlComponent.h b/Gems/ROS2/Code/Source/RobotControl/ROS2RobotControlComponent.h index 149daa312..48975441a 100644 --- a/Gems/ROS2/Code/Source/RobotControl/ROS2RobotControlComponent.h +++ b/Gems/ROS2/Code/Source/RobotControl/ROS2RobotControlComponent.h @@ -32,7 +32,7 @@ namespace ROS2 void SetControlConfiguration(const ControlConfiguration& controlConfiguration); - const TopicConfiguration& GetSubscriberConfigration() const; + const TopicConfiguration& GetSubscriberConfiguration() const; void SetSubscriberConfiguration(const TopicConfiguration& subscriberConfiguration); diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp similarity index 74% rename from Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.cpp rename to Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp index d3a2e06ea..04f9c7929 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp @@ -6,33 +6,17 @@ * */ -#include "ROS2SensorHooks.h" - #include #include -#include -#include +#include +#include +#include #include -#include +#include namespace ROS2::SDFormat { - namespace Internal - { - void AddTopicConfiguration( - SensorConfiguration& sensorConfig, - const AZStd::string& topic, - const AZStd::string& messageType, - const AZStd::string& configName) - { - TopicConfiguration config; - config.m_topic = topic; - config.m_type = messageType; - sensorConfig.m_publishersConfigurations.insert(AZStd::make_pair(configName, config)); - } - } // namespace Internal - SensorImporterHook ROS2SensorHooks::ROS2CameraSensor() { SensorImporterHook importerHook; @@ -49,14 +33,22 @@ namespace ROS2::SDFormat const sdf::Sensor& sdfSensor) -> SensorImporterHook::ConvertSensorOutcome { auto* cameraSensor = sdfSensor.CameraSensor(); + if (!cameraSensor) + { + return AZ::Failure(AZStd::string("Failed to read parsed SDFormat data of %s camera sensor", sdfSensor.Name().c_str())); + } CameraSensorConfiguration cameraConfiguration; cameraConfiguration.m_depthCamera = cameraSensor->HasDepthCamera(); cameraConfiguration.m_colorCamera = (sdfSensor.Type() != sdf::SensorType::DEPTH_CAMERA) ? true : false; cameraConfiguration.m_width = cameraSensor->ImageWidth(); cameraConfiguration.m_height = cameraSensor->ImageHeight(); - cameraConfiguration.m_verticalFieldOfViewDeg = - cameraSensor->HorizontalFov().Degree() * (cameraConfiguration.m_height / cameraConfiguration.m_width); + if (cameraConfiguration.m_width != 0) + { + double aspectRatio = static_cast(cameraConfiguration.m_height) / cameraConfiguration.m_width; + cameraConfiguration.m_verticalFieldOfViewDeg = + 2.0 * AZStd::atan(AZStd::tan(cameraSensor->HorizontalFov().Radian() / 2.0) * aspectRatio); + } if (sdfSensor.Type() != sdf::SensorType::DEPTH_CAMERA) { cameraConfiguration.m_nearClipDistance = static_cast(cameraSensor->NearClip()); @@ -71,21 +63,25 @@ namespace ROS2::SDFormat SensorConfiguration sensorConfiguration; sensorConfiguration.m_frequency = sdfSensor.UpdateRate(); if (sdfSensor.Type() != sdf::SensorType::DEPTH_CAMERA) - { - Internal::AddTopicConfiguration( + { // COLOR_CAMERA and RGBD_CAMERA + Utils::AddTopicConfiguration( sensorConfiguration, "camera_image_color", CameraConstants::ImageMessageType, CameraConstants::ColorImageConfig); - Internal::AddTopicConfiguration( + Utils::AddTopicConfiguration( sensorConfiguration, "color_camera_info", CameraConstants::CameraInfoMessageType, CameraConstants::ColorInfoConfig); } if (sdfSensor.Type() != sdf::SensorType::CAMERA) - { - Internal::AddTopicConfiguration( + { // DEPTH_CAMERA and RGBD_CAMERA + Utils::AddTopicConfiguration( sensorConfiguration, "camera_image_depth", CameraConstants::ImageMessageType, CameraConstants::DepthImageConfig); - Internal::AddTopicConfiguration( + Utils::AddTopicConfiguration( sensorConfiguration, "depth_camera_info", CameraConstants::CameraInfoMessageType, CameraConstants::DepthInfoConfig); } - if (entity.CreateComponent(sensorConfiguration, cameraConfiguration)) + // Create required components + Utils::CreateComponent(entity); + + // Create Camera component + if (Utils::CreateComponent(entity, sensorConfiguration, cameraConfiguration)) { return AZ::Success(); } @@ -97,5 +93,4 @@ namespace ROS2::SDFormat return importerHook; } - } // namespace ROS2::SDFormat diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2GNSSSensorHook.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2GNSSSensorHook.cpp new file mode 100644 index 000000000..0a8485f1a --- /dev/null +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2GNSSSensorHook.cpp @@ -0,0 +1,50 @@ +/* + * Copyright (c) Contributors to the Open 3D Engine Project. + * For complete copyright and license terms please see the LICENSE at the root of this distribution. + * + * SPDX-License-Identifier: Apache-2.0 OR MIT + * + */ + +#include +#include +#include + +#include +#include + +namespace ROS2::SDFormat +{ + SensorImporterHook ROS2SensorHooks::ROS2GNSSSensor() + { + SensorImporterHook importerHook; + importerHook.m_sensorTypes = AZStd::unordered_set{ sdf::SensorType::NAVSAT }; + importerHook.m_supportedSensorParams = AZStd::unordered_set{ ">update_rate" }; + importerHook.m_pluginNames = AZStd::unordered_set{ "libgazebo_ros_gps_sensor.so" }; + importerHook.m_supportedPluginParams = AZStd::unordered_set{}; + importerHook.m_sdfSensorToComponentCallback = [](AZ::Entity& entity, + const sdf::Sensor& sdfSensor) -> SensorImporterHook::ConvertSensorOutcome + { + if (!sdfSensor.NavSatSensor()) + { + return AZ::Failure(AZStd::string("Failed to read parsed SDFormat data of %s NavSat sensor", sdfSensor.Name().c_str())); + } + + SensorConfiguration sensorConfiguration; + sensorConfiguration.m_frequency = sdfSensor.UpdateRate(); + const AZStd::string messageType = "sensor_msgs::msg::NavSatFix"; + Utils::AddTopicConfiguration(sensorConfiguration, "gnss", messageType, messageType); + + if (Utils::CreateComponent(entity, sensorConfiguration, GNSSSensorConfiguration())) + { + return AZ::Success(); + } + else + { + return AZ::Failure(AZStd::string("Failed to create ROS2 GNSS Sensor component")); + } + }; + + return importerHook; + } +} // namespace ROS2::SDFormat diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2ImuSensorHook.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2ImuSensorHook.cpp new file mode 100644 index 000000000..aa135ab2d --- /dev/null +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2ImuSensorHook.cpp @@ -0,0 +1,101 @@ +/* + * Copyright (c) Contributors to the Open 3D Engine Project. + * For complete copyright and license terms please see the LICENSE at the root of this distribution. + * + * SPDX-License-Identifier: Apache-2.0 OR MIT + * + */ + +#include +#include +#include +#include +#include + +#include +#include + +namespace ROS2::SDFormat +{ + SensorImporterHook ROS2SensorHooks::ROS2ImuSensor() + { + SensorImporterHook importerHook; + importerHook.m_sensorTypes = AZStd::unordered_set{ sdf::SensorType::IMU }; + importerHook.m_supportedSensorParams = AZStd::unordered_set{ ">update_rate", + ">imu>angular_velocity>x>noise>mean", + ">imu>angular_velocity>x>noise>stddev", + ">imu>angular_velocity>y>noise>mean", + ">imu>angular_velocity>y>noise>stddev", + ">imu>angular_velocity>z>noise>mean", + ">imu>angular_velocity>z>noise>stddev", + ">imu>linear_acceleration>x>noise>mean", + ">imu>linear_acceleration>x>noise>stddev", + ">imu>linear_acceleration>y>noise>mean", + ">imu>linear_acceleration>y>noise>stddev", + ">imu>linear_acceleration>z>noise>mean", + ">imu>linear_acceleration>z>noise>stddev" }; + importerHook.m_pluginNames = AZStd::unordered_set{ "libgazebo_ros_imu_sensor.so" }; + importerHook.m_supportedPluginParams = AZStd::unordered_set{}; + importerHook.m_sdfSensorToComponentCallback = [](AZ::Entity& entity, + const sdf::Sensor& sdfSensor) -> SensorImporterHook::ConvertSensorOutcome + { + auto* imuSensor = sdfSensor.ImuSensor(); + if (!imuSensor) + { + return AZ::Failure(AZStd::string("Failed to read parsed SDFormat data of %s imu sensor", sdfSensor.Name().c_str())); + } + + ImuSensorConfiguration imuConfiguration; + const auto& angVelXNoise = imuSensor->AngularVelocityXNoise(); + const auto& angVelYNoise = imuSensor->AngularVelocityYNoise(); + const auto& angVelZNoise = imuSensor->AngularVelocityZNoise(); + if (angVelXNoise.Type() == sdf::NoiseType::GAUSSIAN && angVelYNoise.Type() == sdf::NoiseType::GAUSSIAN && + angVelZNoise.Type() == sdf::NoiseType::GAUSSIAN) + { + if (angVelXNoise.Mean() == 0.0 && angVelYNoise.Mean() == 0.0 && angVelZNoise.Mean() == 0.0) + { + imuConfiguration.m_angularVelocityVariance = AZ::Vector3( + static_cast(angVelXNoise.StdDev() * angVelXNoise.StdDev()), + static_cast(angVelYNoise.StdDev() * angVelYNoise.StdDev()), + static_cast(angVelZNoise.StdDev() * angVelZNoise.StdDev())); + } + } + + const auto& linAccXNoise = imuSensor->LinearAccelerationXNoise(); + const auto& linAccYNoise = imuSensor->LinearAccelerationYNoise(); + const auto& linAccZNoise = imuSensor->LinearAccelerationZNoise(); + if (linAccXNoise.Type() == sdf::NoiseType::GAUSSIAN && linAccYNoise.Type() == sdf::NoiseType::GAUSSIAN && + linAccZNoise.Type() == sdf::NoiseType::GAUSSIAN) + { + if (linAccXNoise.Mean() == 0.0 && linAccYNoise.Mean() == 0.0 && linAccZNoise.Mean() == 0.0) + { + imuConfiguration.m_linearAccelerationVariance = AZ::Vector3( + static_cast(linAccXNoise.StdDev() * linAccXNoise.StdDev()), + static_cast(linAccYNoise.StdDev() * linAccYNoise.StdDev()), + static_cast(linAccZNoise.StdDev() * linAccZNoise.StdDev())); + } + } + + SensorConfiguration sensorConfiguration; + sensorConfiguration.m_frequency = sdfSensor.UpdateRate(); + const AZStd::string messageType = "sensor_msgs::msg::Imu"; + Utils::AddTopicConfiguration(sensorConfiguration, "imu", messageType, messageType); + + // Create required components + Utils::CreateComponent(entity); + Utils::CreateComponent(entity); + + // Create Imu component + if (Utils::CreateComponent(entity, sensorConfiguration, imuConfiguration)) + { + return AZ::Success(); + } + else + { + return AZ::Failure(AZStd::string("Failed to create ROS2 Imu Sensor component")); + } + }; + + return importerHook; + } +} // namespace ROS2::SDFormat diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2LidarSensorHook.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2LidarSensorHook.cpp new file mode 100644 index 000000000..fdd09cc64 --- /dev/null +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2LidarSensorHook.cpp @@ -0,0 +1,87 @@ +/* + * Copyright (c) Contributors to the Open 3D Engine Project. + * For complete copyright and license terms please see the LICENSE at the root of this distribution. + * + * SPDX-License-Identifier: Apache-2.0 OR MIT + * + */ + +#include +#include +#include +#include + +#include +#include + +namespace ROS2::SDFormat +{ + SensorImporterHook ROS2SensorHooks::ROS2LidarSensor() + { + SensorImporterHook importerHook; + importerHook.m_sensorTypes = AZStd::unordered_set{ sdf::SensorType::LIDAR }; + importerHook.m_supportedSensorParams = AZStd::unordered_set{ + ">update_rate", + ">lidar>scan>horizontal>samples", + ">lidar>scan>horizontal>min_angle", + ">lidar>scan>horizontal>max_angle", + ">lidar>scan>vertical>samples", + ">lidar>scan>vertical>min_angle", + ">lidar>scan>vertical>max_angle", + ">lidar>range>min", + ">lidar>range>max", + // Gazebo-classic + ">ray>scan>horizontal>samples", + ">ray>scan>horizontal>min_angle", + ">ray>scan>horizontal>max_angle", + ">ray>scan>vertical>samples", + ">ray>scan>vertical>min_angle", + ">ray>scan>vertical>max_angle", + ">ray>range>min", + ">ray>range>max", + }; + importerHook.m_pluginNames = AZStd::unordered_set{ "libgazebo_ros_ray_sensor.so", "libgazebo_ros_laser.so" }; + importerHook.m_supportedPluginParams = AZStd::unordered_set{}; + importerHook.m_sdfSensorToComponentCallback = [](AZ::Entity& entity, + const sdf::Sensor& sdfSensor) -> SensorImporterHook::ConvertSensorOutcome + { + auto* lidarSensor = sdfSensor.LidarSensor(); + if (!lidarSensor) + { + return AZ::Failure(AZStd::string("Failed to read parsed SDFormat data of %s Lidar sensor", sdfSensor.Name().c_str())); + } + + SensorConfiguration sensorConfiguration; + sensorConfiguration.m_frequency = sdfSensor.UpdateRate(); + const AZStd::string messageType = "sensor_msgs::msg::PointCloud2"; + Utils::AddTopicConfiguration(sensorConfiguration, "pc", messageType, messageType); + + LidarSensorConfiguration lidarConfiguration; + lidarConfiguration.m_lidarParameters.m_model = LidarTemplate::LidarModel::Custom3DLidar; + lidarConfiguration.m_lidarParameters.m_name = AZStd::string(sdfSensor.Name().c_str()); + lidarConfiguration.m_lidarParameters.m_minHAngle = lidarSensor->HorizontalScanMinAngle().Degree(); + lidarConfiguration.m_lidarParameters.m_maxHAngle = lidarSensor->HorizontalScanMaxAngle().Degree(); + lidarConfiguration.m_lidarParameters.m_minVAngle = lidarSensor->VerticalScanMinAngle().Degree(); + lidarConfiguration.m_lidarParameters.m_maxVAngle = lidarSensor->VerticalScanMaxAngle().Degree(); + lidarConfiguration.m_lidarParameters.m_layers = lidarSensor->HorizontalScanSamples(); + lidarConfiguration.m_lidarParameters.m_numberOfIncrements = lidarSensor->VerticalScanSamples(); + lidarConfiguration.m_lidarParameters.m_minRange = lidarSensor->RangeMin(); + lidarConfiguration.m_lidarParameters.m_maxRange = lidarSensor->RangeMax(); + + // Create required components + Utils::CreateComponent(entity); + + // Create Lidar component + if (Utils::CreateComponent(entity, sensorConfiguration, lidarConfiguration)) + { + return AZ::Success(); + } + else + { + return AZ::Failure(AZStd::string("Failed to create ROS2 Lidar Sensor component")); + } + }; + + return importerHook; + } +} // namespace ROS2::SDFormat diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.h b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.h index 32d2db0d8..2ba4d2636 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.h +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.h @@ -14,6 +14,17 @@ namespace ROS2::SDFormat { namespace ROS2SensorHooks { + //! Retrieve a sensor importer hook which is used to map SDFormat sensor of type camera, depth or rgbd into O3DE + //! ROS2CameraSensorComponent SensorImporterHook ROS2CameraSensor(); + + //! Retrieve a sensor importer hook which is used to map SDFormat sensor of type navsat into O3DE ROS2GNSSSensorComponent + SensorImporterHook ROS2GNSSSensor(); + + //! Retrieve a sensor importer hook which is used to map SDFormat sensor of type imu into O3DE ROS2ImuSensorComponent + SensorImporterHook ROS2ImuSensor(); + + //! Retrieve a sensor importer hook which is used to map SDFormat sensor of type lidar into O3DE ROS2LidarSensorComponent + SensorImporterHook ROS2LidarSensor(); } // namespace ROS2SensorHooks } // namespace ROS2::SDFormat diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooksUtils.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooksUtils.cpp new file mode 100644 index 000000000..004a5755a --- /dev/null +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooksUtils.cpp @@ -0,0 +1,28 @@ +/* + * 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 "ROS2SensorHooksUtils.h" +#include + +namespace ROS2::SDFormat +{ + namespace ROS2SensorHooks + { + void Utils::AddTopicConfiguration( + SensorConfiguration& sensorConfig, + const AZStd::string& topic, + const AZStd::string& messageType, + const AZStd::string& configName) + { + TopicConfiguration config; + config.m_topic = topic; + config.m_type = messageType; + sensorConfig.m_publishersConfigurations.insert(AZStd::make_pair(configName, config)); + } + } // namespace ROS2SensorHooks +} // namespace ROS2::SDFormat diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooksUtils.h b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooksUtils.h new file mode 100644 index 000000000..a11aa414c --- /dev/null +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooksUtils.h @@ -0,0 +1,78 @@ +/* + * Copyright (c) Contributors to the Open 3D Engine Project. + * For complete copyright and license terms please see the LICENSE at the root of this distribution. + * + * SPDX-License-Identifier: Apache-2.0 OR MIT + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace ROS2::SDFormat +{ + namespace ROS2SensorHooks + { + namespace Utils + { + //! Add a ROS2 topic configuration to sensor parameters. + //! @param sensorConfig sensor's configuration which hosts multiple topic configurations + //! @param topic ROS2 topic name + //! @param messageType ROS2 message type + //! @param configName name under which topic configuration is stored in sensor's configuration + void AddTopicConfiguration( + SensorConfiguration& sensorConfig, + const AZStd::string& topic, + const AZStd::string& messageType, + const AZStd::string& configName); + + //! Create a component and attach the component to the entity. + //! This method ensures that game components are wrapped into GenericComponentWrapper. + //! @param entity entity to which the new component is added + //! @param args constructor arguments used to create the new component + //! @return A pointer to the component. Returns a null pointer if the component could not be created. + template + AZ::Component* CreateComponent(AZ::Entity& entity, Args&&... args) + { + // Do not create a component if the same type is already added. + if (entity.FindComponent()) + { + return nullptr; + } + + // Create component. + // If it's not an "editor component" then wrap it in a GenericComponentWrapper. + AZ::Component* component = nullptr; + if (AZ::GetRttiHelper() && + AZ::GetRttiHelper()->IsTypeOf(AzToolsFramework::Components::EditorComponentBase::RTTI_Type())) + { + component = aznew ComponentType(AZStd::forward(args)...); + } + else + { + AZ::Component* gameComponent = aznew ComponentType(AZStd::forward(args)...); + component = aznew AzToolsFramework::Components::GenericComponentWrapper(gameComponent); + } + AZ_Assert(component, "Failed to create component: %s", AZ::AzTypeInfo::Name()); + + if (component) + { + if (!entity.IsComponentReadyToAdd(component) || !entity.AddComponent(component)) + { + delete component; + component = nullptr; + } + } + return component; + } + } // namespace Utils + } // namespace ROS2SensorHooks +} // namespace ROS2::SDFormat diff --git a/Gems/ROS2/Code/Tests/SdfParserTest.cpp b/Gems/ROS2/Code/Tests/SdfParserTest.cpp index 42451ebcf..c90e2b516 100644 --- a/Gems/ROS2/Code/Tests/SdfParserTest.cpp +++ b/Gems/ROS2/Code/Tests/SdfParserTest.cpp @@ -88,56 +88,165 @@ namespace UnitTest )"; } + + std::string GetSdfWithImuSensor() + { + return R"( + + + + + true + 200 + + + + + 0.0 + 2e-4 + + + + + 0.0 + 3e-4 + + + + + 0.0 + 4e-4 + + + + + + + 0.0 + 1.7e-2 + + + + + 0.0 + 1.8e-2 + + + + + 0.0 + 1.9e-2 + + + + + + + ~/out:=imu + + + + + + )"; + } }; TEST_F(SdfParserTest, CheckModelCorrectness) { - const auto xmlStr = GetSdfWithTwoSensors(); - const auto sdfRoot = Parse(xmlStr); - ASSERT_TRUE(sdfRoot); - const auto* sdfModel = sdfRoot->Model(); - ASSERT_TRUE(sdfModel); - - EXPECT_EQ(sdfModel->Name(), "test_two_sensors"); - EXPECT_EQ(sdfModel->LinkCount(), 2U); - - const auto* link1 = sdfModel->LinkByName("link1"); - ASSERT_TRUE(link1); - EXPECT_EQ(link1->SensorCount(), 1U); - const auto* sensor1 = link1->SensorByIndex(0U); - ASSERT_TRUE(sensor1); - EXPECT_EQ(sensor1->Type(), sdf::SensorType::CAMERA); - EXPECT_EQ(sensor1->UpdateRate(), 10); - auto* cameraSensor = sensor1->CameraSensor(); - ASSERT_TRUE(cameraSensor); - EXPECT_EQ(cameraSensor->ImageWidth(), 640); - EXPECT_EQ(cameraSensor->ImageHeight(), 480); - EXPECT_NEAR(cameraSensor->HorizontalFov().Radian(), 2.0, 1e-5); - EXPECT_NEAR(cameraSensor->NearClip(), 0.01, 1e-5); - EXPECT_NEAR(cameraSensor->FarClip(), 1000, 1e-5); - EXPECT_EQ(sensor1->Plugins().size(), 1U); - EXPECT_EQ(sensor1->Plugins().at(0).Name(), "camera_plug"); - EXPECT_EQ(sensor1->Plugins().at(0).Filename(), "libgazebo_ros_camera.so"); - - const auto* link2 = sdfModel->LinkByName("link2"); - ASSERT_TRUE(link2); - EXPECT_EQ(link2->SensorCount(), 1U); - const auto* sensor2 = link2->SensorByIndex(0U); - ASSERT_TRUE(sensor2); - EXPECT_EQ(sensor2->Type(), sdf::SensorType::LIDAR); - EXPECT_EQ(sensor2->UpdateRate(), 20); - auto* lidarSensor = sensor2->LidarSensor(); - ASSERT_TRUE(lidarSensor); - EXPECT_EQ(lidarSensor->HorizontalScanSamples(), 640); - EXPECT_NEAR(lidarSensor->HorizontalScanResolution(), 1.0, 1e-5); - EXPECT_NEAR(lidarSensor->HorizontalScanMinAngle().Radian(), -2.0, 1e-5); - EXPECT_NEAR(lidarSensor->HorizontalScanMaxAngle().Radian(), 2.5, 1e-5); - EXPECT_NEAR(lidarSensor->RangeResolution(), 0.01, 1e-5); - EXPECT_NEAR(lidarSensor->RangeMin(), 0.02, 1e-5); - EXPECT_NEAR(lidarSensor->RangeMax(), 10.0, 1e-5); - EXPECT_EQ(sensor2->Plugins().size(), 1U); - EXPECT_EQ(sensor2->Plugins().at(0).Name(), "laser_plug"); - EXPECT_EQ(sensor2->Plugins().at(0).Filename(), "librayplugin.so"); + { + const auto xmlStr = GetSdfWithTwoSensors(); + const auto sdfRoot = Parse(xmlStr); + ASSERT_TRUE(sdfRoot); + const auto* sdfModel = sdfRoot->Model(); + ASSERT_TRUE(sdfModel); + + EXPECT_EQ(sdfModel->Name(), "test_two_sensors"); + EXPECT_EQ(sdfModel->LinkCount(), 2U); + + const auto* link1 = sdfModel->LinkByName("link1"); + ASSERT_TRUE(link1); + EXPECT_EQ(link1->SensorCount(), 1U); + const auto* sensor1 = link1->SensorByIndex(0U); + ASSERT_TRUE(sensor1); + EXPECT_EQ(sensor1->Type(), sdf::SensorType::CAMERA); + EXPECT_EQ(sensor1->UpdateRate(), 10); + auto* cameraSensor = sensor1->CameraSensor(); + ASSERT_TRUE(cameraSensor); + EXPECT_EQ(cameraSensor->ImageWidth(), 640); + EXPECT_EQ(cameraSensor->ImageHeight(), 480); + EXPECT_NEAR(cameraSensor->HorizontalFov().Radian(), 2.0, 1e-5); + EXPECT_NEAR(cameraSensor->NearClip(), 0.01, 1e-5); + EXPECT_NEAR(cameraSensor->FarClip(), 1000, 1e-5); + EXPECT_EQ(sensor1->Plugins().size(), 1U); + EXPECT_EQ(sensor1->Plugins().at(0).Name(), "camera_plug"); + EXPECT_EQ(sensor1->Plugins().at(0).Filename(), "libgazebo_ros_camera.so"); + + const auto* link2 = sdfModel->LinkByName("link2"); + ASSERT_TRUE(link2); + EXPECT_EQ(link2->SensorCount(), 1U); + const auto* sensor2 = link2->SensorByIndex(0U); + ASSERT_TRUE(sensor2); + EXPECT_EQ(sensor2->Type(), sdf::SensorType::LIDAR); + EXPECT_EQ(sensor2->UpdateRate(), 20); + auto* lidarSensor = sensor2->LidarSensor(); + ASSERT_TRUE(lidarSensor); + EXPECT_EQ(lidarSensor->HorizontalScanSamples(), 640); + EXPECT_NEAR(lidarSensor->HorizontalScanResolution(), 1.0, 1e-5); + EXPECT_NEAR(lidarSensor->HorizontalScanMinAngle().Radian(), -2.0, 1e-5); + EXPECT_NEAR(lidarSensor->HorizontalScanMaxAngle().Radian(), 2.5, 1e-5); + EXPECT_NEAR(lidarSensor->RangeResolution(), 0.01, 1e-5); + EXPECT_NEAR(lidarSensor->RangeMin(), 0.02, 1e-5); + EXPECT_NEAR(lidarSensor->RangeMax(), 10.0, 1e-5); + EXPECT_EQ(sensor2->Plugins().size(), 1U); + EXPECT_EQ(sensor2->Plugins().at(0).Name(), "laser_plug"); + EXPECT_EQ(sensor2->Plugins().at(0).Filename(), "librayplugin.so"); + } + + { + const auto xmlStr = GetSdfWithImuSensor(); + const auto sdfRoot = Parse(xmlStr); + ASSERT_TRUE(sdfRoot); + const auto* sdfModel = sdfRoot->Model(); + ASSERT_TRUE(sdfModel); + + EXPECT_EQ(sdfModel->Name(), "test_imu_sensor"); + EXPECT_EQ(sdfModel->LinkCount(), 1U); + + const auto* link1 = sdfModel->LinkByName("link1"); + ASSERT_TRUE(link1); + EXPECT_EQ(link1->SensorCount(), 1U); + const auto* sensor1 = link1->SensorByIndex(0U); + ASSERT_TRUE(sensor1); + EXPECT_EQ(sensor1->Type(), sdf::SensorType::IMU); + EXPECT_EQ(sensor1->Name(), "link1_imu"); + EXPECT_EQ(sensor1->UpdateRate(), 200); + + auto* imuSensor = sensor1->ImuSensor(); + ASSERT_TRUE(imuSensor); + EXPECT_EQ(imuSensor->AngularVelocityXNoise().Type(), sdf::NoiseType::GAUSSIAN); + EXPECT_EQ(imuSensor->AngularVelocityYNoise().Type(), sdf::NoiseType::GAUSSIAN); + EXPECT_EQ(imuSensor->AngularVelocityZNoise().Type(), sdf::NoiseType::GAUSSIAN); + EXPECT_EQ(imuSensor->AngularVelocityXNoise().Mean(), 0.0); + EXPECT_EQ(imuSensor->AngularVelocityYNoise().Mean(), 0.0); + EXPECT_EQ(imuSensor->AngularVelocityZNoise().Mean(), 0.0); + EXPECT_NEAR(imuSensor->AngularVelocityXNoise().StdDev(), 2e-4, 1e-5); + EXPECT_NEAR(imuSensor->AngularVelocityYNoise().StdDev(), 3e-4, 1e-5); + EXPECT_NEAR(imuSensor->AngularVelocityZNoise().StdDev(), 4e-4, 1e-5); + EXPECT_EQ(imuSensor->LinearAccelerationXNoise().Type(), sdf::NoiseType::GAUSSIAN); + EXPECT_EQ(imuSensor->LinearAccelerationYNoise().Type(), sdf::NoiseType::GAUSSIAN); + EXPECT_EQ(imuSensor->LinearAccelerationZNoise().Type(), sdf::NoiseType::GAUSSIAN); + EXPECT_EQ(imuSensor->LinearAccelerationXNoise().Mean(), 0.0); + EXPECT_EQ(imuSensor->LinearAccelerationYNoise().Mean(), 0.0); + EXPECT_EQ(imuSensor->LinearAccelerationZNoise().Mean(), 0.0); + EXPECT_NEAR(imuSensor->LinearAccelerationXNoise().StdDev(), 1.7e-2, 1e-5); + EXPECT_NEAR(imuSensor->LinearAccelerationYNoise().StdDev(), 1.8e-2, 1e-5); + EXPECT_NEAR(imuSensor->LinearAccelerationZNoise().StdDev(), 1.9e-2, 1e-5); + + EXPECT_EQ(sensor1->Plugins().size(), 1U); + EXPECT_EQ(sensor1->Plugins().at(0).Name(), "imu_plugin"); + EXPECT_EQ(sensor1->Plugins().at(0).Filename(), "libgazebo_ros_imu_sensor.so"); + } } TEST_F(SdfParserTest, RobotImporterUtils) @@ -212,30 +321,55 @@ namespace UnitTest TEST_F(SdfParserTest, SensorPluginImporterHookCheck) { - const auto xmlStr = GetSdfWithTwoSensors(); - const auto sdfRoot = Parse(xmlStr); - const auto* sdfModel = sdfRoot->Model(); - const sdf::ElementPtr cameraElement = sdfModel->LinkByName("link1")->SensorByIndex(0U)->Element(); - const auto& importerHook = ROS2::SDFormat::ROS2SensorHooks::ROS2CameraSensor(); + { + const auto xmlStr = GetSdfWithTwoSensors(); + const auto sdfRoot = Parse(xmlStr); + const auto* sdfModel = sdfRoot->Model(); + const sdf::ElementPtr cameraElement = sdfModel->LinkByName("link1")->SensorByIndex(0U)->Element(); + const auto& cameraImporterHook = ROS2::SDFormat::ROS2SensorHooks::ROS2CameraSensor(); - const auto& unsupportedCameraParams = - ROS2::Utils::SDFormat::GetUnsupportedParams(cameraElement, importerHook.m_supportedSensorParams); - EXPECT_EQ(unsupportedCameraParams.size(), 1U); - EXPECT_EQ(unsupportedCameraParams[0U], ">pose"); + const auto& unsupportedCameraParams = + ROS2::Utils::SDFormat::GetUnsupportedParams(cameraElement, cameraImporterHook.m_supportedSensorParams); + EXPECT_EQ(unsupportedCameraParams.size(), 1U); + EXPECT_EQ(unsupportedCameraParams[0U], ">pose"); - sdf::Plugin plug; - plug.SetName("test_camera"); - plug.SetFilename("libgazebo_ros_camera.so"); - EXPECT_TRUE(ROS2::Utils::SDFormat::IsPluginSupported(plug, importerHook.m_pluginNames)); - plug.SetFilename("/usr/lib/libgazebo_ros_openni_kinect.so"); - EXPECT_TRUE(ROS2::Utils::SDFormat::IsPluginSupported(plug, importerHook.m_pluginNames)); - plug.SetFilename("~/dev/libgazebo_ros_imu.so"); - EXPECT_FALSE(ROS2::Utils::SDFormat::IsPluginSupported(plug, importerHook.m_pluginNames)); - plug.SetFilename("libgazebo_ros_camera"); - EXPECT_FALSE(ROS2::Utils::SDFormat::IsPluginSupported(plug, importerHook.m_pluginNames)); - - EXPECT_TRUE(importerHook.m_sensorTypes.contains(sdf::SensorType::CAMERA)); - EXPECT_TRUE(importerHook.m_sensorTypes.contains(sdf::SensorType::DEPTH_CAMERA)); - EXPECT_FALSE(importerHook.m_sensorTypes.contains(sdf::SensorType::GPS)); + sdf::Plugin plug; + plug.SetName("test_camera"); + plug.SetFilename("libgazebo_ros_camera.so"); + EXPECT_TRUE(ROS2::Utils::SDFormat::IsPluginSupported(plug, cameraImporterHook.m_pluginNames)); + plug.SetFilename("/usr/lib/libgazebo_ros_openni_kinect.so"); + EXPECT_TRUE(ROS2::Utils::SDFormat::IsPluginSupported(plug, cameraImporterHook.m_pluginNames)); + plug.SetFilename("~/dev/libgazebo_ros_imu.so"); + EXPECT_FALSE(ROS2::Utils::SDFormat::IsPluginSupported(plug, cameraImporterHook.m_pluginNames)); + plug.SetFilename("libgazebo_ros_camera"); + EXPECT_FALSE(ROS2::Utils::SDFormat::IsPluginSupported(plug, cameraImporterHook.m_pluginNames)); + + EXPECT_TRUE(cameraImporterHook.m_sensorTypes.contains(sdf::SensorType::CAMERA)); + EXPECT_TRUE(cameraImporterHook.m_sensorTypes.contains(sdf::SensorType::DEPTH_CAMERA)); + EXPECT_FALSE(cameraImporterHook.m_sensorTypes.contains(sdf::SensorType::GPS)); + + const sdf::ElementPtr lidarElement = sdfModel->LinkByName("link2")->SensorByIndex(0U)->Element(); + const auto& lidarImporterHook = ROS2::SDFormat::ROS2SensorHooks::ROS2LidarSensor(); + const auto& unsupportedLidarParams = + ROS2::Utils::SDFormat::GetUnsupportedParams(lidarElement, lidarImporterHook.m_supportedSensorParams); + EXPECT_EQ(unsupportedLidarParams.size(), 5U); + EXPECT_EQ(unsupportedLidarParams[0U], ">always_on"); + EXPECT_EQ(unsupportedLidarParams[1U], ">visualize"); + EXPECT_EQ(unsupportedLidarParams[2U], ">pose"); + EXPECT_EQ(unsupportedLidarParams[3U], ">ray>scan>horizontal>resolution"); + EXPECT_EQ(unsupportedLidarParams[4U], ">ray>range>resolution"); + } + { + const auto xmlStr = GetSdfWithImuSensor(); + const auto sdfRoot = Parse(xmlStr); + const auto* sdfModel = sdfRoot->Model(); + const sdf::ElementPtr imuElement = sdfModel->LinkByName("link1")->SensorByIndex(0U)->Element(); + const auto& importerHook = ROS2::SDFormat::ROS2SensorHooks::ROS2ImuSensor(); + + const auto& unsupportedImuParams = + ROS2::Utils::SDFormat::GetUnsupportedParams(imuElement, importerHook.m_supportedSensorParams); + EXPECT_EQ(unsupportedImuParams.size(), 1U); + EXPECT_EQ(unsupportedImuParams[0U], ">always_on"); + } } } // namespace UnitTest diff --git a/Gems/ROS2/Code/ros2_editor_files.cmake b/Gems/ROS2/Code/ros2_editor_files.cmake index dcdfde185..2314f02fb 100644 --- a/Gems/ROS2/Code/ros2_editor_files.cmake +++ b/Gems/ROS2/Code/ros2_editor_files.cmake @@ -28,7 +28,12 @@ set(FILES Source/RobotImporter/RobotImporterWidget.h Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.cpp Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.h - Source/RobotImporter/SDFormat/ROS2SensorHooks.cpp + Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp + Source/RobotImporter/SDFormat/Hooks/ROS2GNSSSensorHook.cpp + Source/RobotImporter/SDFormat/Hooks/ROS2ImuSensorHook.cpp + Source/RobotImporter/SDFormat/Hooks/ROS2LidarSensorHook.cpp + Source/RobotImporter/SDFormat/ROS2SensorHooksUtils.cpp + Source/RobotImporter/SDFormat/ROS2SensorHooksUtils.h Source/RobotImporter/SDFormat/ROS2SensorHooks.h Source/RobotImporter/URDF/ArticulationsMaker.cpp Source/RobotImporter/URDF/ArticulationsMaker.h